diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..3ca657d --- /dev/null +++ b/.clang-format @@ -0,0 +1,227 @@ +BasedOnStyle: Google + +Language: Java +AccessModifierOffset: -1 +AlignAfterOpenBracket: BlockIndent +AlignArrayOfStructures: None +AlignConsecutiveAssignments: false +AlignConsecutiveBitFields: false +AlignConsecutiveDeclarations: false +AlignConsecutiveMacros: false +AlignEscapedNewlines: Left +AlignOperands: DontAlign +AlignTrailingComments: false +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortEnumsOnASingleLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Empty +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: Yes +AttributeMacros: + - __capability +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: NonAssignment +BreakBeforeConceptDeclarations: true +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 100 +CommentPragmas: '^ IWYU pragma:' +QualifierAlignment: Leave +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: true +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +PackConstructorInitializers: NextLine +ConstructorInitializerAllOnOneLineOrOnePerLine: false +AllowAllConstructorInitializersOnNextLine: true +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseLabels: true +IndentCaseBlocks: false +IndentGotoLabels: true +IndentPPDirectives: None +IndentExternBlock: AfterExternBlock +IndentRequires: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +# InsertBraces: false +InsertTrailingCommas: None +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +LambdaBodyIndentation: Signature +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PenaltyIndentedWhitespace: 0 +PointerAlignment: Left +PPIndentWidth: -1 +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + - ParseTestProto + - ParsePartialTestProto + CanonicalDelimiter: pb + BasedOnStyle: google +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +# RequiresClausePosition: OwnLine +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: true +SpaceAfterCStyleCast: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + # AfterRequiresInClause: false + # AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false +SpaceAroundPointerQualifiers: Default +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: Never +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +BitFieldColonSpacing: Both +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 4 +UseCRLF: false +UseTab: Never +WhitespaceSensitiveMacros: + - STRINGIZE + - PP_STRINGIZE + - BOOST_PP_STRINGIZE + - NS_SWIFT_NAME + - CF_SWIFT_NAME \ No newline at end of file diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS new file mode 100644 index 0000000..e69de29 diff --git a/.github/ISSUE_TEMPLATE/command-group.md b/.github/ISSUE_TEMPLATE/command-group.md new file mode 100644 index 0000000..3dabba0 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/command-group.md @@ -0,0 +1,10 @@ +--- +name: Command group +about: Add a grouping of commands for complex functionality +title: Add for +labels: '' +assignees: '' + +--- + + diff --git a/.github/ISSUE_TEMPLATE/command.md b/.github/ISSUE_TEMPLATE/command.md new file mode 100644 index 0000000..5d39f54 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/command.md @@ -0,0 +1,10 @@ +--- +name: Command +about: Add a base command to the robot +title: Add for +labels: '' +assignees: '' + +--- + + diff --git a/.github/ISSUE_TEMPLATE/configuration.md b/.github/ISSUE_TEMPLATE/configuration.md new file mode 100644 index 0000000..38c9baa --- /dev/null +++ b/.github/ISSUE_TEMPLATE/configuration.md @@ -0,0 +1,10 @@ +--- +name: Configuration +about: Add or tune a configuration +title: " " +labels: '' +assignees: '' + +--- + + diff --git a/.github/ISSUE_TEMPLATE/subsystem-functionality.md b/.github/ISSUE_TEMPLATE/subsystem-functionality.md new file mode 100644 index 0000000..b680afe --- /dev/null +++ b/.github/ISSUE_TEMPLATE/subsystem-functionality.md @@ -0,0 +1,10 @@ +--- +name: Subsystem functionality +about: Add a functionality to a subsystem +title: Add to +labels: '' +assignees: '' + +--- + + diff --git a/.github/templates/README.md b/.github/templates/README.md new file mode 100644 index 0000000..2358517 --- /dev/null +++ b/.github/templates/README.md @@ -0,0 +1,10 @@ +## Label template directory +This directory contains templates used in the `add-template-to-issue.yml` workflow + +Each template must have a filename of `label-.md` to automatically be applied to a label of that name + +For example, if you would like to add a template to an issue when the `badbug` label is applied, add a file in this directory called + +`label-badbug.md` + +The template will be added as a new comment as to avoid muddling existing comments diff --git a/.github/templates/label-base_command.md b/.github/templates/label-base_command.md new file mode 100644 index 0000000..2f241c5 --- /dev/null +++ b/.github/templates/label-base_command.md @@ -0,0 +1,12 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What subsystem does this command use? +- Is the subsystem a subsystem requirement for the command? +- Where/when will this command be used? +- What does this command do generally? +- What does this command logically do on initialization? +- What does this command logically do during each execution loop? +- When (if ever) is this command done? +- What does this command do when it is done? diff --git a/.github/templates/label-complex_command.md b/.github/templates/label-complex_command.md new file mode 100644 index 0000000..1128fd5 --- /dev/null +++ b/.github/templates/label-complex_command.md @@ -0,0 +1,12 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What subsystems does this command use? +- Which of the subsystems is a requirement for the command? +- Where/when will this command be used? +- What does this command do generally? +- What does this command logically do on initialization? +- What does this command logically do during each execution loop? +- When (if ever) is this command done? +- What does this command do when it is done? diff --git a/.github/templates/label-compound_command.md b/.github/templates/label-compound_command.md new file mode 100644 index 0000000..ecb4ff6 --- /dev/null +++ b/.github/templates/label-compound_command.md @@ -0,0 +1,10 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What subsystems does this command use? +- Which of the subsystems is a requirement for the command? +- What does this command do generally? +- Where/when will this command be used? +- What commands does this command use? +- Give a brief explanation of the progression of commands diff --git a/.github/templates/label-path_command.md b/.github/templates/label-path_command.md new file mode 100644 index 0000000..e1682f4 --- /dev/null +++ b/.github/templates/label-path_command.md @@ -0,0 +1,12 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What subsystems does this command use? +- Which of the subsystems is a requirement for the command? +- What does this command do generally? +- Where/when will this command be used? +- What commands does this command use? +- Give a brief explanation of the progression of commands +- Is this an alliance specific command? Which alliance? +- List and label the primary waypoints used in this command. diff --git a/.github/templates/label-subsystem_functionality.md b/.github/templates/label-subsystem_functionality.md new file mode 100644 index 0000000..ac4924a --- /dev/null +++ b/.github/templates/label-subsystem_functionality.md @@ -0,0 +1,8 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What subsystem does is this functionality for? +- Where will this functionality be used? +- Does this function take any sensor values as arguments? +- Does this function take any other parameters as arguments? diff --git a/.github/templates/label-testing.md b/.github/templates/label-testing.md new file mode 100644 index 0000000..6ec43b9 --- /dev/null +++ b/.github/templates/label-testing.md @@ -0,0 +1,7 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What values or commands are being tested? +- What are they being tested for? +- What is the definition of done for this task? diff --git a/.github/templates/label-testonly.md b/.github/templates/label-testonly.md new file mode 100644 index 0000000..a3d031c --- /dev/null +++ b/.github/templates/label-testonly.md @@ -0,0 +1,7 @@ +## This is a test template + +- Example scala code: + +```scala +val someVal = 10 +``` diff --git a/.github/templates/label-tuning.md b/.github/templates/label-tuning.md new file mode 100644 index 0000000..7bb2921 --- /dev/null +++ b/.github/templates/label-tuning.md @@ -0,0 +1,8 @@ +Level 3 time estimate: ## minutes + +## Please answer these questions before starting to code + +- What values or commands are being tuned? +- What are they being tuned for? +- What behavior would we see from optimally tuned values? +- What is the definition of done for this task? diff --git a/.github/workflows/add-template-to-issue.yml b/.github/workflows/add-template-to-issue.yml new file mode 100644 index 0000000..67cc7e3 --- /dev/null +++ b/.github/workflows/add-template-to-issue.yml @@ -0,0 +1,38 @@ +name: Add Template to Issue +on: + issues: + types: [labeled] +jobs: + add-template: + runs-on: ubuntu-latest + permissions: + issues: write + steps: + - shell: bash + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + OWNER: ${{ github.repository_owner }} + REPO: ${{ github.event.repository.name }} + GH_KEY: ${{ secrets.STEVE_KEY }} + TEMPLATE_PATH: .github/templates + run: | + SSH_PATH=~/.ssh/id_rsa + label="${{ github.event.label.name }}" + filename="$TEMPLATE_PATH/label-$label.md" + repo="git@github.com:$OWNER/$REPO" + mkdir ${SSH_PATH%/*} + echo "$GH_KEY" > $SSH_PATH + chmod 600 $SSH_PATH + git clone $repo --sparse --depth 1 --no-checkout + cd $REPO + git sparse-checkout set $TEMPLATE_PATH + git checkout HEAD + if [[ -f "$filename" ]] ; then + template=$(cat $filename) + issue=${{ github.event.issue.number }} + gh issue comment $issue -F $filename + echo "added template $filename to issue $issue" + else + echo "no template exists for label $label." + fi + rm $SSH_PATH diff --git a/.github/workflows/add-to-project.yml b/.github/workflows/add-to-project.yml new file mode 100644 index 0000000..788d764 --- /dev/null +++ b/.github/workflows/add-to-project.yml @@ -0,0 +1,16 @@ +name: Auto Assign to Project( + +on: + pull_request_target: + types: [opened] + +jobs: + add-to-project: + if: github.repository == 'team1868/offseason2023' + runs-on: ubuntu-latest + steps: + - uses: actions/add-to-project@v0.3.0 + with: + project-url: https://github.com/orgs/team1868/projects/5 + github-token: ${{ secrets.PROJECT_TOKEN }} + diff --git a/.github/workflows/auto-tag-and-add-to-project.yml b/.github/workflows/auto-tag-and-add-to-project.yml new file mode 100644 index 0000000..28fefdb --- /dev/null +++ b/.github/workflows/auto-tag-and-add-to-project.yml @@ -0,0 +1,30 @@ +name: Label issues +on: + issues: + types: [opened] +jobs: + label_issues: + runs-on: ubuntu-latest + permissions: + issues: write + steps: + - uses: actions/github-script@v6 + with: + script: | + github.rest.issues.addLabels({ + issue_number: context.issue.number, + owner: context.repo.owner, + repo: context.repo.repo, + labels: ["Needs Triage"] + }) + + add-to-project: + if: github.repository == 'team1868/offseason2023' + runs-on: ubuntu-latest + steps: + - uses: actions/add-to-project@v0.3.0 + with: + project-url: https://github.com/orgs/team1868/projects/5 + github-token: ${{ secrets.PROJECT_TOKEN }} + + diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000..e1d728b --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,38 @@ +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: + workflow_dispatch: + + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + build: + name: "build and format" + runs-on: ubuntu-latest + container: wpilib/roborio-cross-ubuntu:2023-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + - uses: actions/checkout@v3 + - uses: DoozyX/clang-format-lint-action@v0.14 + with: + source: '.' + exclude: './lib' + extensions: 'java,h,cpp,c' + clangFormatVersion: 14 + # - uses: EndBug/add-and-commit@v4 + # with: + # author_name: Clang Format Robot + # author_email: spacecookies.data@gmail.com + # message: 'Committing clang-format changes' + # env: + # GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + # - name: Grant execute permission for gradlew + # run: chmod +x gradlew + - name: Compile and run tests on robot code + run: ./gradlew build diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a8d1911 --- /dev/null +++ b/.gitignore @@ -0,0 +1,172 @@ +# 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 + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_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 + +### 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 +*-window.json diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..1dc3700 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,17 @@ +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v2.3.0 + hooks: + - id: end-of-file-fixer + files: \.(java|c|h|hpp|cpp)$ + - id: trailing-whitespace + files: \.(java|c|h|hpp|cpp)$ + - id: no-commit-to-branch + args: [--branch, main] +- repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 + hooks: + - id: clang-format + files: \.(java|c|h|hpp|cpp)$ + types_or: [file] + args: ['-fallback-style=none', '-style=file'] diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "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 0000000..6b40836 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,34 @@ +{ + "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" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.format.settings.url": "eclipse-formatter.xml", + "editor.formatOnSave": true, + "editor.defaultFormatter": "xaver.clang-format" +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..c237d2c --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2023", + "teamNumber": 1868 +} \ No newline at end of file diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md new file mode 100644 index 0000000..1f0de1a --- /dev/null +++ b/CONTRIBUTORS.md @@ -0,0 +1,4 @@ +- Sophie (sop26) +- Anh (maroonApricot) +- Anika (anika-kum) +- Diya (Diyatb) diff --git a/WPILib-License.md b/WPILib-License.md new file mode 100644 index 0000000..3d5a824 --- /dev/null +++ b/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 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. diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..08dd71a --- /dev/null +++ b/build.gradle @@ -0,0 +1,103 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2023.4.3" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +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 = false + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +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() + + implementation 'gov.nist.math:jama:1.0.3' + + 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-api:5.8.2' + testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + + testImplementation 'junit:junit:4.12' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +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) } } + 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' +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..249e583 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..c23a1b3 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100755 index 0000000..a69d9cb --- /dev/null +++ b/gradlew @@ -0,0 +1,240 @@ +#!/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/master/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 + +APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&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 + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + 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 + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +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 0000000..f127cfd --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,91 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem 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 0000000..48c039e --- /dev/null +++ b/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2023' + 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 + } + } +} diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_1_TO_CHARGE_2.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_1_TO_CHARGE_2.path new file mode 100644 index 0000000..185d93f --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_1_TO_CHARGE_2.path @@ -0,0 +1,59 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.95, + "y": 1.12 + }, + "prevControl": null, + "nextControl": { + "x": 5.950000000000002, + "y": 1.12 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.5, + "y": 2.45 + }, + "prevControl": { + "x": 6.5, + "y": 2.45 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_1_TO_SCORE_2.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_1_TO_SCORE_2.path new file mode 100644 index 0000000..d4e0cf3 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_1_TO_SCORE_2.path @@ -0,0 +1,143 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.66, + "y": 1.12 + }, + "prevControl": null, + "nextControl": { + "x": 5.785646281362008, + "y": 1.0577584366974282 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCube" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.682543682795699, + "y": 1.0503359774080658 + }, + "prevControl": { + "x": 6.039401608317107, + "y": 1.0756767173256563 + }, + "nextControl": { + "x": 5.572608542554494, + "y": 1.042529403426037 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.7, + "y": 1.0 + }, + "prevControl": { + "x": 5.407633301910041, + "y": 1.0 + }, + "nextControl": { + "x": 4.03026365844964, + "y": 0.9999999999999999 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.3, + "y": 1.0 + }, + "prevControl": { + "x": 3.7597729588814044, + "y": 1.0 + }, + "nextControl": { + "x": 2.424375884282757, + "y": 0.9999999999999999 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.0, + "y": 1.3 + }, + "prevControl": { + "x": 2.367257860090388, + "y": 1.3 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCube2", + "scoreCube2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 2.47869318181818, + "names": [ + "prepScoreCube2" + ] + }, + { + "position": 0.0, + "names": [ + "postGroundIntake" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_2_TO_SCORE_3.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_2_TO_SCORE_3.path new file mode 100644 index 0000000..126c7fa --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_2_TO_SCORE_3.path @@ -0,0 +1,84 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.861556, + "y": 1.775474753 + }, + "prevControl": null, + "nextControl": { + "x": 6.470290878981326, + "y": 1.097783684166071 + }, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.848897594279563, + "y": 1.0 + }, + "prevControl": { + "x": 5.950754088242831, + "y": 1.0 + }, + "nextControl": { + "x": 4.463895975525589, + "y": 1.0 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.25724172815867, + "y": 1.0 + }, + "prevControl": { + "x": 3.6367338137796206, + "y": 1.0 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_3_TO_CHARGE_2.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_3_TO_CHARGE_2.path new file mode 100644 index 0000000..7c69969 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_3_TO_CHARGE_2.path @@ -0,0 +1,84 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.92, + "y": 3.63 + }, + "prevControl": null, + "nextControl": { + "x": 6.572137026218497, + "y": 3.310590806945503 + }, + "holonomicAngle": 300.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntake" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.57, + "y": 3.2299295501368777 + }, + "prevControl": { + "x": 6.166405657904978, + "y": 3.2299295501368777 + }, + "nextControl": { + "x": 4.973594342095023, + "y": 3.2299295501368777 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8, + "y": 3.23 + }, + "prevControl": { + "x": 4.1014018979988425, + "y": 3.23 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntake" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_3_TO_SCORE_7.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_3_TO_SCORE_7.path new file mode 100644 index 0000000..61e8585 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_3_TO_SCORE_7.path @@ -0,0 +1,111 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.86, + "y": 3.723 + }, + "prevControl": null, + "nextControl": { + "x": 6.368889215208987, + "y": 4.576410055597889 + }, + "holonomicAngle": -60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntake" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.145050826559986, + "y": 4.46362346717749 + }, + "prevControl": { + "x": 4.982022652968862, + "y": 4.486384134891946 + }, + "nextControl": { + "x": 2.409297124149804, + "y": 4.416421265134197 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.2, + "y": 3.86 + }, + "prevControl": { + "x": 2.4177053788135354, + "y": 4.45451935695538 + }, + "nextControl": { + "x": 2.1943954395315477, + "y": 3.844694821488192 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8, + "y": 3.86 + }, + "prevControl": { + "x": 1.6494859813402605, + "y": 3.8733528463931926 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntake" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_BOTTOM_TO_CHARGE.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_BOTTOM_TO_CHARGE.path new file mode 100644 index 0000000..85ecf9d --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_BOTTOM_TO_CHARGE.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 5.57, + "y": 2.2 + }, + "prevControl": null, + "nextControl": { + "x": 4.57, + "y": 2.2 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 2.2 + }, + "prevControl": { + "x": 4.0, + "y": 2.2 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_TOP_TO_CHARGE.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_TOP_TO_CHARGE.path new file mode 100644 index 0000000..e36a716 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_TOP_TO_CHARGE.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 5.57, + "y": 3.23 + }, + "prevControl": null, + "nextControl": { + "x": 4.57, + "y": 3.23 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 3.23 + }, + "prevControl": { + "x": 4.0, + "y": 3.23 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_TO_CHARGE.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_TO_CHARGE.path new file mode 100644 index 0000000..7013e64 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/INTAKE_TO_CHARGE.path @@ -0,0 +1,52 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 5.35, + "y": 2.45 + }, + "prevControl": null, + "nextControl": { + "x": 4.35, + "y": 2.45 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.39, + "y": 2.45 + }, + "prevControl": { + "x": 4.390000000000001, + "y": 2.45 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "isReversed": null, + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_1_TO_INTAKE_1.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_1_TO_INTAKE_1.path new file mode 100644 index 0000000..4e3cb9b --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_1_TO_INTAKE_1.path @@ -0,0 +1,115 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8, + "y": 0.51 + }, + "prevControl": null, + "nextControl": { + "x": 1.8015173088070988, + "y": 0.5105522552419739 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": true, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.45567, + "y": 0.9 + }, + "prevControl": { + "x": 2.590654221681564, + "y": 0.9 + }, + "nextControl": { + "x": 4.251488685443741, + "y": 0.9 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.30657, + "y": 0.8984833867918937 + }, + "prevControl": { + "x": 4.250309846500218, + "y": 0.8984833867918937 + }, + "nextControl": { + "x": 5.235044183349022, + "y": 0.8984833867918937 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.75, + "y": 1.22 + }, + "prevControl": { + "x": 5.9554293332665145, + "y": 1.22 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + }, + { + "position": 2.5097301136363637, + "names": [ + "prepGroundIntakeCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_2_TO_CHARGE_2.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_2_TO_CHARGE_2.path new file mode 100644 index 0000000..3fa920a --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_2_TO_CHARGE_2.path @@ -0,0 +1,81 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.79705, + "y": 1.0668 + }, + "prevControl": null, + "nextControl": { + "x": 1.8340565292241697, + "y": 1.552163810924844 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.3251565895492905, + "y": 2.141727432760421 + }, + "prevControl": { + "x": 1.7885939232297847, + "y": 2.141727432760421 + }, + "nextControl": { + "x": 2.8617192558687963, + "y": 2.141727432760421 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 2.14 + }, + "prevControl": { + "x": 4.731718666840247, + "y": 2.14 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_2_TO_INTAKE_2.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_2_TO_INTAKE_2.path new file mode 100644 index 0000000..1932530 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_2_TO_INTAKE_2.path @@ -0,0 +1,115 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.0, + "y": 1.3 + }, + "prevControl": null, + "nextControl": { + "x": 2.6184597074055262, + "y": 1.2392895129540253 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.36, + "y": 0.7953049152626078 + }, + "prevControl": { + "x": 2.344745039974985, + "y": 0.7953049152626078 + }, + "nextControl": { + "x": 3.751580302470226, + "y": 0.7953049152626078 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.463752672297011, + "y": 0.789100203200406 + }, + "prevControl": { + "x": 4.015593762999934, + "y": 0.789100203200406 + }, + "nextControl": { + "x": 5.044914662541221, + "y": 0.789100203200406 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.827400040216747, + "y": 1.7477572107921988 + }, + "prevControl": { + "x": 6.214794072748084, + "y": 0.6866925501165881 + }, + "nextControl": null, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.0, + "maxAcceleration": null, + "isReversed": null, + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + }, + { + "position": 2.6, + "names": [ + "prepGroundIntakeCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_6_MOBILITY_TO_CHARGE.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_6_MOBILITY_TO_CHARGE.path new file mode 100644 index 0000000..0d74c66 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_6_MOBILITY_TO_CHARGE.path @@ -0,0 +1,111 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.79705, + "y": 3.302 + }, + "prevControl": null, + "nextControl": { + "x": 2.361979235042634, + "y": 3.302 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.498576, + "y": 3.3 + }, + "prevControl": { + "x": 2.16178721584989, + "y": 3.3 + }, + "nextControl": { + "x": 2.83536478415011, + "y": 3.3 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.1, + "y": 3.3 + }, + "prevControl": { + "x": 6.042951592540702, + "y": 3.3 + }, + "nextControl": { + "x": 6.157048407459297, + "y": 3.3 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.77, + "y": 3.3 + }, + "prevControl": { + "x": 3.701790260119384, + "y": 3.3 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_8_TO_CHARGE_2.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_8_TO_CHARGE_2.path new file mode 100644 index 0000000..c99ad46 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_8_TO_CHARGE_2.path @@ -0,0 +1,81 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.17, + "y": 4.44 + }, + "prevControl": null, + "nextControl": { + "x": 2.113595791705472, + "y": 3.7103480621877916 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.3727067193717724, + "y": 3.2738844262410747 + }, + "prevControl": { + "x": 2.167896837112962, + "y": 3.2738844262410747 + }, + "nextControl": { + "x": 2.769792385512419, + "y": 3.2738844262410747 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.3, + "y": 3.27 + }, + "prevControl": { + "x": 4.433625308269995, + "y": 3.27 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_TO_CHARGE.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_TO_CHARGE.path new file mode 100644 index 0000000..1f0945d --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_TO_CHARGE.path @@ -0,0 +1,52 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.4, + "y": 2.74 + }, + "prevControl": null, + "nextControl": { + "x": 3.4, + "y": 2.74 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.54, + "y": 2.74 + }, + "prevControl": { + "x": 4.050431845767949, + "y": 2.74 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "isReversed": null, + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_TO_CHARGE_ALIGN_I3.path b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_TO_CHARGE_ALIGN_I3.path new file mode 100644 index 0000000..806ad93 --- /dev/null +++ b/src/main/deploy/pathplanner/CHAMPSUNUSED/SCORE_TO_CHARGE_ALIGN_I3.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.498576, + "y": 3.357626 + }, + "prevControl": null, + "nextControl": { + "x": 3.498576, + "y": 3.357626 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.0, + "y": 3.357626 + }, + "prevControl": { + "x": 6.510431845767949, + "y": 3.357626 + }, + "nextControl": { + "x": 7.489568154232051, + "y": 3.357626 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 9.5, + "y": 3.357626 + }, + "prevControl": { + "x": 9.268018829089739, + "y": 3.357626 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_1_TO_SCORE_2.path b/src/main/deploy/pathplanner/INTAKE_1_TO_SCORE_2.path new file mode 100644 index 0000000..66b15b5 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_1_TO_SCORE_2.path @@ -0,0 +1,71 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.8, + "y": 1.04 + }, + "prevControl": null, + "nextControl": { + "x": 6.335831176573733, + "y": 1.04 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCube" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.3, + "y": 1.05 + }, + "prevControl": { + "x": 2.774586420187295, + "y": 0.9663176094431354 + }, + "nextControl": null, + "holonomicAngle": 165.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.5, + "names": [ + "prepScoreCube2" + ] + }, + { + "position": 0.0, + "names": [ + "postGroundIntake" + ] + }, + { + "position": 0.9491477272727269, + "names": [ + "forceCubeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_1_TO_WIRESHOT.path b/src/main/deploy/pathplanner/INTAKE_1_TO_WIRESHOT.path new file mode 100644 index 0000000..94a44b4 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_1_TO_WIRESHOT.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.05, + "y": 1.24 + }, + "prevControl": null, + "nextControl": { + "x": 6.585831176573733, + "y": 1.24 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.1, + "y": 0.6 + }, + "prevControl": { + "x": 5.353140843619115, + "y": 1.2954987517852137 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.4, + "names": [ + "wirePrepScore" + ] + }, + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_2_TO_CHARGE_2.path b/src/main/deploy/pathplanner/INTAKE_2_TO_CHARGE_2.path new file mode 100644 index 0000000..5ddfdd7 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_2_TO_CHARGE_2.path @@ -0,0 +1,81 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.1, + "y": 2.55 + }, + "prevControl": null, + "nextControl": { + "x": 7.491449978881401, + "y": 2.7760037506814506 + }, + "holonomicAngle": 30.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.57, + "y": 3.05 + }, + "prevControl": { + "x": 6.190833841409208, + "y": 3.05 + }, + "nextControl": { + "x": 5.002342163179404, + "y": 3.05 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.4, + "y": 3.05 + }, + "prevControl": { + "x": 3.6881075198270707, + "y": 3.05 + }, + "nextControl": null, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_2_TO_SCORE_1.path b/src/main/deploy/pathplanner/INTAKE_2_TO_SCORE_1.path new file mode 100644 index 0000000..8b27f87 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_2_TO_SCORE_1.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.05, + "y": 2.55 + }, + "prevControl": null, + "nextControl": { + "x": 5.957701828297997, + "y": 1.9193613565325127 + }, + "holonomicAngle": 30.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 1.25 + }, + "prevControl": { + "x": 4.416547516969998, + "y": 1.373931849080672 + }, + "nextControl": null, + "holonomicAngle": -175.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + }, + { + "position": 0.7, + "names": [ + "forceConeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_2_TO_SCORE_2.path b/src/main/deploy/pathplanner/INTAKE_2_TO_SCORE_2.path new file mode 100644 index 0000000..670cd87 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_2_TO_SCORE_2.path @@ -0,0 +1,93 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.05, + "y": 2.1 + }, + "prevControl": null, + "nextControl": { + "x": 6.830292237966282, + "y": 1.7194549933403493 + }, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.018231151045636, + "y": 1.214441913258826 + }, + "prevControl": { + "x": 4.841941929251928, + "y": 0.9146357083474845 + }, + "nextControl": { + "x": 3.6445029221457013, + "y": 1.3504678642834282 + }, + "holonomicAngle": 160.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCone0", + "forceConeShot" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.4, + "y": 1.65 + }, + "prevControl": { + "x": 2.7497047008020754, + "y": 1.3565629145360172 + }, + "nextControl": null, + "holonomicAngle": 160.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + }, + { + "position": 1.9500000000000002, + "names": [ + "forceConeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_3_CONE_TO_CHARGE_2.path b/src/main/deploy/pathplanner/INTAKE_3_CONE_TO_CHARGE_2.path new file mode 100644 index 0000000..367b8d0 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_3_CONE_TO_CHARGE_2.path @@ -0,0 +1,81 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.2, + "y": 3.43 + }, + "prevControl": null, + "nextControl": { + "x": 7.436130727106743, + "y": 3.0210095834229405 + }, + "holonomicAngle": 300.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.57, + "y": 2.53 + }, + "prevControl": { + "x": 6.166405657904978, + "y": 2.53 + }, + "nextControl": { + "x": 4.973594342095023, + "y": 2.53 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.6, + "y": 2.53 + }, + "prevControl": { + "x": 3.9014018979988427, + "y": 2.53 + }, + "nextControl": null, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_3_TO_SCORE_8.path b/src/main/deploy/pathplanner/INTAKE_3_TO_SCORE_8.path new file mode 100644 index 0000000..8b2b43a --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_3_TO_SCORE_8.path @@ -0,0 +1,102 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.92, + "y": 3.63 + }, + "prevControl": null, + "nextControl": { + "x": 6.375177661347359, + "y": 4.573659971644864 + }, + "holonomicAngle": -60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCube" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.1591419016171365, + "y": 4.4 + }, + "prevControl": { + "x": 5.10104165163369, + "y": 4.4 + }, + "nextControl": { + "x": 3.056425661875796, + "y": 4.4 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.5, + "y": 4.1 + }, + "prevControl": { + "x": 2.805948779131104, + "y": 4.1 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCube1", + "scoreCube1" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.0, + "maxAcceleration": null, + "isReversed": null, + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntake" + ] + }, + { + "position": 1.0, + "names": [ + "prepScoreCube1" + ] + }, + { + "position": 1.8975142045454536, + "names": [ + "forceCubeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_3_TO_SCORE_9_CONE.path b/src/main/deploy/pathplanner/INTAKE_3_TO_SCORE_9_CONE.path new file mode 100644 index 0000000..38fe043 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_3_TO_SCORE_9_CONE.path @@ -0,0 +1,93 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.5, + "y": 3.51 + }, + "prevControl": null, + "nextControl": { + "x": 6.955177661347359, + "y": 4.453659971644864 + }, + "holonomicAngle": -60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.3, + "y": 4.35 + }, + "prevControl": { + "x": 5.241899750016553, + "y": 4.35 + }, + "nextControl": { + "x": 3.1972837602586592, + "y": 4.35 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 4.35 + }, + "prevControl": { + "x": 3.305948779131104, + "y": 4.35 + }, + "nextControl": null, + "holonomicAngle": 167.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.0, + "maxAcceleration": null, + "isReversed": null, + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + }, + { + "position": 1.915056818181818, + "names": [ + "forceConeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/INTAKE_4_TO_SCORE_8.path b/src/main/deploy/pathplanner/INTAKE_4_TO_SCORE_8.path new file mode 100644 index 0000000..117edb5 --- /dev/null +++ b/src/main/deploy/pathplanner/INTAKE_4_TO_SCORE_8.path @@ -0,0 +1,96 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.94, + "y": 4.55 + }, + "prevControl": null, + "nextControl": { + "x": 5.813511936259985, + "y": 4.756355233821578 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCube" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.75531372590523, + "y": 4.796706327787655 + }, + "prevControl": { + "x": 4.552922412054962, + "y": 4.796706327787655 + }, + "nextControl": { + "x": 2.8697239934250143, + "y": 4.796706327787655 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.4, + "y": 4.440660233751023 + }, + "prevControl": { + "x": 3.081009183185686, + "y": 4.440660233751023 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntake" + ] + }, + { + "position": 0.9961647727272721, + "names": [ + "prepScoreCube2" + ] + }, + { + "position": 1.953764204545448, + "names": [ + "forceCubeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MOBILITY_INTAKE_2_TO_CHARGE.path b/src/main/deploy/pathplanner/MOBILITY_INTAKE_2_TO_CHARGE.path new file mode 100644 index 0000000..bd0fb77 --- /dev/null +++ b/src/main/deploy/pathplanner/MOBILITY_INTAKE_2_TO_CHARGE.path @@ -0,0 +1,90 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.55, + "y": 2.2 + }, + "prevControl": null, + "nextControl": { + "x": 7.11406657442139, + "y": 2.2 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.6, + "y": 2.3 + }, + "prevControl": { + "x": 6.100424097640838, + "y": 2.3 + }, + "nextControl": { + "x": 5.099575902359161, + "y": 2.3 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9, + "y": 2.295252795633797 + }, + "prevControl": { + "x": 4.172238078651217, + "y": 2.295252795633797 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + }, + { + "position": 0.0, + "names": [ + "event" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MOBILITY_INTAKE_3_TO_CHARGE.path b/src/main/deploy/pathplanner/MOBILITY_INTAKE_3_TO_CHARGE.path new file mode 100644 index 0000000..f8c8128 --- /dev/null +++ b/src/main/deploy/pathplanner/MOBILITY_INTAKE_3_TO_CHARGE.path @@ -0,0 +1,84 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.55, + "y": 3.21 + }, + "prevControl": null, + "nextControl": { + "x": 7.11406657442139, + "y": 3.21 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.75, + "y": 3.2 + }, + "prevControl": { + "x": 6.250424097640838, + "y": 3.2 + }, + "nextControl": { + "x": 5.249575902359162, + "y": 3.2 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.9, + "y": 3.2 + }, + "prevControl": { + "x": 4.143774791662821, + "y": 3.2 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MOBILITY_TO_CHARGE_4.path b/src/main/deploy/pathplanner/MOBILITY_TO_CHARGE_4.path new file mode 100644 index 0000000..1822608 --- /dev/null +++ b/src/main/deploy/pathplanner/MOBILITY_TO_CHARGE_4.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.12, + "y": 2.18 + }, + "prevControl": null, + "nextControl": { + "x": 6.249458066318134, + "y": 2.18 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7, + "y": 2.18 + }, + "prevControl": { + "x": 3.321100478200147, + "y": 2.18 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/MOBILITY_TO_CHARGE_6.path b/src/main/deploy/pathplanner/MOBILITY_TO_CHARGE_6.path new file mode 100644 index 0000000..19a5fe1 --- /dev/null +++ b/src/main/deploy/pathplanner/MOBILITY_TO_CHARGE_6.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 6.1, + "y": 3.3 + }, + "prevControl": null, + "nextControl": { + "x": 6.107077376961943, + "y": 3.3 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7, + "y": 3.3 + }, + "prevControl": { + "x": 3.686621229746352, + "y": 3.3 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_1_TO_WIRESHOT.path b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_1_TO_WIRESHOT.path new file mode 100644 index 0000000..94a44b4 --- /dev/null +++ b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_1_TO_WIRESHOT.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.05, + "y": 1.24 + }, + "prevControl": null, + "nextControl": { + "x": 6.585831176573733, + "y": 1.24 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.1, + "y": 0.6 + }, + "prevControl": { + "x": 5.353140843619115, + "y": 1.2954987517852137 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.4, + "names": [ + "wirePrepScore" + ] + }, + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_2_TO_CHARGE_2.path b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_2_TO_CHARGE_2.path new file mode 100644 index 0000000..4b5b30b --- /dev/null +++ b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_2_TO_CHARGE_2.path @@ -0,0 +1,87 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.11, + "y": 2.18 + }, + "prevControl": null, + "nextControl": { + "x": 7.485595771554933, + "y": 2.830550959441176 + }, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.5, + "y": 2.9 + }, + "prevControl": { + "x": 6.091670808700733, + "y": 2.9 + }, + "nextControl": { + "x": 5.040161216411942, + "y": 2.9 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.7, + "y": 2.9 + }, + "prevControl": { + "x": 3.922501516537441, + "y": 2.9 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 3.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_2_TO_SCORE_2.path b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_2_TO_SCORE_2.path new file mode 100644 index 0000000..6bb18d8 --- /dev/null +++ b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/INTAKE_2_TO_SCORE_2.path @@ -0,0 +1,118 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.05, + "y": 2.1 + }, + "prevControl": null, + "nextControl": { + "x": 6.658734878981326, + "y": 1.4223089311660713 + }, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitIntake", + "groundIntakeCone" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.1, + "y": 1.4 + }, + "prevControl": { + "x": 6.201856493963267, + "y": 1.4 + }, + "nextControl": { + "x": 4.714998381246025, + "y": 1.4 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.2, + "y": 1.4 + }, + "prevControl": { + "x": 3.579492085620951, + "y": 1.4 + }, + "nextControl": { + "x": 2.8205079143790495, + "y": 1.4 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCone0", + "forceConeShot" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.4, + "y": 1.65 + }, + "prevControl": { + "x": 2.5805399661959916, + "y": 1.4985089809521845 + }, + "nextControl": null, + "holonomicAngle": 160.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postGroundIntakeCone" + ] + }, + { + "position": 2.95, + "names": [ + "forceConeShot" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/SCORE_1_TO_INTAKE_CONE_1.path b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/SCORE_1_TO_INTAKE_CONE_1.path new file mode 100644 index 0000000..b88e0c0 --- /dev/null +++ b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/SCORE_1_TO_INTAKE_CONE_1.path @@ -0,0 +1,115 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8, + "y": 0.51 + }, + "prevControl": null, + "nextControl": { + "x": 1.8015173088070988, + "y": 0.5105522552419739 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": true, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.45567, + "y": 0.9 + }, + "prevControl": { + "x": 2.590654221681564, + "y": 0.9 + }, + "nextControl": { + "x": 4.251488685443741, + "y": 0.9 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.30657, + "y": 0.8984833867918937 + }, + "prevControl": { + "x": 4.250309846500218, + "y": 0.8984833867918937 + }, + "nextControl": { + "x": 5.235044183349022, + "y": 0.8984833867918937 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.05, + "y": 1.24 + }, + "prevControl": { + "x": 6.255429333266514, + "y": 1.24 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 2.3473721590909076, + "names": [ + "prepGroundIntakeCone" + ] + }, + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/WIRESHOT_TO_INTAKE_2.path b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/WIRESHOT_TO_INTAKE_2.path new file mode 100644 index 0000000..47e21d6 --- /dev/null +++ b/src/main/deploy/pathplanner/ORIGINALWIRECOVERPATHS/WIRESHOT_TO_INTAKE_2.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 5.1, + "y": 0.6 + }, + "prevControl": null, + "nextControl": { + "x": 5.304411276133676, + "y": 1.1616153654591785 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitWireScore", + "wireScore" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.05, + "y": 2.1 + }, + "prevControl": { + "x": 6.55, + "y": 1.2339745962155615 + }, + "nextControl": null, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "wirePostScore" + ] + }, + { + "position": 0.4, + "names": [ + "prepGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_1_TO_INTAKE_1.path b/src/main/deploy/pathplanner/SCORE_1_TO_INTAKE_1.path new file mode 100644 index 0000000..18dcce7 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_1_TO_INTAKE_1.path @@ -0,0 +1,90 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8, + "y": 0.51 + }, + "prevControl": null, + "nextControl": { + "x": 1.8015173088070988, + "y": 0.5105522552419739 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": true, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.85, + "y": 1.0 + }, + "prevControl": { + "x": 4.126597270258259, + "y": 1.0 + }, + "nextControl": { + "x": 6.375393151806239, + "y": 1.0 + }, + "holonomicAngle": 1.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.85, + "y": 1.0 + }, + "prevControl": { + "x": 6.2632358812844675, + "y": 1.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.8427556818181821, + "names": [ + "prepGroundIntake" + ] + }, + { + "position": 0.01122159090909091, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_1_TO_INTAKE_CONE_1.path b/src/main/deploy/pathplanner/SCORE_1_TO_INTAKE_CONE_1.path new file mode 100644 index 0000000..b88e0c0 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_1_TO_INTAKE_CONE_1.path @@ -0,0 +1,115 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8, + "y": 0.51 + }, + "prevControl": null, + "nextControl": { + "x": 1.8015173088070988, + "y": 0.5105522552419739 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": true, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.45567, + "y": 0.9 + }, + "prevControl": { + "x": 2.590654221681564, + "y": 0.9 + }, + "nextControl": { + "x": 4.251488685443741, + "y": 0.9 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.30657, + "y": 0.8984833867918937 + }, + "prevControl": { + "x": 4.250309846500218, + "y": 0.8984833867918937 + }, + "nextControl": { + "x": 5.235044183349022, + "y": 0.8984833867918937 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.05, + "y": 1.24 + }, + "prevControl": { + "x": 6.255429333266514, + "y": 1.24 + }, + "nextControl": null, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 2.3473721590909076, + "names": [ + "prepGroundIntakeCone" + ] + }, + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_2_TO_CHARGE_2.path b/src/main/deploy/pathplanner/SCORE_2_TO_CHARGE_2.path new file mode 100644 index 0000000..8c234c9 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_2_TO_CHARGE_2.path @@ -0,0 +1,81 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.3, + "y": 1.05 + }, + "prevControl": null, + "nextControl": { + "x": 2.2575749771485722, + "y": 1.534920230137662 + }, + "holonomicAngle": 170.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.5, + "y": 2.34 + }, + "prevControl": { + "x": 1.9634373336804942, + "y": 2.34 + }, + "nextControl": { + "x": 3.0365626663195058, + "y": 2.34 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.43, + "y": 2.34 + }, + "prevControl": { + "x": 4.161718666840247, + "y": 2.34 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_2_TO_INTAKE_2.path b/src/main/deploy/pathplanner/SCORE_2_TO_INTAKE_2.path new file mode 100644 index 0000000..bc7f3ac --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_2_TO_INTAKE_2.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.3, + "y": 1.05 + }, + "prevControl": null, + "nextControl": { + "x": 4.49073299720831, + "y": 0.25263839779139174 + }, + "holonomicAngle": 170.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCube2", + "scoreCube2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.1, + "y": 2.55 + }, + "prevControl": { + "x": 6.233974596215561, + "y": 2.05 + }, + "nextControl": null, + "holonomicAngle": 30.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.06, + "names": [ + "postScore" + ] + }, + { + "position": 0.25, + "names": [ + "prepGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_4_TO_INTAKE_2.path b/src/main/deploy/pathplanner/SCORE_4_TO_INTAKE_2.path new file mode 100644 index 0000000..f783033 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_4_TO_INTAKE_2.path @@ -0,0 +1,140 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.79705, + "y": 2.1844 + }, + "prevControl": null, + "nextControl": { + "x": 2.238598570415018, + "y": 2.1844 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.5, + "y": 2.138426 + }, + "prevControl": { + "x": 2.0744295669439907, + "y": 2.138426 + }, + "nextControl": { + "x": 2.9255704330560093, + "y": 2.138426 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.65, + "y": 2.138426 + }, + "prevControl": { + "x": 5.43555475951556, + "y": 2.138426 + }, + "nextControl": { + "x": 5.86444524048444, + "y": 2.138426 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.4, + "y": 2.2 + }, + "prevControl": { + "x": 6.311926303255559, + "y": 2.2 + }, + "nextControl": { + "x": 6.488073696744442, + "y": 2.2 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.55, + "y": 2.2 + }, + "prevControl": { + "x": 7.476469316351541, + "y": 2.2 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + }, + { + "position": 2.0760298295454547, + "names": [ + "prepGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_4_TO_MOBILITY.path b/src/main/deploy/pathplanner/SCORE_4_TO_MOBILITY.path new file mode 100644 index 0000000..f24127c --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_4_TO_MOBILITY.path @@ -0,0 +1,84 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.79705, + "y": 2.1844 + }, + "prevControl": null, + "nextControl": { + "x": 2.361979235042634, + "y": 2.1844 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.498576, + "y": 2.1844 + }, + "prevControl": { + "x": 2.16178721584989, + "y": 2.1844 + }, + "nextControl": { + "x": 2.83536478415011, + "y": 2.1844 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.12, + "y": 2.1844 + }, + "prevControl": { + "x": 6.062951592540703, + "y": 2.1844 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_6_TO_INTAKE_3.path b/src/main/deploy/pathplanner/SCORE_6_TO_INTAKE_3.path new file mode 100644 index 0000000..8f64136 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_6_TO_INTAKE_3.path @@ -0,0 +1,140 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.79705, + "y": 3.302 + }, + "prevControl": null, + "nextControl": { + "x": 2.361979235042634, + "y": 3.302 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.498576, + "y": 3.357626 + }, + "prevControl": { + "x": 2.16178721584989, + "y": 3.357626 + }, + "nextControl": { + "x": 2.83536478415011, + "y": 3.357626 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.65, + "y": 3.36 + }, + "prevControl": { + "x": 5.524226691532708, + "y": 3.36 + }, + "nextControl": { + "x": 5.775773308467293, + "y": 3.36 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.4, + "y": 3.21 + }, + "prevControl": { + "x": 6.342951592540703, + "y": 3.21 + }, + "nextControl": { + "x": 6.457048407459298, + "y": 3.21 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.55, + "y": 3.21 + }, + "prevControl": { + "x": 7.481790260119384, + "y": 3.21 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + }, + { + "position": 2.08, + "names": [ + "prepGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_6_TO_MOBILITY.path b/src/main/deploy/pathplanner/SCORE_6_TO_MOBILITY.path new file mode 100644 index 0000000..e15c092 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_6_TO_MOBILITY.path @@ -0,0 +1,84 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.79705, + "y": 3.302 + }, + "prevControl": null, + "nextControl": { + "x": 2.361979235042634, + "y": 3.302 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.498576, + "y": 3.3 + }, + "prevControl": { + "x": 2.16178721584989, + "y": 3.3 + }, + "nextControl": { + "x": 2.83536478415011, + "y": 3.3 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.1, + "y": 3.3 + }, + "prevControl": { + "x": 6.042951592540702, + "y": 3.3 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_8_TO_INTAKE_3.path b/src/main/deploy/pathplanner/SCORE_8_TO_INTAKE_3.path new file mode 100644 index 0000000..d019198 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_8_TO_INTAKE_3.path @@ -0,0 +1,93 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.17, + "y": 4.44 + }, + "prevControl": null, + "nextControl": { + "x": 2.816700299941278, + "y": 4.44 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCube2", + "scoreCube2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.341282094022522, + "y": 4.678352894900234 + }, + "prevControl": { + "x": 3.381517470764554, + "y": 4.650106876653835 + }, + "nextControl": { + "x": 5.547141788728022, + "y": 4.713841528377501 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.146043897990574, + "y": 3.51 + }, + "prevControl": { + "x": 6.6221726854695575, + "y": 4.417371556709117 + }, + "nextControl": null, + "holonomicAngle": 300.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.0, + "maxAcceleration": null, + "isReversed": null, + "markers": [ + { + "position": 0.062286931818181845, + "names": [ + "postScore" + ] + }, + { + "position": 1.2087357954545443, + "names": [ + "prepGroundIntakeCube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_8_TO_INTAKE_3_CONE.path b/src/main/deploy/pathplanner/SCORE_8_TO_INTAKE_3_CONE.path new file mode 100644 index 0000000..b438bf0 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_8_TO_INTAKE_3_CONE.path @@ -0,0 +1,93 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.17, + "y": 4.44 + }, + "prevControl": null, + "nextControl": { + "x": 2.816700299941278, + "y": 4.44 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScoreCube2", + "scoreCube2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.341282094022522, + "y": 4.678352894900234 + }, + "prevControl": { + "x": 3.381517470764554, + "y": 4.650106876653835 + }, + "nextControl": { + "x": 5.547141788728022, + "y": 4.713841528377501 + }, + "holonomicAngle": 270.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.5, + "y": 3.51 + }, + "prevControl": { + "x": 6.9761287874789835, + "y": 4.417371556709117 + }, + "nextControl": null, + "holonomicAngle": 300.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 4.0, + "maxAcceleration": null, + "isReversed": null, + "markers": [ + { + "position": 0.062286931818181845, + "names": [ + "postScore" + ] + }, + { + "position": 1.2087357954545443, + "names": [ + "prepGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/SCORE_9_TO_INTAKE_4.path b/src/main/deploy/pathplanner/SCORE_9_TO_INTAKE_4.path new file mode 100644 index 0000000..f927c48 --- /dev/null +++ b/src/main/deploy/pathplanner/SCORE_9_TO_INTAKE_4.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8, + "y": 4.98 + }, + "prevControl": null, + "nextControl": { + "x": 2.6800243745516896, + "y": 4.792945044759482 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitScore", + "scoreCone2" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.935605579544493, + "y": 4.547648025851608 + }, + "prevControl": { + "x": 5.407181682328561, + "y": 4.547648025851608 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.683636363636358, + "names": [ + "prepGroundIntakeCube" + ] + }, + { + "position": 0.0, + "names": [ + "postScore" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/WIRESHOT_TO_INTAKE_2.path b/src/main/deploy/pathplanner/WIRESHOT_TO_INTAKE_2.path new file mode 100644 index 0000000..47e21d6 --- /dev/null +++ b/src/main/deploy/pathplanner/WIRESHOT_TO_INTAKE_2.path @@ -0,0 +1,65 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 5.1, + "y": 0.6 + }, + "prevControl": null, + "nextControl": { + "x": 5.304411276133676, + "y": 1.1616153654591785 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "waitWireScore", + "wireScore" + ], + "executionBehavior": "parallelDeadline", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.05, + "y": 2.1 + }, + "prevControl": { + "x": 6.55, + "y": 1.2339745962155615 + }, + "nextControl": null, + "holonomicAngle": 60.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 0.0, + "names": [ + "wirePostScore" + ] + }, + { + "position": 0.4, + "names": [ + "prepGroundIntakeCone" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/com/team254/lib/Constants.java b/src/main/java/com/team254/lib/Constants.java new file mode 100644 index 0000000..105bff3 --- /dev/null +++ b/src/main/java/com/team254/lib/Constants.java @@ -0,0 +1,490 @@ +package com.team254.lib; + +import com.team254.lib.drivers.CanDeviceId; +import com.team254.lib.drivers.ServoMotorSubsystem.ServoMotorSubsystemConstants; +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.swerve.SwerveDriveKinematics; +import com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits; +import com.team254.lib.util.InterpolatingDouble; +import com.team254.lib.util.InterpolatingTreeMap; +import com.team254.lib.util.PolynomialRegression; +import com.team254.lib.util.ShootingParameters; +import com.team254.lib.vision.GoalTracker; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import java.net.NetworkInterface; +import java.net.SocketException; +import java.util.Enumeration; + +public class Constants { + // TODO resolve usage and centralize + // public static final double kLooperDt = 0.01; + public static final double kLooperDt = 0.02; + + // CAN + public static final int kCANTimeoutMs = 10; // use for important on the fly updates + public static final int kLongCANTimeoutMs = 100; // use for constructors + public static final String kRioCANBusName = "rio"; + public static final String kCANivoreCANBusName = "canivore"; + public static final double kCancoderBootAllowanceSeconds = 10.0; + + // Swerve Config + public static final String kPracticeBotMACAddress = "00:80:2F:33:D1:5F"; + public static final boolean kPracticeBot = getMACAddress().equals(kPracticeBotMACAddress); + public static final boolean kUseVelocityDrive = true; + + // Controls + public static final boolean kForceDriveGamepad = false; + public static final boolean kUseHeadingController = false; + public static final int kDriveGamepadPort = 0; + public static final int kMainThrottleJoystickPort = 0; + public static final int kMainTurnJoystickPort = 1; + public static final int kOperatorControllerPort = 2; + public static final double kDriveJoystickThreshold = 0.075; + public static final double kJoystickThreshold = 0.1; + public static final boolean kLimelightSnapshotMode = false; + + public static final int kPigeonIMUId = 20; + + // Drive constants + public static final double kDriveReduction = (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0); + public static final double kSteerReduction = (14.0 / 50.0) * (10.0 / 60.0); + public static final double kDriveWheelDiameter = 0.10033 * 81.0 / 84.213; /// meters, TODO measure + public static final double kDriveTrackwidthMeters = 0.61595; // DONE Measure and set trackwidth + public static final double kDriveWheelbaseMeters = 0.61595; // DONE Measure and set wheelbase + + public static final double kMaxDriveVoltage = 12.0; + + // Measure the drivetrain's maximum velocity or calculate the theoretical. + // The formula for calculating the theoretical maximum velocity is: + // / 60 * * * pi + // TODO measure this + public static final double kMaxVelocityMetersPerSecond = 4.959668; + + // Robot constants + public static final double kMaxDriveAcceleration = 1867 * 0.8; // m/s^2 tuned 2/18 practice bot + public static final double kTrackScrubFactor = 1; + + /** + * The maximum angular velocity of the robot in radians per second. + *

+ * This is a measure of how fast the robot can rotate in place. + */ + // Here we calculate the theoretical maximum angular velocity. You can also replace this with a + // measured amount. + public static final double kMaxAngularVelocityRadiansPerSecond = 11.386413; + + public static final double kScaleTranslationInputs = 0.5; + public static final double kScaleRotationInputs = 0.2; + + public static final KinematicLimits kUncappedKinematicLimits = new KinematicLimits(); + static { + kUncappedKinematicLimits.kMaxDriveVelocity = Constants.kMaxVelocityMetersPerSecond; + kUncappedKinematicLimits.kMaxDriveAcceleration = Double.MAX_VALUE; + kUncappedKinematicLimits.kMaxSteeringVelocity = Double.MAX_VALUE; + } + + public static final KinematicLimits kAzimuthOnlyKinematicLimits = new KinematicLimits(); + static { + kAzimuthOnlyKinematicLimits.kMaxDriveVelocity = Constants.kMaxVelocityMetersPerSecond; + kAzimuthOnlyKinematicLimits.kMaxDriveAcceleration = Double.MAX_VALUE; + kAzimuthOnlyKinematicLimits.kMaxSteeringVelocity = Units.degreesToRadians(1500.0); + } + + public static final KinematicLimits kTeleopKinematicLimits = new KinematicLimits(); + static { + kTeleopKinematicLimits.kMaxDriveVelocity = Constants.kMaxVelocityMetersPerSecond; + kTeleopKinematicLimits.kMaxDriveAcceleration = kTeleopKinematicLimits.kMaxDriveVelocity / 0.1; + kTeleopKinematicLimits.kMaxSteeringVelocity = Units.degreesToRadians(1500.0); + } + + public static final KinematicLimits kFastKinematicLimits = new KinematicLimits(); + static { + kFastKinematicLimits.kMaxDriveVelocity = Constants.kMaxVelocityMetersPerSecond; + kFastKinematicLimits.kMaxDriveAcceleration = kFastKinematicLimits.kMaxDriveVelocity / 0.2; + kFastKinematicLimits.kMaxSteeringVelocity = Units.degreesToRadians(1000.0); + } + + public static final KinematicLimits kSmoothKinematicLimits = new KinematicLimits(); + static { + kSmoothKinematicLimits.kMaxDriveVelocity = Constants.kMaxVelocityMetersPerSecond * .7; + kSmoothKinematicLimits.kMaxDriveAcceleration = kSmoothKinematicLimits.kMaxDriveVelocity / 1.0; + kSmoothKinematicLimits.kMaxSteeringVelocity = Units.degreesToRadians(750.0); + } + + public static final SwerveDriveKinematics kKinematics = new SwerveDriveKinematics( + // Front left + new Translation2d( + Constants.kDriveTrackwidthMeters / 2.0, Constants.kDriveWheelbaseMeters / 2.0 + ), + // Front right + new Translation2d( + Constants.kDriveTrackwidthMeters / 2.0, -Constants.kDriveWheelbaseMeters / 2.0 + ), + // Back left + new Translation2d( + -Constants.kDriveTrackwidthMeters / 2.0, Constants.kDriveWheelbaseMeters / 2.0 + ), + // Back right + new Translation2d( + -Constants.kDriveTrackwidthMeters / 2.0, -Constants.kDriveWheelbaseMeters / 2.0 + ) + ); + + // Module Configurations + public static final CanDeviceId kBackLeftDriveTalonId = new CanDeviceId(3, kCANivoreCANBusName); + public static final CanDeviceId kBackLeftAziTalonId = new CanDeviceId(2, kCANivoreCANBusName); + public static final CanDeviceId kBackLeftEncoderPortId = new CanDeviceId(4, kCANivoreCANBusName); + public static final Rotation2d254 kBackLeftAziEncoderOffset = + kPracticeBot ? Rotation2d254.fromDegrees(272.285) : Rotation2d254.fromDegrees(283.62); + + public static final CanDeviceId kBackRightDriveTalonId = new CanDeviceId(6, kCANivoreCANBusName); + public static final CanDeviceId kBackRightAziTalonId = new CanDeviceId(5, kCANivoreCANBusName); + public static final CanDeviceId kBackRightEncoderPortId = new CanDeviceId(7, kCANivoreCANBusName); + public static final Rotation2d254 kBackRightAziEncoderOffset = + kPracticeBot ? Rotation2d254.fromDegrees(194.67) : Rotation2d254.fromDegrees(201.5); + + public static final CanDeviceId kFrontRightDriveTalonId = + new CanDeviceId(12, kCANivoreCANBusName); + public static final CanDeviceId kFrontRightAziTalonId = new CanDeviceId(11, kCANivoreCANBusName); + public static final CanDeviceId kFrontRightEncoderPortId = + new CanDeviceId(13, kCANivoreCANBusName); + public static final Rotation2d254 kFrontRightAziEncoderOffset = + kPracticeBot ? Rotation2d254.fromDegrees(103.45) : Rotation2d254.fromDegrees(67.5); + + public static final CanDeviceId kFrontLeftDriveTalonId = new CanDeviceId(9, kCANivoreCANBusName); + public static final CanDeviceId kFrontLeftAziTalonId = new CanDeviceId(8, kCANivoreCANBusName); + public static final CanDeviceId kFrontLeftEncoderPortId = + new CanDeviceId(10, kCANivoreCANBusName); + public static final Rotation2d254 kFrontLeftAziEncoderOffset = + kPracticeBot ? Rotation2d254.fromDegrees(14.5) : Rotation2d254.fromDegrees(10.75); + + // TODO rename these to be steer/drive + public static final double kMk4AziKp = 0.75; + public static final double kMk4AziKi = 0; + public static final double kMk4AziKd = 15; + + public static final double kMk4DriveVelocityKp = 0.1; + public static final double kMk4DriveVelocityKi = 0.0; + public static final double kMk4DriveVelocityKd = 0.01; + public static final double kMk4DriveVelocityKf = 1023 + / (kMaxVelocityMetersPerSecond + / (Math.PI * Constants.kDriveWheelDiameter * Constants.kDriveReduction / 2048.0 * 10)); + + public static final double kMaxAngularSpeedRadiansPerSecond = kMaxVelocityMetersPerSecond + / Math.hypot(kDriveTrackwidthMeters / 2.0, kDriveWheelbaseMeters / 2.0); + public static final double kMaxAngularSpeedRadiansPerSecondSquared = kMaxVelocityMetersPerSecond + / Math.hypot(kDriveTrackwidthMeters / 2.0, kDriveWheelbaseMeters / 2.0); + public static final TrapezoidProfile.Constraints kThetaControllerConstraints = + new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared + ); + + // Left Intake + public static final CanDeviceId kLeftIntakeMotorId = + new CanDeviceId(15, Constants.kCANivoreCANBusName); + public static final int kLeftIntakeSolenoidId = 4; + public static final boolean kLeftIntakeInvertRoller = true; + + // Right Intake + public static final CanDeviceId kRightIntakeMotorId = new CanDeviceId(14, kCANivoreCANBusName); + public static final int kRightIntakeSolenoidId = 5; + public static final boolean kRightIntakeInvertRoller = false; + + // Feeder/Serializer + public static final CanDeviceId kRightFeederMasterId = new CanDeviceId(16, kCANivoreCANBusName); + public static final boolean kInvertRightFeeder = true; + public static final int kRightFeederBannerSensorId = 5; + public static final int kRightFeederColorSensorId = 9; + + public static final CanDeviceId kLeftFeederMasterId = new CanDeviceId(17, kCANivoreCANBusName); + public static final boolean kInvertLeftFeeder = true; + public static final int kLeftFeederBannerSensorId = 4; + public static final int kLeftFeederColorSensorId = 8; + + public static final int kUpperBannerSensorId = 6; + + public static final double kFeederKp = 0.04; + public static final double kFeederKi = 0.0; + public static final double kFeederKd = 2.0; + public static final double kFeederKf = 0.04844; + + // Shooter + public static final CanDeviceId kShooterLeftId = new CanDeviceId(18, kCANivoreCANBusName); + public static final CanDeviceId kShooterRightId = new CanDeviceId(19, kCANivoreCANBusName); + + public static final double kShooterKp = 0.03; + public static final double kShooterKi = 0.0003; + public static final double kShooterIZoneRPM = 100.0; + public static final double kShooterKd = 0.0; + public static final double kShooterKf = 0.04782; + public static final double kShooterTicksPerRevolution = 2048.0 + / (14.0 / 72.0); // based on gear reduction between encoder and output shaft, and encoder port + public static final double kShooterAllowablePercentError = 0.96; + + // CANdle + public static final int kCANdleId = 30; + + // Shoot on move + public static final boolean kAllowShootOnMove = true; + public static final double kMaxTurretAdjustmentForShootOnMove = 45.0; // degrees + + // Pneumatics + public static final int kAnalogSensorChannel = 0; + + // Turret + public static final boolean kTurretHomingUseReverseLimit = kPracticeBot ? false : true; + public static final ServoMotorSubsystemConstants kTurretConstants = + new ServoMotorSubsystemConstants(); + static { + kTurretConstants.kName = "Turret"; + + kTurretConstants.kMasterConstants.id = new CanDeviceId(21, Constants.kCANivoreCANBusName); + kTurretConstants.kMasterConstants.invert_motor = false; + kTurretConstants.kMasterConstants.invert_sensor_phase = true; + + // Unit == Degrees + kTurretConstants.kHomePosition = 0; + kTurretConstants.kTicksPerUnitDistance = + 1.0 / (1.0 / 2048.0 * 11.0 / 38.0 * 10.0 / 130.0 * 360.0); + + kTurretConstants.kPositionKp = 0.2; + kTurretConstants.kPositionKd = 0.0; + kTurretConstants.kPositionKi = 0.0; + kTurretConstants.kPositionIZone = 0; + + kTurretConstants.kKf = 0.045; // Tuned by Sending 0.4 percent output to turret motor and + // measuring closed loop velocity + kTurretConstants.kKa = 0.002; + kTurretConstants.kPositionDeadband = 50; + kTurretConstants.kCruiseVelocity = 20000; + kTurretConstants.kDeadband = 50; + kTurretConstants.kAcceleration = 20000; + + kTurretConstants.kEnableStatorCurrentLimit = false; + kTurretConstants.kStatorContinuousCurrentLimit = 60; + + kTurretConstants.kMinUnitsLimit = -115.0; + kTurretConstants.kMaxUnitsLimit = 115.0; + + // TODO current limits, should recover position on reset? + + kTurretConstants.kRecoverPositionOnReset = false; + } + + // Hood + public static final ServoMotorSubsystemConstants kHoodConstants = + new ServoMotorSubsystemConstants(); + static { + kHoodConstants.kName = "Hood"; + + kHoodConstants.kMasterConstants.id = new CanDeviceId(23, Constants.kCANivoreCANBusName); + + // Unit == Degrees + kHoodConstants.kHomePosition = 0.0; + kHoodConstants.kTicksPerUnitDistance = + 1.0 / ((1.0 / 2048.0) * (16.0 / 56.0) * (14.0 / 42.0) * (14.0 / 510.0) * 360.0); + + kHoodConstants.kPositionKp = 2.0; + kHoodConstants.kPositionKd = 0.0; + kHoodConstants.kPositionDeadband = (int) (0.2 * kHoodConstants.kTicksPerUnitDistance); // Ticks + + kHoodConstants.kMasterConstants.invert_motor = false; + kHoodConstants.kMasterConstants.invert_sensor_phase = false; + + kHoodConstants.kEnableStatorCurrentLimit = true; + kHoodConstants.kStatorContinuousCurrentLimit = 100; + kHoodConstants.kStatorPeakCurrentLimit = 100; + kHoodConstants.kStatorPeakCurrentDuration = 0.2; + + kHoodConstants.kMinUnitsLimit = 0.0; + kHoodConstants.kMaxUnitsLimit = 35.0; + + // TODO current limits, should recover position on reset? + kHoodConstants.kRecoverPositionOnReset = true; + } + + // Swerve Heading Controller + public static final double kSwerveHeadingControllerErrorTolerance = 1.5; // degree error + + // TODO tune heading controller snap PID + public static final double kSnapSwerveHeadingKp = 0.05; + public static final double kSnapSwerveHeadingKi = 0.0; + public static final double kSnapSwerveHeadingKd = 0.0075; + + // TODO tune heading controller maintain PID + public static final double kMaintainSwerveHeadingKp = 0.01; + public static final double kMaintainSwerveHeadingKi = 0.0; + public static final double kMaintainSwerveHeadingKd = 0.0; + + // TODO tune radius controller snap PID + public static final double kSnapRadiusKp = 2.0; + public static final double kSnapRadiusKi = 0.0; + public static final double kSnapRadiusKd = 0.0; + + // TODO tune radius controller maintain PID + public static final double kMaintainRadiusKp = 1.5; + public static final double kMaintainRadiusKi = 0.0; + public static final double kMaintainRadiusKd = 0.0; + + // Goal Tracker + public static final GoalTracker.Configuration kGoalTrackerConstants = + new GoalTracker.Configuration(); + static { + kGoalTrackerConstants.kMaxTrackerDistance = 1.0; // m + kGoalTrackerConstants.kMaxGoalTrackAge = + kLimelightSnapshotMode ? Double.MAX_VALUE : 20.0; // seconds + kGoalTrackerConstants.kMaxGoalTrackSmoothingTime = 0.5; // seconds + kGoalTrackerConstants.kCameraFrameRate = 22.0; // fps + + kGoalTrackerConstants.kStabilityWeight = 0.0; + kGoalTrackerConstants.kAgeWeight = 10.0; + kGoalTrackerConstants.kSwitchingWeight = 0.25; + } + + // Camera Calibration + public static final double kHorizontalFOV = 29.8 * 2; // degrees + public static final double kVerticalFOV = 24.85 * 2; // degrees + public static final double kResolutionWidthHalf = 960.0 / 2.0; // pixels + public static final double kResolutionHeightHalf = 720.0 / 2.0; // pixels + public static final double kLensHeight = 0.9190187; // Sensor to floor + public static final Rotation2d254 kHorizontalPlaneToLens = + Rotation2d254.fromDegrees(35.7); // Measured on comp bot + public static final double kVisionTargetHeight = 2.64; + public static final double kTurretRotationFudgeFactor = -1.5; + public static final Pose2d kTurretToLens = new Pose2d( + new Translation2d(-0.1882556, 0.0), Rotation2d254.fromDegrees(kTurretRotationFudgeFactor) + ); // Turret center to sensor + + public static final double kVisionTargetToGoalCenter = 0.67786; // meters + public static final double kLimelightTransmissionTimeLatency = 0.0 / 1000.0; // seconds + public static final double kImageCaptureLatency = 11.0 / 1000.0; // seconds + public static final double kMaxValidAziVelocity = 50.0; + + // Hood Tuning + public static final double kDefaultShooterRPM = 600; + public static final double kMaxHoodAngleForShooting = Constants.kHoodConstants.kMaxUnitsLimit; + public static final boolean kIsShooterTuning = false; + + // Positive direction means bias shots away from robot. + // This number is adjustable in SmartDashboard / Shuffleboard as "Shooter range offset" + public static final double kRangeOffset = -0.15; // meters + + public static final boolean kUseSinMap = true; + // Sin Map Parameters + public static final double kVerticalRpmPerMeter = 30.0; + public static final double kVerticalRpmOffset = 625.0; + public static final double kHorizontalRpmPerMeter = 93.142; + public static final double kHorizontalRpmOffset = 20.0; + // Exit angle of ball (in degrees) when hood is at "0" as measured on video + public static final double kHoodAngleOffset = 12.0; + + public static double[][] kRPMValues = { + {6.7, 1084}, + {5.5, 990}, + {4.5, 900}, + {3.5, 813}, + {2.5, 770}, + {2, 730}, + {1.5, 740}, + }; + + public static double[][] kHoodValues = { + {6.7, 25}, {5.5, 21.66}, {4.5, 19.2}, {3.5, 12}, {2.5, 7}, {2, 4.5}, {1.5, 0}}; + + public static InterpolatingTreeMap kHoodMap = + new InterpolatingTreeMap<>(); + public static InterpolatingTreeMap kRPMMap = + new InterpolatingTreeMap<>(); + + public static PolynomialRegression kHoodRegression; + public static PolynomialRegression kRPMRegression; + + static { + for (double[] pair : kRPMValues) { + kRPMMap.put(new InterpolatingDouble(pair[0]), new InterpolatingDouble(pair[1])); + } + + for (double[] pair : kHoodValues) { + kHoodMap.put(new InterpolatingDouble(pair[0]), new InterpolatingDouble(pair[1])); + } + + kHoodRegression = new PolynomialRegression(kHoodValues, 1); + kRPMRegression = new PolynomialRegression(kRPMValues, 1); + } + + public static final ShootingParameters kShootingParams = new ShootingParameters( + kHoodMap, // old hood map + kHoodMap, // medium hood map + kHoodMap, // new hood map + kRPMMap, // rpm map + Pose2d.identity(), // vision target to goal offset + 20.0, // shooter allowable error (rpm) + 0.4, // turret allowable error (m at goal) + 0.6 // hood allowable error (°) + ); + + // Climber + public static final double kClimberKp = 0.13; + public static final double kClimberKi = 0; + public static final double kClimberKd = 0; + + public static final double kClimberTraversalTransferKp = 0.13 / 8; + public static final double kClimberTraversalTransferKi = 0; + public static final double kClimberTraversalTransferKd = 0; + + public static final double kClimberTicksToInches = 1.0 / 2048.0 * 11.0 / 40.0 * 22.0 / 38.0 * 14.0 + / 48.0 // ticks to sprocket revs + * 0.25 / Math.sin(Math.toRadians(180.0 / 22.0)) + * Math.PI; // sprocket revs to inches of travel + + public static final CanDeviceId kClimberMasterId = new CanDeviceId(22, kCANivoreCANBusName); + public static final int kTailExtendId = 6; + public static final int kDropStingerId = 7; + + public static final Translation2d goalToTruss = new Translation2d(-7, 2.5); + public static final Translation2d goalToMiddleDriverStation = new Translation2d(-7, 0); + public static final boolean kTrussEject = true; + + /** + * @return the MAC address of the robot + */ + public static String getMACAddress() { + try { + Enumeration nwInterface = NetworkInterface.getNetworkInterfaces(); + StringBuilder ret = new StringBuilder(); + while (nwInterface.hasMoreElements()) { + NetworkInterface nis = nwInterface.nextElement(); + System.out.println("NIS: " + nis.getDisplayName()); + if (nis != null && "eth0".equals(nis.getDisplayName())) { + byte[] mac = nis.getHardwareAddress(); + if (mac != null) { + for (int i = 0; i < mac.length; i++) { + ret.append(String.format("%02X%s", mac[i], (i < mac.length - 1) ? ":" : "")); + } + String addr = ret.toString(); + System.out.println("NIS " + nis.getDisplayName() + " addr: " + addr); + return addr; + } else { + System.out.println("Address doesn't exist or is not accessible"); + } + } else { + System.out.println("Skipping adaptor: " + nis.getDisplayName()); + } + } + } catch (SocketException | NullPointerException e) { + e.printStackTrace(); + } + + return ""; + } + + // Pure Pursuit Constants + public static final double kPathLookaheadTime = 0.25; // From 1323 (2019) + public static final double kPathMinLookaheadDistance = 12.0; // From 1323 (2019) + public static final double kAdaptivePathMinLookaheadDistance = 6.0; + public static final double kAdaptivePathMaxLookaheadDistance = 24.0; + public static final double kAdaptiveErrorLookaheadCoefficient = 0.01; +} diff --git a/src/main/java/com/team254/lib/control/Lookahead.java b/src/main/java/com/team254/lib/control/Lookahead.java new file mode 100644 index 0000000..87d3c7c --- /dev/null +++ b/src/main/java/com/team254/lib/control/Lookahead.java @@ -0,0 +1,29 @@ +package com.team254.lib.control; + +/** + * A utility class for interpolating lookahead distance based on current speed. + */ +public class Lookahead { + public final double min_distance; + public final double max_distance; + public final double min_speed; + public final double max_speed; + + protected final double delta_distance; + protected final double delta_speed; + + public Lookahead(double min_distance, double max_distance, double min_speed, double max_speed) { + this.min_distance = min_distance; + this.max_distance = max_distance; + this.min_speed = min_speed; + this.max_speed = max_speed; + delta_distance = max_distance - min_distance; + delta_speed = max_speed - min_speed; + } + + public double getLookaheadForSpeed(double speed) { + double lookahead = delta_distance * (speed - min_speed) / delta_speed + min_distance; + return Double.isNaN(lookahead) ? min_distance + : Math.max(min_distance, Math.min(max_distance, lookahead)); + } +} diff --git a/src/main/java/com/team254/lib/control/RadiusController.java b/src/main/java/com/team254/lib/control/RadiusController.java new file mode 100644 index 0000000..990e6a5 --- /dev/null +++ b/src/main/java/com/team254/lib/control/RadiusController.java @@ -0,0 +1,87 @@ +package com.team254.lib.control; + +// import com.team254.frc2022.Constants; +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.SynchronousPIDF; + +/** + * Controls overall swerve heading of the robot through motion profile. + *

+ * All units are in degrees (for this class only) for easy integration with DPad + */ +public class RadiusController { + private static RadiusController mInstance; + + public static RadiusController getInstance() { + if (mInstance == null) { + mInstance = new RadiusController(); + } + + return mInstance; + } + + public enum RadiusControllerState { + OFF, + SNAP, + MAINTAIN, + } + + private final SynchronousPIDF mPIDFController; + private double mSetpoint = 0.0; + + private RadiusControllerState mRadiusControllerState = RadiusControllerState.OFF; + + private RadiusController() { + mPIDFController = new SynchronousPIDF(); + } + + public RadiusControllerState getRadiusControllerState() { + return mRadiusControllerState; + } + + public void setRadiusControllerState(RadiusControllerState state) { + mRadiusControllerState = state; + } + + /** + * @param goal_pos pos in degrees + */ + public void setGoal(double goal_pos) { + mSetpoint = goal_pos; + } + + public double getGoal() { + return mSetpoint; + } + + public boolean isAtGoal() { + // return mPIDFController.onTarget(Constants.kSwerveHeadingControllerErrorTolerance); + return false; + } + + /** + * Should be called from a looper at a constant dt + */ + public double update(double current_radius) { + mPIDFController.setSetpoint(mSetpoint); + + switch (mRadiusControllerState) { + case OFF: + return 0.0; + // case SNAP: + // mPIDFController.setPID( + // Constants.kSnapRadiusKp, Constants.kSnapRadiusKi, Constants.kSnapRadiusKd + // ); + // break; + // case MAINTAIN: + // mPIDFController.setPID( + // Constants.kMaintainRadiusKp, Constants.kMaintainRadiusKi, + // Constants.kMaintainRadiusKd + // ); + // break; + } + + return -mPIDFController.calculate(current_radius); + } +} diff --git a/src/main/java/com/team254/lib/control/SwerveHeadingController.java b/src/main/java/com/team254/lib/control/SwerveHeadingController.java new file mode 100644 index 0000000..71d1323 --- /dev/null +++ b/src/main/java/com/team254/lib/control/SwerveHeadingController.java @@ -0,0 +1,135 @@ +package com.team254.lib.control; + +// import com.team254.frc2022.Constants; +// import com.team254.frc2022.RobotState; +// import com.team254.frc2022.subsystems.Drive; +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.SynchronousPIDF; + +/** + * Controls overall swerve heading of the robot through motion profile. + *

+ * All units are in degrees (for this class only) for easy integration with DPad + */ +public class SwerveHeadingController { + private static SwerveHeadingController mInstance; + // private RobotState mRobotState = RobotState.getInstance(); + + public static SwerveHeadingController getInstance() { + if (mInstance == null) { + mInstance = new SwerveHeadingController(); + } + + return mInstance; + } + + public Pose2d centerOfGoal; + + public enum HeadingControllerState { + OFF, + SNAP, // for snapping to specific headings + MAINTAIN, // maintaining current heading while driving + POLAR_MAINTAIN, // for maintaining heading toward origin + POLAR_SNAP, // for snapping heading toward origin + } + + private final SynchronousPIDF mPIDFController; + private double mSetpoint = 0.0; + + private HeadingControllerState mHeadingControllerState = HeadingControllerState.OFF; + + private SwerveHeadingController() { + mPIDFController = new SynchronousPIDF(); + } + + public HeadingControllerState getHeadingControllerState() { + return mHeadingControllerState; + } + + public void setHeadingControllerState(HeadingControllerState state) { + mHeadingControllerState = state; + } + + /** + * @param goal_pos pos in degrees + */ + public void setGoal(double goal_pos) { + mSetpoint = goal_pos; + } + + public double getGoal() { + return mSetpoint; + } + + public boolean isAtGoal() { + // return mPIDFController.onTarget(Constants.kSwerveHeadingControllerErrorTolerance); + return false; + } + + public double calculateAngleToOrigin(Pose2d current_pose) { + // centerOfGoal = mRobotState.getFieldToGoal(); + double r = current_pose.getTranslation().distance(Translation2d.identity()); + double theta = Math.atan2(current_pose.getTranslation().y(), current_pose.getTranslation().x()); + double r_central = centerOfGoal.getTranslation().distance(Translation2d.identity()); + double theta_central = + Math.atan2(centerOfGoal.getTranslation().y(), centerOfGoal.getTranslation().x()); + + double angle = Math.toDegrees( + Math.PI + + Math.atan2( + r * Math.sin(theta) - r_central * Math.sin(theta_central), + r * Math.cos(theta) - r_central * Math.cos(theta_central) + ) + ); + if (angle < 0) { + return -angle; + } + return angle; + } + + /** + * Should be called from a looper at a constant dt + */ + public double update(double current_angle) { + mPIDFController.setSetpoint(mSetpoint); + double current_error = mSetpoint - current_angle; + + if (current_error > 180) { + current_angle += 360; + } else if (current_error < -180) { + current_angle -= 360; + } + + switch (mHeadingControllerState) { + case OFF: + return 0.0; + // case SNAP: + // mPIDFController.setPID( + // Constants.kSnapSwerveHeadingKp, Constants.kSnapSwerveHeadingKi, + // Constants.kSnapSwerveHeadingKd + // ); + // break; + // case MAINTAIN: + // mPIDFController.setPID( + // Constants.kMaintainSwerveHeadingKp, Constants.kMaintainSwerveHeadingKi, + // Constants.kMaintainSwerveHeadingKd + // ); + // break; + // case POLAR_MAINTAIN: + // mPIDFController.setPID( + // Constants.kMaintainSwerveHeadingKp, Constants.kMaintainSwerveHeadingKi, + // Constants.kMaintainSwerveHeadingKd + // ); + // break; + // case POLAR_SNAP: + // mPIDFController.setPID( + // Constants.kSnapSwerveHeadingKp, Constants.kSnapSwerveHeadingKi, + // Constants.kSnapSwerveHeadingKd + // ); + // break; + } + + return mPIDFController.calculate(current_angle); + } +} diff --git a/src/main/java/com/team254/lib/drivers/CanDeviceId.java b/src/main/java/com/team254/lib/drivers/CanDeviceId.java new file mode 100644 index 0000000..8abca08 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/CanDeviceId.java @@ -0,0 +1,28 @@ +package com.team254.lib.drivers; + +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, ""); + } + + public int getDeviceNumber() { + return mDeviceNumber; + } + + public String getBus() { + return mBus; + } + + public boolean equals(CanDeviceId other) { + return other.mDeviceNumber == mDeviceNumber && other.mBus == mBus; + } +} diff --git a/src/main/java/com/team254/lib/drivers/LazySolenoid.java b/src/main/java/com/team254/lib/drivers/LazySolenoid.java new file mode 100644 index 0000000..0cb503c --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/LazySolenoid.java @@ -0,0 +1,21 @@ +package com.team254.lib.drivers; + +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.Solenoid; + +public class LazySolenoid extends Solenoid { + private boolean mLastValue = false; + + public LazySolenoid(PneumaticsModuleType moduleType, int channel) { + super(moduleType, channel); + set(false); + } + + @Override + public void set(boolean on) { + if (on != mLastValue) { + super.set(on); + mLastValue = on; + } + } +} diff --git a/src/main/java/com/team254/lib/drivers/LazyTalonFX.java b/src/main/java/com/team254/lib/drivers/LazyTalonFX.java new file mode 100644 index 0000000..fb34339 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/LazyTalonFX.java @@ -0,0 +1,30 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +/** + * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping + * duplicate set commands. (By default the Talon flushes the Tx buffer on every set call). + */ +public class LazyTalonFX extends TalonFX { + protected double mLastSet = Double.NaN; + protected TalonFXControlMode mLastControlMode = null; + + public LazyTalonFX(CanDeviceId id) { + super(id.getDeviceNumber(), id.getBus()); + } + + public double getLastSet() { + return mLastSet; + } + + @Override + public void set(TalonFXControlMode mode, double value) { + if (value != mLastSet || mode != mLastControlMode) { + mLastSet = value; + mLastControlMode = mode; + super.set(mode, value); + } + } +} diff --git a/src/main/java/com/team254/lib/drivers/LazyTalonSRX.java b/src/main/java/com/team254/lib/drivers/LazyTalonSRX.java new file mode 100644 index 0000000..89edd4d --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/LazyTalonSRX.java @@ -0,0 +1,30 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +/** + * This class is a thin wrapper around the CANTalon that reduces CAN bus / CPU overhead by skipping + * duplicate set commands. (By default the Talon flushes the Tx buffer on every set call). + */ +public class LazyTalonSRX extends TalonSRX { + protected double mLastSet = Double.NaN; + protected ControlMode mLastControlMode = null; + + public LazyTalonSRX(int deviceNumber) { + super(deviceNumber); + } + + public double getLastSet() { + return mLastSet; + } + + @Override + public void set(ControlMode mode, double value) { + if (value != mLastSet || mode != mLastControlMode) { + mLastSet = value; + mLastControlMode = mode; + super.set(mode, value); + } + } +} diff --git a/src/main/java/com/team254/lib/drivers/MotorChecker.java b/src/main/java/com/team254/lib/drivers/MotorChecker.java new file mode 100644 index 0000000..adf467a --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/MotorChecker.java @@ -0,0 +1,127 @@ +package com.team254.lib.drivers; + +import com.team254.lib.util.Util; +import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.function.DoubleSupplier; + +public abstract class MotorChecker { + public static class CheckerConfig { + public double mCurrentFloor = 5; + public double mRPMFloor = 2000; + + public double mCurrentEpsilon = 5.0; + public double mRPMEpsilon = 500; + public DoubleSupplier mRPMSupplier = null; + + public double mRunTimeSec = 4.0; + public double mWaitTimeSec = 2.0; + public double mRunOutputPercentage = 0.5; + } + + public static class MotorConfig { + public String mName; + public T mMotor; + + public MotorConfig(String name, T motor) { + mName = name; + mMotor = motor; + } + } + + protected ArrayList> mMotorsToCheck; + + protected abstract void storeConfiguration(); + + protected abstract void restoreConfiguration(); + + protected abstract void setMotorOutput(T motor, double output); + + protected abstract double getMotorCurrent(T motor); + + protected boolean checkMotorsImpl( + Subsystem subsystem, ArrayList> motorsToCheck, CheckerConfig checkerConfig + ) { + boolean failure = false; + System.out.println("////////////////////////////////////////////////"); + System.out.println( + "Checking subsystem " + subsystem.getClass() + " for " + motorsToCheck.size() + " motors." + ); + + ArrayList currents = new ArrayList<>(); + ArrayList rpms = new ArrayList<>(); + + mMotorsToCheck = motorsToCheck; + storeConfiguration(); + + for (MotorConfig config : motorsToCheck) { + setMotorOutput(config.mMotor, 0.0); + } + + for (MotorConfig config : motorsToCheck) { + System.out.println("Checking: " + config.mName); + + setMotorOutput(config.mMotor, checkerConfig.mRunOutputPercentage); + Timer.delay(checkerConfig.mRunTimeSec); + + // poll the interesting information + double current = getMotorCurrent(config.mMotor); + currents.add(current); + System.out.print("Current: " + current); + + double rpm = Double.NaN; + if (checkerConfig.mRPMSupplier != null) { + rpm = checkerConfig.mRPMSupplier.getAsDouble(); + rpms.add(rpm); + System.out.print(" RPM: " + rpm); + } + System.out.print('\n'); + + setMotorOutput(config.mMotor, 0.0); + + // perform checks + if (current < checkerConfig.mCurrentFloor) { + System.out.println( + config.mName + " has failed current floor check vs " + checkerConfig.mCurrentFloor + + "!!" + ); + failure = true; + } + if (checkerConfig.mRPMSupplier != null) { + if (rpm < checkerConfig.mRPMFloor) { + System.out.println( + config.mName + " has failed rpm floor check vs " + checkerConfig.mRPMFloor + "!!" + ); + failure = true; + } + } + + Timer.delay(checkerConfig.mWaitTimeSec); + } + + // run aggregate checks + + if (currents.size() > 0) { + double average = currents.stream().mapToDouble(val -> val).average().getAsDouble(); + + if (!Util.allCloseTo(currents, average, checkerConfig.mCurrentEpsilon)) { + System.out.println("Currents varied!!!!!!!!!!!"); + failure = true; + } + } + + if (rpms.size() > 0) { + double average = rpms.stream().mapToDouble(val -> val).average().getAsDouble(); + + if (!Util.allCloseTo(rpms, average, checkerConfig.mRPMEpsilon)) { + System.out.println("RPMs varied!!!!!!!!"); + failure = true; + } + } + + // restore talon configurations + restoreConfiguration(); + + return !failure; + } +} diff --git a/src/main/java/com/team254/lib/drivers/PWMColorSensor.java b/src/main/java/com/team254/lib/drivers/PWMColorSensor.java new file mode 100644 index 0000000..e4209e6 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/PWMColorSensor.java @@ -0,0 +1,55 @@ +package com.team254.lib.drivers; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.SynchronousInterrupt; + +public class PWMColorSensor { + public static final double kPulsePeriod = 1.0 / 4482.0; + public static final double kPWMBound = 300.0 / 4095.0; + public static final double kBlueThreshold = 0.41; + public static final double kRedThreshold = 0.60; + + private final SynchronousInterrupt mInterrupt; + private double mPrevRatio = 0.5; + + public enum ColorSensorState { + RED, + BLUE, + NONE, + } + + public PWMColorSensor(int channel) { + mInterrupt = new SynchronousInterrupt(new DigitalInput(channel)); + mInterrupt.setInterruptEdges(true, true); + } + + private double adjustRatio(double value) { + double ret = (value - kPWMBound) / (1.0 - 2 * kPWMBound); + if (ret < 0.0 || ret > 1.0) { + return /* mPrevRatio */ 0.5; + } + return ret; + } + + public double getRatio() { + double rising = mInterrupt.getRisingTimestamp(); + double falling = mInterrupt.getFallingTimestamp(); + if (rising < falling) { + mPrevRatio = adjustRatio((falling - rising) / kPulsePeriod); + } else { + mPrevRatio = adjustRatio(1.0 + (falling - rising) / kPulsePeriod); + } + return mPrevRatio; + } + + public ColorSensorState getColor() { + double ratio = getRatio(); + + if (ratio > kRedThreshold) { + return ColorSensorState.RED; + } else if (ratio < kBlueThreshold) { + return ColorSensorState.BLUE; + } + return ColorSensorState.NONE; + } +} diff --git a/src/main/java/com/team254/lib/drivers/ServoMotorSubsystem.java b/src/main/java/com/team254/lib/drivers/ServoMotorSubsystem.java new file mode 100644 index 0000000..d649d02 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/ServoMotorSubsystem.java @@ -0,0 +1,717 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.team254.lib.loops.ILooper; +import com.team254.lib.loops.Loop; +import com.team254.lib.motion.IMotionProfileGoal; +import com.team254.lib.motion.MotionProfileConstraints; +import com.team254.lib.motion.MotionState; +import com.team254.lib.motion.SetpointGenerator; +import com.team254.lib.motion.SetpointGenerator.Setpoint; +import com.team254.lib.util.ReflectingCSVWriter; +import com.team254.lib.util.Util; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Abstract base class for a subsystem with a single sensored servo-mechanism. + */ +public abstract class ServoMotorSubsystem extends Subsystem { + private static final int kMotionProfileSlot = 0; + private static final int kPositionPIDSlot = 1; + + // Recommend initializing in a static block! + public static class TalonFXConstants { + public CanDeviceId id = new CanDeviceId(-1); + public boolean invert_motor = false; + public boolean invert_sensor_phase = false; + public int encoder_ppr = 2048; + } + + // Recommend initializing in a static block! + public static class ServoMotorSubsystemConstants { + public String kName = "ERROR_ASSIGN_A_NAME"; + + public double kLooperDt = 0.01; + public int kCANTimeoutMs = 10; // use for important on the fly updates + public int kLongCANTimeoutMs = 100; // use for constructors + + public TalonFXConstants kMasterConstants = new TalonFXConstants(); + public TalonFXConstants[] kSlaveConstants = new TalonFXConstants[0]; + + public double kHomePosition = 0.0; // Units + public double kTicksPerUnitDistance = 1.0; + public double kKp = 0; // Raw output / raw error + public double kKi = 0; // Raw output / sum of raw error + public double kKd = 0; // Raw output / (err - prevErr) + public double kKf = 0; // Raw output / velocity in ticks/100ms + public double kKa = 0; // Raw output / accel in (ticks/100ms) / s + public double kMaxIntegralAccumulator = 0; + public int kIZone = 0; // Ticks + public int kDeadband = 0; // Ticks + + public double kPositionKp = 0; + public double kPositionKi = 0; + public double kPositionKd = 0; + public double kPositionKf = 0; + public double kPositionMaxIntegralAccumulator = 0; + public int kPositionIZone = 0; // Ticks + public int kPositionDeadband = 0; // Ticks + + public int kCruiseVelocity = 0; // Ticks / 100ms + public int kAcceleration = 0; // Ticks / 100ms / s + public double kRampRate = 0.0; // s + public double kMaxVoltage = 12.0; + + public int kSupplyContinuousCurrentLimit = 20; // amps + public int kSupplyPeakCurrentLimit = 60; // amps + public double kSupplyPeakCurrentDuration = 0.2; // seconds + public boolean kEnableSupplyCurrentLimit = false; + + public int kStatorContinuousCurrentLimit = 20; // amps + public int kStatorPeakCurrentLimit = 60; // amps + public double kStatorPeakCurrentDuration = 0.2; // seconds + public boolean kEnableStatorCurrentLimit = false; + + public double kMaxUnitsLimit = Double.POSITIVE_INFINITY; + public double kMinUnitsLimit = Double.NEGATIVE_INFINITY; + + public int kStatusFrame8UpdateRate = 1000; + public boolean kRecoverPositionOnReset = false; + } + + protected final ServoMotorSubsystemConstants mConstants; + protected final TalonFX mMaster; + protected final TalonFX[] mSlaves; + + protected MotionState mMotionStateSetpoint = null; + + protected final int mForwardSoftLimitTicks; + protected final int mReverseSoftLimitTicks; + + protected ServoMotorSubsystem(final ServoMotorSubsystemConstants constants) { + mConstants = constants; + mMaster = TalonFXFactory.createDefaultTalon(mConstants.kMasterConstants.id); + mSlaves = new TalonFX[mConstants.kSlaveConstants.length]; + + TalonUtil.checkError( + mMaster.configSelectedFeedbackSensor( + FeedbackDevice.IntegratedSensor, 0, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not detect encoder: " + ); + + mForwardSoftLimitTicks = (int + ) ((mConstants.kMaxUnitsLimit - mConstants.kHomePosition) * mConstants.kTicksPerUnitDistance); + TalonUtil.checkError( + mMaster.configForwardSoftLimitThreshold( + mForwardSoftLimitTicks, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set forward soft limit: " + ); + + TalonUtil.checkError( + mMaster.configForwardSoftLimitEnable(true, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not enable forward soft limit: " + ); + + mReverseSoftLimitTicks = (int + ) ((mConstants.kMinUnitsLimit - mConstants.kHomePosition) * mConstants.kTicksPerUnitDistance); + TalonUtil.checkError( + mMaster.configReverseSoftLimitThreshold( + mReverseSoftLimitTicks, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set reverse soft limit: " + ); + + TalonUtil.checkError( + mMaster.configReverseSoftLimitEnable(true, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not enable reverse soft limit: " + ); + + TalonUtil.checkError( + mMaster.configVoltageCompSaturation(12.0, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set voltage compensation saturation: " + ); + + TalonUtil.checkError( + mMaster.config_kP(kMotionProfileSlot, mConstants.kKp, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": could not set kP: " + ); + + TalonUtil.checkError( + mMaster.config_kI(kMotionProfileSlot, mConstants.kKi, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": could not set kI: " + ); + + TalonUtil.checkError( + mMaster.config_kD(kMotionProfileSlot, mConstants.kKd, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": could not set kD: " + ); + + TalonUtil.checkError( + mMaster.config_kF(kMotionProfileSlot, mConstants.kKf, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set kF: " + ); + + TalonUtil.checkError( + mMaster.configMaxIntegralAccumulator( + kMotionProfileSlot, mConstants.kMaxIntegralAccumulator, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set max integral: " + ); + + TalonUtil.checkError( + mMaster.config_IntegralZone( + kMotionProfileSlot, mConstants.kIZone, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set i zone: " + ); + + TalonUtil.checkError( + mMaster.configAllowableClosedloopError( + kMotionProfileSlot, mConstants.kDeadband, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set deadband: " + ); + + TalonUtil.checkError( + mMaster.config_kP(kPositionPIDSlot, mConstants.kPositionKp, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": could not set kP: " + ); + + TalonUtil.checkError( + mMaster.config_kI(kPositionPIDSlot, mConstants.kPositionKi, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": could not set kI: " + ); + + TalonUtil.checkError( + mMaster.config_kD(kPositionPIDSlot, mConstants.kPositionKd, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": could not set kD: " + ); + + TalonUtil.checkError( + mMaster.config_kF(kPositionPIDSlot, mConstants.kPositionKf, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set kF: " + ); + + TalonUtil.checkError( + mMaster.configMaxIntegralAccumulator( + kPositionPIDSlot, + mConstants.kPositionMaxIntegralAccumulator, + mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set max integral: " + ); + + TalonUtil.checkError( + mMaster.config_IntegralZone( + kPositionPIDSlot, mConstants.kPositionIZone, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set i zone: " + ); + + TalonUtil.checkError( + mMaster.configAllowableClosedloopError( + kPositionPIDSlot, mConstants.kPositionDeadband, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set deadband: " + ); + + TalonUtil.checkError( + mMaster.configMotionCruiseVelocity( + mConstants.kCruiseVelocity, mConstants.kLongCANTimeoutMs + ), + mConstants.kName + ": Could not set cruise velocity: " + ); + + TalonUtil.checkError( + mMaster.configMotionAcceleration(mConstants.kAcceleration, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set acceleration: " + ); + + TalonUtil.checkError( + mMaster.configOpenloopRamp(mConstants.kRampRate, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set voltage ramp rate: " + ); + + TalonUtil.checkError( + mMaster.configClosedloopRamp(mConstants.kRampRate, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set closed loop ramp rate: " + ); + + TalonUtil.checkError( + mMaster.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration( + mConstants.kEnableSupplyCurrentLimit, + mConstants.kSupplyContinuousCurrentLimit, + mConstants.kSupplyPeakCurrentLimit, + mConstants.kSupplyPeakCurrentDuration + )), + mConstants.kName + ": Could not set supply current limit." + ); + + TalonUtil.checkError( + mMaster.configStatorCurrentLimit(new StatorCurrentLimitConfiguration( + mConstants.kEnableStatorCurrentLimit, + mConstants.kStatorContinuousCurrentLimit, + mConstants.kStatorPeakCurrentLimit, + mConstants.kStatorPeakCurrentDuration + )), + mConstants.kName + ": Could not set stator current limit." + ); + + mMaster.configVoltageMeasurementFilter(8); + + TalonUtil.checkError( + mMaster.configVoltageCompSaturation(mConstants.kMaxVoltage, mConstants.kLongCANTimeoutMs), + mConstants.kName + ": Could not set voltage comp saturation." + ); + mMaster.enableVoltageCompensation(true); + + mMaster.setInverted(mConstants.kMasterConstants.invert_motor); + mMaster.setSensorPhase(mConstants.kMasterConstants.invert_sensor_phase); + mMaster.setNeutralMode(NeutralMode.Brake); + mMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 5, 20); + mMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 1000, 20); + mMaster.setStatusFramePeriod( + StatusFrameEnhanced.Status_8_PulseWidth, mConstants.kStatusFrame8UpdateRate, 20 + ); + + // Start with kMotionProfileSlot. + mMaster.selectProfileSlot(kMotionProfileSlot, 0); + + for (int i = 0; i < mSlaves.length; ++i) { + mSlaves[i] = TalonFXFactory.createPermanentSlaveTalon( + mConstants.kSlaveConstants[i].id, mConstants.kMasterConstants.id + ); + mSlaves[i].setInverted(mConstants.kSlaveConstants[i].invert_motor); + mSlaves[i].setNeutralMode(NeutralMode.Brake); + mSlaves[i].follow(mMaster); + } + + // The accel term can re-use the velocity unit conversion because both input and output units + // are per second. + mMotionProfileConstraints = new MotionProfileConstraints( + ticksPer100msToUnitsPerSecond(mConstants.kCruiseVelocity), + ticksPer100msToUnitsPerSecond(-mConstants.kCruiseVelocity), + ticksPer100msToUnitsPerSecond(mConstants.kAcceleration) + ); + + // Send a neutral command. + stop(); + } + + public static class PeriodicIO { + // INPUTS + public double timestamp; + public double position_ticks; + public double position_units; + public double velocity_ticks_per_100ms; + public double active_trajectory_position; // ticks + public double active_trajectory_velocity; // ticks/100ms + public double active_trajectory_acceleration; // ticks/100ms/s + public double output_percent; + public double output_voltage; + public double master_current; + public double error_ticks; + public int encoder_wraps; + public int absolute_pulse_offset = 0; + // public int absolute_pulse_position; + public int absolute_pulse_position_modded; + public boolean reset_occured; + + // OUTPUTS + public double demand; + public double feedforward; + } + + protected enum ControlState { OPEN_LOOP, MOTION_MAGIC, POSITION_PID, MOTION_PROFILING } + + protected PeriodicIO mPeriodicIO = new PeriodicIO(); + protected ControlState mControlState = ControlState.OPEN_LOOP; + protected ReflectingCSVWriter mCSVWriter = null; + protected boolean mHasBeenZeroed = false; + protected StickyFaults mFaults = new StickyFaults(); + protected SetpointGenerator mSetpointGenerator = new SetpointGenerator(); + protected MotionProfileConstraints mMotionProfileConstraints; + + @Override + public synchronized void readPeriodicInputs() { + mPeriodicIO.timestamp = Timer.getFPGATimestamp(); + + if (mMaster.hasResetOccurred()) { + DriverStation.reportError(mConstants.kName + ": Talon Reset! ", false); + mPeriodicIO.reset_occured = true; + return; + } else { + mPeriodicIO.reset_occured = false; + } + + mMaster.getStickyFaults(mFaults); + if (mFaults.hasAnyFault()) { + DriverStation.reportError(mConstants.kName + ": Talon Fault! " + mFaults.toString(), false); + mMaster.clearStickyFaults(0); + } + if (mMaster.getControlMode() == ControlMode.MotionMagic) { + mPeriodicIO.active_trajectory_position = mMaster.getActiveTrajectoryPosition(); + + if (mPeriodicIO.active_trajectory_position < mReverseSoftLimitTicks) { + DriverStation.reportError( + mConstants.kName + ": Active trajectory past reverse soft limit!", false + ); + } else if (mPeriodicIO.active_trajectory_position > mForwardSoftLimitTicks) { + DriverStation.reportError( + mConstants.kName + ": Active trajectory past forward soft limit!", false + ); + } + final double newVel = mMaster.getActiveTrajectoryVelocity(); + if (Util.epsilonEquals(newVel, mConstants.kCruiseVelocity, Math.max(1, mConstants.kDeadband)) + || Util.epsilonEquals( + newVel, mPeriodicIO.active_trajectory_velocity, Math.max(1, mConstants.kDeadband) + )) { + // Mechanism is ~constant velocity. + mPeriodicIO.active_trajectory_acceleration = 0.0; + } else { + // Mechanism is accelerating. + mPeriodicIO.active_trajectory_acceleration = + Math.signum(newVel - mPeriodicIO.active_trajectory_velocity) * mConstants.kAcceleration; + } + mPeriodicIO.active_trajectory_velocity = newVel; + } else { + mPeriodicIO.active_trajectory_position = Integer.MIN_VALUE; + mPeriodicIO.active_trajectory_velocity = 0; + mPeriodicIO.active_trajectory_acceleration = 0.0; + } + if (mMaster.getControlMode() == ControlMode.Position) { + mPeriodicIO.error_ticks = mMaster.getClosedLoopError(0); + } else { + mPeriodicIO.error_ticks = 0; + } + mPeriodicIO.master_current = mMaster.getStatorCurrent(); + mPeriodicIO.output_voltage = mMaster.getMotorOutputVoltage(); + mPeriodicIO.output_percent = mMaster.getMotorOutputPercent(); + mPeriodicIO.position_ticks = mMaster.getSelectedSensorPosition(0); + mPeriodicIO.position_units = ticksToHomedUnits(mPeriodicIO.position_ticks); + mPeriodicIO.velocity_ticks_per_100ms = mMaster.getSelectedSensorVelocity(0); + + if (mConstants.kRecoverPositionOnReset) { + mPeriodicIO.absolute_pulse_position_modded = + (int) mMaster.getSensorCollection().getIntegratedSensorAbsolutePosition(); + if (mPeriodicIO.absolute_pulse_position_modded < 0) { + mPeriodicIO.absolute_pulse_position_modded += mConstants.kMasterConstants.encoder_ppr; + } + + double estimated_pulse_pos = + ((mConstants.kMasterConstants.invert_sensor_phase ? -1 : 1) * mPeriodicIO.position_ticks) + + mPeriodicIO.absolute_pulse_offset; + int new_wraps = (int + ) Math.floor(estimated_pulse_pos / ((double) mConstants.kMasterConstants.encoder_ppr)); + // Only set this when we are really sure its a valid change + if (Math.abs(mPeriodicIO.encoder_wraps - new_wraps) <= 1) { + mPeriodicIO.encoder_wraps = new_wraps; + } + } + + if (mCSVWriter != null) { + mCSVWriter.add(mPeriodicIO); + } + } + + /** + * @return absolute encoders raw ticks bounded to one rotation + */ + protected int getAbsoluteEncoderRawPosition() { + int abs_raw_with_rollover = + (int) mMaster.getSensorCollection().getIntegratedSensorAbsolutePosition(); + return abs_raw_with_rollover + + (abs_raw_with_rollover < 0 + ? abs_raw_with_rollover + mConstants.kMasterConstants.encoder_ppr + : 0); + } + + @Override + public synchronized void writePeriodicOutputs() { + if (mControlState == ControlState.MOTION_MAGIC) { + mMaster.set( + ControlMode.MotionMagic, + mPeriodicIO.demand, + DemandType.ArbitraryFeedForward, + mPeriodicIO.feedforward + ); + } else if (mControlState == ControlState.POSITION_PID || mControlState == ControlState.MOTION_PROFILING) { + mMaster.set( + ControlMode.Position, + mPeriodicIO.demand, + DemandType.ArbitraryFeedForward, + mPeriodicIO.feedforward + ); + } else { + mMaster.set( + ControlMode.PercentOutput, + mPeriodicIO.demand, + DemandType.ArbitraryFeedForward, + mPeriodicIO.feedforward + ); + } + } + + public synchronized void handleMasterReset(boolean reset) {} + + @Override + public void registerEnabledLoops(ILooper mEnabledLooper) { + mEnabledLooper.register(new Loop() { + @Override + public void onStart(double timestamp) { + // if (mCSVWriter == null) { + // mCSVWriter = new ReflectingCSVWriter<>("/home/lvuser/" + // + mConstants.kName.replaceAll("[^A-Za-z0-9]+", "").toUpperCase() + + // "-LOGS.csv", PeriodicIO.class); + // } + } + + @Override + public void onLoop(double timestamp) { + if (mPeriodicIO.reset_occured) { + System.out.println( + mConstants.kName + ": Master Talon reset occurred; resetting frame rates." + ); + mMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 5, 20); + mMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, 20); + mMaster.setStatusFramePeriod( + StatusFrameEnhanced.Status_8_PulseWidth, mConstants.kStatusFrame8UpdateRate, 20 + ); + + // Reset encoder position to estimated position from absolute encoder + if (mConstants.kRecoverPositionOnReset) { + mMaster.setSelectedSensorPosition( + estimateSensorPositionFromAbsolute(), 0, mConstants.kCANTimeoutMs + ); + } + } + handleMasterReset(mPeriodicIO.reset_occured); + for (TalonFX slave : mSlaves) { + if (slave.hasResetOccurred()) { + System.out.println(mConstants.kName + ": Slave Talon reset occurred"); + } + } + } + + @Override + public void onStop(double timestamp) { + if (mCSVWriter != null) { + mCSVWriter.flush(); + mCSVWriter = null; + } + + stop(); + } + }); + } + + public synchronized double getPositionTicks() { + return mPeriodicIO.position_ticks; + } + + // In "Units" + public synchronized double getPosition() { + return ticksToHomedUnits(mPeriodicIO.position_ticks); + } + + // In "Units per second" + public synchronized double getVelocity() { + return ticksToUnits(mPeriodicIO.velocity_ticks_per_100ms) * 10.0; + } + + public synchronized boolean hasFinishedTrajectory() { + return Util.epsilonEquals( + mPeriodicIO.active_trajectory_position, + ticksToUnits(getSetpoint()), + Math.max(1, mConstants.kDeadband) + ); + } + + public synchronized double getSetpoint() { + return (mControlState == ControlState.MOTION_MAGIC || mControlState == ControlState.POSITION_PID + || mControlState == ControlState.MOTION_PROFILING) + ? ticksToUnits(mPeriodicIO.demand) + : Double.NaN; + } + + public synchronized double getSetpointHomed() { + return (mControlState == ControlState.MOTION_MAGIC || mControlState == ControlState.POSITION_PID + || mControlState == ControlState.MOTION_PROFILING) + ? ticksToHomedUnits(mPeriodicIO.demand) + : Double.NaN; + } + + public synchronized void setSetpointMotionMagic(double units, double feedforward_v) { + mPeriodicIO.demand = constrainTicks(homeAwareUnitsToTicks(units)); + mPeriodicIO.feedforward = unitsPerSecondToTicksPer100ms(feedforward_v) + * (mConstants.kKf + mConstants.kKd / 100.0) / 1023.0; + if (mControlState != ControlState.MOTION_MAGIC) { + mMaster.selectProfileSlot(kMotionProfileSlot, 0); + mControlState = ControlState.MOTION_MAGIC; + } + } + + public synchronized void setSetpointMotionMagic(double units) { + setSetpointMotionMagic(units, 0.0); + } + + public synchronized void setSetpointPositionPID(double units, double feedforward_v) { + mPeriodicIO.demand = constrainTicks(homeAwareUnitsToTicks(units)); + double feedforward_ticks_per_100ms = unitsPerSecondToTicksPer100ms(feedforward_v); + mPeriodicIO.feedforward = + feedforward_ticks_per_100ms * (mConstants.kKf + mConstants.kKd / 100.0) / 1023.0; + if (mControlState != ControlState.POSITION_PID) { + mMaster.selectProfileSlot(kPositionPIDSlot, 0); + mControlState = ControlState.POSITION_PID; + } + } + + public synchronized void setSetpointPositionPID(double units) { + setSetpointPositionPID(units, 0.0); + } + + public synchronized void setSetpointMotionProfiling( + IMotionProfileGoal goal, double feedforward_v + ) { + if (mControlState != ControlState.MOTION_PROFILING) { + mMaster.selectProfileSlot(kPositionPIDSlot, 0); + mControlState = ControlState.MOTION_PROFILING; + mMotionStateSetpoint = new MotionState( + mPeriodicIO.timestamp, + mPeriodicIO.position_units, + ticksPer100msToUnitsPerSecond(mPeriodicIO.velocity_ticks_per_100ms), + 0.0 + ); + mSetpointGenerator.reset(); + } + Setpoint setpoint = mSetpointGenerator.getSetpoint( + mMotionProfileConstraints, + goal, + mMotionStateSetpoint, + mPeriodicIO.timestamp + mConstants.kLooperDt + ); + mPeriodicIO.demand = constrainTicks(homeAwareUnitsToTicks(setpoint.motion_state.pos())); + mPeriodicIO.feedforward = + (unitsPerSecondToTicksPer100ms(feedforward_v + setpoint.motion_state.vel()) * mConstants.kKf + + unitsPerSecondToTicksPer100ms(setpoint.motion_state.acc()) * mConstants.kKa) + / 1023.0; + mMotionStateSetpoint = setpoint.motion_state; + } + + protected double ticksToUnits(double ticks) { + return ticks / mConstants.kTicksPerUnitDistance; + } + + protected double ticksToHomedUnits(double ticks) { + double val = ticksToUnits(ticks); + return val + mConstants.kHomePosition; + } + + protected double unitsToTicks(double units) { + return units * mConstants.kTicksPerUnitDistance; + } + + protected double homeAwareUnitsToTicks(double units) { + return unitsToTicks(units - mConstants.kHomePosition); + } + + protected double constrainTicks(double ticks) { + return Util.limit(ticks, mReverseSoftLimitTicks, mForwardSoftLimitTicks); + } + + protected double ticksPer100msToUnitsPerSecond(double ticks_per_100ms) { + return ticksToUnits(ticks_per_100ms) * 10.0; + } + + protected double unitsPerSecondToTicksPer100ms(double units_per_second) { + return unitsToTicks(units_per_second) / 10.0; + } + + public synchronized void setOpenLoop(double percentage) { + mPeriodicIO.demand = percentage; + if (mControlState != ControlState.OPEN_LOOP) { + mControlState = ControlState.OPEN_LOOP; + } + } + + public synchronized double getActiveTrajectoryUnits() { + return ticksToHomedUnits(mPeriodicIO.active_trajectory_position); + } + + public synchronized double getActiveTrajectoryVelocityUnitsPerSec() { + return ticksPer100msToUnitsPerSecond(mPeriodicIO.active_trajectory_velocity); + } + + public synchronized double getPredictedPositionUnits(double lookahead_secs) { + if (mMaster.getControlMode() != ControlMode.MotionMagic) { + return getPosition(); + } + + double predicted_units = ticksToHomedUnits( + mPeriodicIO.active_trajectory_position + + lookahead_secs * mPeriodicIO.active_trajectory_velocity + + 0.5 * mPeriodicIO.active_trajectory_acceleration * lookahead_secs * lookahead_secs + ); + if (mPeriodicIO.demand >= mPeriodicIO.active_trajectory_position) { + return Math.min(predicted_units, ticksToHomedUnits(mPeriodicIO.demand)); + } else { + return Math.max(predicted_units, ticksToHomedUnits(mPeriodicIO.demand)); + } + } + + public boolean atHomingLocation() { + return false; + } + + public synchronized void resetIfAtHome() { + if (atHomingLocation()) { + zeroSensors(); + } + } + + @Override + public synchronized void zeroSensors() { + mMaster.setSelectedSensorPosition(0, 0, mConstants.kCANTimeoutMs); + mPeriodicIO.absolute_pulse_offset = getAbsoluteEncoderRawPosition(); + mHasBeenZeroed = true; + } + + public synchronized void forceZero() { + mMaster.setSelectedSensorPosition(0, 0, mConstants.kCANTimeoutMs); + mPeriodicIO.absolute_pulse_offset = getAbsoluteEncoderRawPosition(); + } + + public synchronized boolean hasBeenZeroed() { + return mHasBeenZeroed; + } + + @Override + public void stop() { + setOpenLoop(0.0); + mMaster.set(ControlMode.PercentOutput, 0.0); + } + + public int estimateSensorPositionFromAbsolute() { + int estimated_pulse_pos = (mPeriodicIO.encoder_wraps * mConstants.kMasterConstants.encoder_ppr) + + mPeriodicIO.absolute_pulse_position_modded; + int estimate_position_ticks = (mConstants.kMasterConstants.invert_sensor_phase ? -1 : 1) + * (estimated_pulse_pos - mPeriodicIO.absolute_pulse_offset); + return estimate_position_ticks; + } + + @Override + public void outputTelemetry() { + SmartDashboard.putNumber(mConstants.kName + ": Position (units)", mPeriodicIO.position_units); + SmartDashboard.putBoolean(mConstants.kName + ": Homing Location", atHomingLocation()); + // synchronized (this) { + // if (mCSVWriter != null) { + // mCSVWriter.write(); + // } + // } + } +} diff --git a/src/main/java/com/team254/lib/drivers/Subsystem.java b/src/main/java/com/team254/lib/drivers/Subsystem.java new file mode 100644 index 0000000..9aad815 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/Subsystem.java @@ -0,0 +1,32 @@ +package com.team254.lib.drivers; + +import com.team254.lib.loops.ILooper; + +/** + * The Subsystem abstract class, which serves as a basic framework for all robot subsystems. Each + * subsystem outputs commands to SmartDashboard, has a stop routine (for after each match), and a + * routine to zero all sensors, which helps with calibration.

All Subsystems only have one + * instance (after all, one robot does not have two drivetrains), and functions get the instance of + * the drivetrain and act accordingly. Subsystems are also a state machine with a desired state and + * actual state; the robot code will try to match the two states with actions. Each Subsystem also + * is responsible for instantializing all member components at the start of the match. + */ +public abstract class Subsystem { + public void writeToLog() {} + + // Optional design pattern for caching periodic reads to avoid hammering the HAL/CAN. + public void readPeriodicInputs() {} + + // Optional design pattern for caching periodic writes to avoid hammering the HAL/CAN. + public void writePeriodicOutputs() {} + + public void registerEnabledLoops(ILooper mEnabledLooper) {} + + public void zeroSensors() {} + + public abstract void stop(); + + public abstract boolean checkSystem(); + + public abstract void outputTelemetry(); +} diff --git a/src/main/java/com/team254/lib/drivers/TalonChecker.java b/src/main/java/com/team254/lib/drivers/TalonChecker.java new file mode 100644 index 0000000..b93a657 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/TalonChecker.java @@ -0,0 +1,56 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.BaseTalon; +import java.util.ArrayList; + +public class TalonChecker extends MotorChecker { + private static class StoredTalonSRXConfiguration { + public ControlMode mMode; + public double mSetValue; + } + + protected ArrayList mStoredConfigurations = new ArrayList<>(); + + public static boolean checkMotors( + Subsystem subsystem, + ArrayList> motorsToCheck, + CheckerConfig checkerConfig + ) { + TalonChecker checker = new TalonChecker(); + return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig); + } + + @Override + protected void storeConfiguration() { + // record previous configuration for all talons + for (MotorConfig config : mMotorsToCheck) { + LazyTalonSRX talon = (LazyTalonSRX) config.mMotor; + + StoredTalonSRXConfiguration configuration = new StoredTalonSRXConfiguration(); + configuration.mMode = talon.getControlMode(); + configuration.mSetValue = talon.getLastSet(); + + mStoredConfigurations.add(configuration); + } + } + + @Override + protected void restoreConfiguration() { + for (int i = 0; i < mMotorsToCheck.size(); ++i) { + mMotorsToCheck.get(i).mMotor.set( + mStoredConfigurations.get(i).mMode, mStoredConfigurations.get(i).mSetValue + ); + } + } + + @Override + protected void setMotorOutput(BaseTalon motor, double output) { + motor.set(ControlMode.PercentOutput, output); + } + + @Override + protected double getMotorCurrent(BaseTalon motor) { + return motor.getSupplyCurrent(); + } +} diff --git a/src/main/java/com/team254/lib/drivers/TalonFXChecker.java b/src/main/java/com/team254/lib/drivers/TalonFXChecker.java new file mode 100644 index 0000000..02eb243 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/TalonFXChecker.java @@ -0,0 +1,58 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.BaseTalon; +import java.util.ArrayList; + +public class TalonFXChecker extends MotorChecker { + private static class StoredTalonFXConfiguration { + public ControlMode mMode; + public double mSetValue; + } + + protected ArrayList mStoredConfigurations = + new ArrayList<>(); + + public static boolean checkMotors( + Subsystem subsystem, + ArrayList> motorsToCheck, + MotorChecker.CheckerConfig checkerConfig + ) { + TalonFXChecker checker = new TalonFXChecker(); + return checker.checkMotorsImpl(subsystem, motorsToCheck, checkerConfig); + } + + @Override + protected void storeConfiguration() { + // record previous configuration for all talons + for (MotorChecker.MotorConfig config : mMotorsToCheck) { + LazyTalonFX talon = (LazyTalonFX) config.mMotor; + + TalonFXChecker.StoredTalonFXConfiguration configuration = + new TalonFXChecker.StoredTalonFXConfiguration(); + configuration.mMode = talon.getControlMode(); + configuration.mSetValue = talon.getLastSet(); + + mStoredConfigurations.add(configuration); + } + } + + @Override + protected void restoreConfiguration() { + for (int i = 0; i < mMotorsToCheck.size(); ++i) { + mMotorsToCheck.get(i).mMotor.set( + mStoredConfigurations.get(i).mMode, mStoredConfigurations.get(i).mSetValue + ); + } + } + + @Override + protected void setMotorOutput(BaseTalon motor, double output) { + motor.set(ControlMode.PercentOutput, output); + } + + @Override + protected double getMotorCurrent(BaseTalon motor) { + return motor.getSupplyCurrent(); + } +} diff --git a/src/main/java/com/team254/lib/drivers/TalonFXFactory.java b/src/main/java/com/team254/lib/drivers/TalonFXFactory.java new file mode 100644 index 0000000..88327b9 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/TalonFXFactory.java @@ -0,0 +1,180 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.ParamEnum; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; + +/** + * Creates CANTalon objects and configures all the parameters we care about to factory defaults. + * Closed-loop and sensor parameters are not set, as these are expected to be set by the + * application. + */ +public class TalonFXFactory { + private final static int kTimeoutMs = 100; + + public static class Configuration { + public NeutralMode NEUTRAL_MODE = NeutralMode.Coast; + // factory default + public double NEUTRAL_DEADBAND = 0.04; + + public SensorInitializationStrategy SENSOR_INITIALIZATION_STRATEGY = + SensorInitializationStrategy.BootToZero; + public double SENSOR_OFFSET_DEGREES = 0; + + public boolean ENABLE_SUPPLY_CURRENT_LIMIT = false; + public boolean ENABLE_STATOR_CURRENT_LIMIT = false; + + public boolean ENABLE_SOFT_LIMIT = false; + public boolean ENABLE_LIMIT_SWITCH = false; + public int FORWARD_SOFT_LIMIT = 0; + public int REVERSE_SOFT_LIMIT = 0; + + public boolean INVERTED = false; + public boolean SENSOR_PHASE = false; + + public int CONTROL_FRAME_PERIOD_MS = 10; + public int MOTION_CONTROL_FRAME_PERIOD_MS = 1000; + public int GENERAL_STATUS_FRAME_RATE_MS = 10; + public int FEEDBACK_STATUS_FRAME_RATE_MS = 1000; + public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; + public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; + public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; + + public SensorVelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = + SensorVelocityMeasPeriod.Period_100Ms; + public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64; + + public double OPEN_LOOP_RAMP_RATE = 0.0; + public double CLOSED_LOOP_RAMP_RATE = 0.0; + } + + private static final Configuration kDefaultConfiguration = new Configuration(); + private static final Configuration kSlaveConfiguration = new Configuration(); + + static { + // This control frame value seems to need to be something reasonable to avoid the Talon's + // LEDs behaving erratically. Potentially try to increase as much as possible. + kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 100; + kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000; + kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.ENABLE_SOFT_LIMIT = false; + } + + // create a CANTalon with the default (out of the box) configuration + public static TalonFX createDefaultTalon(CanDeviceId id) { + return createTalon(id, kDefaultConfiguration); + } + + public static TalonFX createPermanentSlaveTalon(CanDeviceId slave_id, CanDeviceId master_id) { + if (slave_id.getBus() != master_id.getBus()) { + throw new RuntimeException("Master and Slave Talons must be on the same CAN bus"); + } + final TalonFX talon = createTalon(slave_id, kSlaveConfiguration); + talon.set(ControlMode.Follower, master_id.getDeviceNumber()); + return talon; + } + + public static TalonFX createTalon(CanDeviceId id, Configuration config) { + TalonFX talon = new LazyTalonFX(id); + talon.set(ControlMode.PercentOutput, 0.0); + + talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS); + talon.clearMotionProfileHasUnderrun(kTimeoutMs); + talon.clearMotionProfileTrajectories(); + + talon.clearStickyFaults(kTimeoutMs); + + talon.configForwardLimitSwitchSource( + LimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled, kTimeoutMs + ); + talon.configReverseLimitSwitchSource( + LimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled, kTimeoutMs + ); + talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH); + + // Turn off re-zeroing by default. + talon.configSetParameter(ParamEnum.eClearPositionOnLimitF, 0, 0, 0, kTimeoutMs); + talon.configSetParameter(ParamEnum.eClearPositionOnLimitR, 0, 0, 0, kTimeoutMs); + + talon.configNominalOutputForward(0, kTimeoutMs); + talon.configNominalOutputReverse(0, kTimeoutMs); + talon.configNeutralDeadband(config.NEUTRAL_DEADBAND, kTimeoutMs); + + talon.configMotorCommutation(MotorCommutation.Trapezoidal); + + talon.configPeakOutputForward(1.0, kTimeoutMs); + talon.configPeakOutputReverse(-1.0, kTimeoutMs); + + talon.setNeutralMode(config.NEUTRAL_MODE); + + talon.configForwardSoftLimitThreshold(config.FORWARD_SOFT_LIMIT, kTimeoutMs); + talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs); + + talon.configReverseSoftLimitThreshold(config.REVERSE_SOFT_LIMIT, kTimeoutMs); + talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs); + talon.overrideSoftLimitsEnable(config.ENABLE_SOFT_LIMIT); + + talon.setInverted(config.INVERTED); + talon.setSensorPhase(config.SENSOR_PHASE); + + talon.selectProfileSlot(0, 0); + + talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, kTimeoutMs); + talon.configVelocityMeasurementWindow( + config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, kTimeoutMs + ); + + talon.configOpenloopRamp(config.OPEN_LOOP_RAMP_RATE, kTimeoutMs); + talon.configClosedloopRamp(config.CLOSED_LOOP_RAMP_RATE, kTimeoutMs); + + talon.configVoltageCompSaturation(0.0, kTimeoutMs); + talon.configVoltageMeasurementFilter(32, kTimeoutMs); + talon.enableVoltageCompensation(false); + + talon.configSupplyCurrentLimit( + new SupplyCurrentLimitConfiguration(config.ENABLE_SUPPLY_CURRENT_LIMIT, 20, 60, .2), + kTimeoutMs + ); + talon.configStatorCurrentLimit( + new StatorCurrentLimitConfiguration(config.ENABLE_STATOR_CURRENT_LIMIT, 20, 60, .2), + kTimeoutMs + ); + + talon.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, kTimeoutMs); + talon.configIntegratedSensorInitializationStrategy( + config.SENSOR_INITIALIZATION_STRATEGY, kTimeoutMs + ); + talon.configIntegratedSensorOffset(config.SENSOR_OFFSET_DEGREES, kTimeoutMs); + + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, kTimeoutMs + ); + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, kTimeoutMs + ); + + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_3_Quadrature, + config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, + kTimeoutMs + ); + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_4_AinTempVbat, + config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, + kTimeoutMs + ); + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_8_PulseWidth, config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, kTimeoutMs + ); + + talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS); + + return talon; + } +} diff --git a/src/main/java/com/team254/lib/drivers/TalonSRXFactory.java b/src/main/java/com/team254/lib/drivers/TalonSRXFactory.java new file mode 100644 index 0000000..73e379b --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/TalonSRXFactory.java @@ -0,0 +1,157 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.ParamEnum; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; + +/** + * Creates CANTalon objects and configures all the parameters we care about to factory defaults. + * Closed-loop and sensor parameters are not set, as these are expected to be set by the + * application. + */ +public class TalonSRXFactory { + private final static int kTimeoutMs = 100; + + public static class Configuration { + public NeutralMode NEUTRAL_MODE = NeutralMode.Coast; + // factory default + public double NEUTRAL_DEADBAND = 0.04; + + public boolean ENABLE_CURRENT_LIMIT = false; + public boolean ENABLE_SOFT_LIMIT = false; + public boolean ENABLE_LIMIT_SWITCH = false; + public int FORWARD_SOFT_LIMIT = 0; + public int REVERSE_SOFT_LIMIT = 0; + + public boolean INVERTED = false; + public boolean SENSOR_PHASE = false; + + public int CONTROL_FRAME_PERIOD_MS = 5; + public int MOTION_CONTROL_FRAME_PERIOD_MS = 100; + public int GENERAL_STATUS_FRAME_RATE_MS = 5; + public int FEEDBACK_STATUS_FRAME_RATE_MS = 100; + public int QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; + public int ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; + public int PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; + + public SensorVelocityMeasPeriod VELOCITY_MEASUREMENT_PERIOD = + SensorVelocityMeasPeriod.Period_100Ms; + public int VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64; + + public double OPEN_LOOP_RAMP_RATE = 0.0; + public double CLOSED_LOOP_RAMP_RATE = 0.0; + } + + private static final Configuration kDefaultConfiguration = new Configuration(); + private static final Configuration kSlaveConfiguration = new Configuration(); + + static { + // This control frame value seems to need to be something reasonable to avoid the Talon's + // LEDs behaving erratically. Potentially try to increase as much as possible. + kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 100; + kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000; + kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000; + kSlaveConfiguration.ENABLE_SOFT_LIMIT = false; + } + + // create a CANTalon with the default (out of the box) configuration + public static TalonSRX createDefaultTalon(int id) { + return createTalon(id, kDefaultConfiguration); + } + + public static TalonSRX createPermanentSlaveTalon(int id, int master_id) { + final TalonSRX talon = createTalon(id, kSlaveConfiguration); + talon.set(ControlMode.Follower, master_id); + return talon; + } + + public static TalonSRX createTalon(int id, Configuration config) { + TalonSRX talon = new LazyTalonSRX(id); + talon.set(ControlMode.PercentOutput, 0.0); + + talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS); + talon.clearMotionProfileHasUnderrun(kTimeoutMs); + talon.clearMotionProfileTrajectories(); + + talon.clearStickyFaults(kTimeoutMs); + + talon.configForwardLimitSwitchSource( + LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.Disabled, kTimeoutMs + ); + talon.configReverseLimitSwitchSource( + LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.Disabled, kTimeoutMs + ); + talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH); + + // Turn off re-zeroing by default. + talon.configSetParameter(ParamEnum.eClearPositionOnLimitF, 0, 0, 0, kTimeoutMs); + talon.configSetParameter(ParamEnum.eClearPositionOnLimitR, 0, 0, 0, kTimeoutMs); + + talon.configNominalOutputForward(0, kTimeoutMs); + talon.configNominalOutputReverse(0, kTimeoutMs); + talon.configNeutralDeadband(config.NEUTRAL_DEADBAND, kTimeoutMs); + + talon.configPeakOutputForward(1.0, kTimeoutMs); + talon.configPeakOutputReverse(-1.0, kTimeoutMs); + + talon.setNeutralMode(config.NEUTRAL_MODE); + + talon.configForwardSoftLimitThreshold(config.FORWARD_SOFT_LIMIT, kTimeoutMs); + talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs); + + talon.configReverseSoftLimitThreshold(config.REVERSE_SOFT_LIMIT, kTimeoutMs); + talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs); + talon.overrideSoftLimitsEnable(config.ENABLE_SOFT_LIMIT); + + talon.setInverted(config.INVERTED); + talon.setSensorPhase(config.SENSOR_PHASE); + + talon.selectProfileSlot(0, 0); + + talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, kTimeoutMs); + talon.configVelocityMeasurementWindow( + config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, kTimeoutMs + ); + + talon.configOpenloopRamp(config.OPEN_LOOP_RAMP_RATE, kTimeoutMs); + talon.configClosedloopRamp(config.CLOSED_LOOP_RAMP_RATE, kTimeoutMs); + + talon.configVoltageCompSaturation(0.0, kTimeoutMs); + talon.configVoltageMeasurementFilter(32, kTimeoutMs); + talon.enableVoltageCompensation(false); + + talon.configSupplyCurrentLimit( + new SupplyCurrentLimitConfiguration(config.ENABLE_CURRENT_LIMIT, 20, 60, .2), kTimeoutMs + ); + + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_1_General, config.GENERAL_STATUS_FRAME_RATE_MS, kTimeoutMs + ); + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_2_Feedback0, config.FEEDBACK_STATUS_FRAME_RATE_MS, kTimeoutMs + ); + + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_3_Quadrature, + config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, + kTimeoutMs + ); + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_4_AinTempVbat, + config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, + kTimeoutMs + ); + talon.setStatusFramePeriod( + StatusFrameEnhanced.Status_8_PulseWidth, config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, kTimeoutMs + ); + + talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS); + + return talon; + } +} diff --git a/src/main/java/com/team254/lib/drivers/TalonUtil.java b/src/main/java/com/team254/lib/drivers/TalonUtil.java new file mode 100644 index 0000000..9935b2a --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/TalonUtil.java @@ -0,0 +1,30 @@ +package com.team254.lib.drivers; + +import com.ctre.phoenix.ErrorCode; +import edu.wpi.first.wpilibj.DriverStation; + +public class TalonUtil { + /** + * checks the specified error code for issues + * + * @param errorCode error code + * @param message message to print if error happens + */ + public static void checkError(ErrorCode errorCode, String message) { + if (errorCode != ErrorCode.OK) { + DriverStation.reportError(message + " " + errorCode, false); + } + } + + /** + * checks the specified error code and throws an exception if there are any issues + * + * @param errorCode error code + * @param message message to print if error happens + */ + public static void checkErrorWithThrow(ErrorCode errorCode, String message) { + if (errorCode != ErrorCode.OK) { + throw new RuntimeException(message + " " + errorCode); + } + } +} diff --git a/src/main/java/com/team254/lib/drivers/WenglorColorSensor.java b/src/main/java/com/team254/lib/drivers/WenglorColorSensor.java new file mode 100644 index 0000000..3f83756 --- /dev/null +++ b/src/main/java/com/team254/lib/drivers/WenglorColorSensor.java @@ -0,0 +1,56 @@ +package com.team254.lib.drivers; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.SynchronousInterrupt; +import edu.wpi.first.wpilibj.Timer; + +public class WenglorColorSensor { + private final DigitalInput m_inputA; + private final DigitalInput m_inputB; + private final SynchronousInterrupt m_channelA; + private final SynchronousInterrupt m_channelB; + private double m_lastCheckedAt; + + public enum Channel { CHANNEL_A, CHANNEL_B, BOTH, NONE } + + public WenglorColorSensor(int channelA, int channelB) { + m_inputA = new DigitalInput(channelA); + m_inputB = new DigitalInput(channelB); + m_channelA = new SynchronousInterrupt(m_inputA); + m_channelB = new SynchronousInterrupt(m_inputB); + m_channelA.setInterruptEdges(false, true); + m_channelB.setInterruptEdges(false, true); + m_lastCheckedAt = Timer.getFPGATimestamp(); + } + + public synchronized double getChannelATriggerTimestamp() { + return m_channelA.getFallingTimestamp(); + } + + public synchronized double getChannelBTriggerTimestamp() { + return m_channelB.getFallingTimestamp(); + } + + public synchronized Channel getLastTrigger() { + boolean channelATriggered = false; + boolean channelBTriggered = false; + if (getChannelATriggerTimestamp() > m_lastCheckedAt || !m_inputA.get()) { + channelATriggered = true; + } + if (getChannelBTriggerTimestamp() > m_lastCheckedAt || !m_inputB.get()) { + channelBTriggered = true; + } + + m_lastCheckedAt = Timer.getFPGATimestamp(); + + if (channelATriggered && channelBTriggered) { + return Channel.BOTH; + } else if (channelATriggered) { + return Channel.CHANNEL_A; + } else if (channelBTriggered) { + return Channel.CHANNEL_B; + } else { + return Channel.NONE; + } + } +} diff --git a/src/main/java/com/team254/lib/geometry/ICurvature.java b/src/main/java/com/team254/lib/geometry/ICurvature.java new file mode 100644 index 0000000..444ba39 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/ICurvature.java @@ -0,0 +1,7 @@ +package com.team254.lib.geometry; + +public interface ICurvature extends State { + double getCurvature(); + + double getDCurvatureDs(); +} diff --git a/src/main/java/com/team254/lib/geometry/IPose2d.java b/src/main/java/com/team254/lib/geometry/IPose2d.java new file mode 100644 index 0000000..d7b8031 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/IPose2d.java @@ -0,0 +1,9 @@ +package com.team254.lib.geometry; + +public interface IPose2d extends IRotation2d, ITranslation2d { + Pose2d getPose(); + + S transformBy(Pose2d transform); + + S mirror(); +} diff --git a/src/main/java/com/team254/lib/geometry/IRotation2d.java b/src/main/java/com/team254/lib/geometry/IRotation2d.java new file mode 100644 index 0000000..9c08fb9 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/IRotation2d.java @@ -0,0 +1,9 @@ +package com.team254.lib.geometry; + +public interface IRotation2d extends State { + Rotation2d254 getRotation(); + + S rotateBy(Rotation2d254 other); + + S mirror(); +} diff --git a/src/main/java/com/team254/lib/geometry/ITranslation2d.java b/src/main/java/com/team254/lib/geometry/ITranslation2d.java new file mode 100644 index 0000000..4cc5158 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/ITranslation2d.java @@ -0,0 +1,5 @@ +package com.team254.lib.geometry; + +public interface ITranslation2d extends State { + Translation2d getTranslation(); +} diff --git a/src/main/java/com/team254/lib/geometry/Pose2d.java b/src/main/java/com/team254/lib/geometry/Pose2d.java new file mode 100644 index 0000000..a20a3e2 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/Pose2d.java @@ -0,0 +1,263 @@ +package com.team254.lib.geometry; + +import com.team254.lib.util.Util; + +/** + * Represents a 2d pose (rigid transform) containing translational and rotational elements. + *

+ * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) + */ +public class Pose2d implements IPose2d { + protected static final Pose2d kIdentity = new Pose2d(); + + public static Pose2d identity() { + return kIdentity; + } + + private final static double kEps = 1E-9; + + protected final Translation2d translation_; + protected final Rotation2d254 rotation_; + + public Pose2d() { + translation_ = new Translation2d(); + rotation_ = new Rotation2d254(); + } + + public Pose2d(double x, double y, final Rotation2d254 rotation) { + translation_ = new Translation2d(x, y); + rotation_ = rotation; + } + + public Pose2d(final Translation2d translation, final Rotation2d254 rotation) { + translation_ = translation; + rotation_ = rotation; + } + + public Pose2d(final Pose2d other) { + translation_ = new Translation2d(other.translation_); + rotation_ = new Rotation2d254(other.rotation_); + } + + public Pose2d(final Pose2dWithCurvature other) { + translation_ = new Translation2d(other.getTranslation()); + rotation_ = new Rotation2d254(other.getRotation()); + } + + public Pose2d(final edu.wpi.first.math.geometry.Pose2d other) { + translation_ = new Translation2d(other.getTranslation()); + rotation_ = new Rotation2d254(other.getRotation()); + } + + public static Pose2d fromTranslation(final Translation2d translation) { + return new Pose2d(translation, new Rotation2d254()); + } + + public static Pose2d fromRotation(final Rotation2d254 rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Obtain a new Pose2d from a (constant curvature) velocity. See: + * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp + */ + public static Pose2d exp(final Twist2d delta) { + double sin_theta = Math.sin(delta.dtheta); + double cos_theta = Math.cos(delta.dtheta); + double s, c; + if (Math.abs(delta.dtheta) < kEps) { + s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta; + c = .5 * delta.dtheta; + } else { + s = sin_theta / delta.dtheta; + c = (1.0 - cos_theta) / delta.dtheta; + } + return new Pose2d( + new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s), + new Rotation2d254(cos_theta, sin_theta, false) + ); + } + + /** + * Logical inverse of the above. + */ + public static Twist2d log(final Pose2d transform) { + final double dtheta = transform.getRotation().getRadians(); + final double half_dtheta = 0.5 * dtheta; + final double cos_minus_one = transform.getRotation().cos() - 1.0; + double halftheta_by_tan_of_halfdtheta; + if (Math.abs(cos_minus_one) < kEps) { + halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halftheta_by_tan_of_halfdtheta = + -(half_dtheta * transform.getRotation().sin()) / cos_minus_one; + } + final Translation2d translation_part = transform.getTranslation().rotateBy( + new Rotation2d254(halftheta_by_tan_of_halfdtheta, -half_dtheta, false) + ); + return new Twist2d(translation_part.x(), translation_part.y(), dtheta); + } + + @Override + public Translation2d getTranslation() { + return translation_; + } + + @Override + public Rotation2d254 getRotation() { + return rotation_; + } + + @Override + public Pose2d rotateBy(Rotation2d254 other) { + return this.transformBy(new Pose2d(Translation2d.identity(), other)); + } + + @Override + public Pose2d add(Pose2d other) { + return this.transformBy(other); + } + + /** + * Transforming this RigidTransform2d means first translating by other.translation and then + * rotating by other.rotation + * + * @param other The other transform. + * @return This transform * other + */ + @Override + public Pose2d transformBy(final Pose2d other) { + return new Pose2d( + translation_.translateBy(other.translation_.rotateBy(rotation_)), + rotation_.rotateBy(other.rotation_) + ); + } + + public Pose2d transformBy(Transform2d other) { + return new Pose2d( + translation_.plus(other.getTranslation().rotateBy(rotation_)), + rotation_.rotateBy(other.getRotation()) + ); + } + + public Transform2d minus(Pose2d other) { + final var pose = this.relativeTo(other); + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public Pose2d relativeTo(Pose2d other) { + var transform = new Transform2d(other, this); + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * The inverse of this transform "undoes" the effect of translating by this transform. + * + * @return The opposite of this transform. + */ + public Pose2d inverse() { + Rotation2d254 rotation_inverted = rotation_.inverse(); + return new Pose2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted); + } + + public Pose2d normal() { + return new Pose2d(translation_, rotation_.normal()); + } + + /** + * Finds the point where the heading of this pose intersects the heading of another. Returns + * (+INF, +INF) if parallel. + */ + public Translation2d intersection(final Pose2d other) { + final Rotation2d254 other_rotation = other.getRotation(); + if (rotation_.isParallel(other_rotation)) { + // Lines are parallel. + return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); + } + if (Math.abs(rotation_.cos()) < Math.abs(other_rotation.cos())) { + return intersectionInternal(this, other); + } else { + return intersectionInternal(other, this); + } + } + + /** + * Return true if this pose is (nearly) colinear with the another. + */ + public boolean isColinear(final Pose2d other) { + if (!getRotation().isParallel(other.getRotation())) + return false; + final Twist2d twist = log(inverse().transformBy(other)); + return (Util.epsilonEquals(twist.dy, 0.0) && Util.epsilonEquals(twist.dtheta, 0.0)); + } + + public boolean epsilonEquals(final Pose2d other, double epsilon) { + return getTranslation().epsilonEquals(other.getTranslation(), epsilon) + && getRotation().isParallel(other.getRotation()); + } + + private static Translation2d intersectionInternal(final Pose2d a, final Pose2d b) { + final Rotation2d254 a_r = a.getRotation(); + final Rotation2d254 b_r = b.getRotation(); + final Translation2d a_t = a.getTranslation(); + final Translation2d b_t = b.getTranslation(); + + final double tan_b = b_r.tan(); + final double t = + ((a_t.x() - b_t.x()) * tan_b + b_t.y() - a_t.y()) / (a_r.sin() - a_r.cos() * tan_b); + if (Double.isNaN(t)) { + return new Translation2d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); + } + return a_t.translateBy(a_r.toTranslation().scale(t)); + } + + /** + * Do twist interpolation of this pose assuming constant curvature. + */ + @Override + public Pose2d interpolate(final Pose2d other, double x) { + if (x <= 0) { + return new Pose2d(this); + } else if (x >= 1) { + return new Pose2d(other); + } + final Twist2d twist = Pose2d.log(inverse().transformBy(other)); + return transformBy(Pose2d.exp(twist.scaled(x))); + } + + @Override + public String toString() { + return "T:" + translation_.toString() + ", R:" + rotation_.toString(); + } + + @Override + public String toCSV() { + return translation_.toCSV() + "," + rotation_.toCSV(); + } + + @Override + public double distance(final Pose2d other) { + return Pose2d.log(inverse().transformBy(other)).norm(); + } + + @Override + public boolean equals(final Object other) { + if (!(other instanceof Pose2d)) { + return false; + } + + return epsilonEquals((Pose2d) other, Util.kEpsilon); + } + + @Override + public Pose2d getPose() { + return this; + } + + @Override + public Pose2d mirror() { + return new Pose2d( + new Translation2d(getTranslation().x(), -getTranslation().y()), getRotation().inverse() + ); + } +} diff --git a/src/main/java/com/team254/lib/geometry/Pose2dWithCurvature.java b/src/main/java/com/team254/lib/geometry/Pose2dWithCurvature.java new file mode 100644 index 0000000..9a0e7ea --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/Pose2dWithCurvature.java @@ -0,0 +1,143 @@ +package com.team254.lib.geometry; + +import com.team254.lib.util.Util; +import java.text.DecimalFormat; + +public class Pose2dWithCurvature + implements IPose2d, ICurvature { + protected static final Pose2dWithCurvature kIdentity = new Pose2dWithCurvature(); + + public static Pose2dWithCurvature identity() { + return kIdentity; + } + + protected final Pose2d pose_; + protected final double curvature_; + protected final double dcurvature_ds_; + + public Pose2dWithCurvature() { + pose_ = new Pose2d(); + curvature_ = 0.0; + dcurvature_ds_ = 0.0; + } + + public Pose2dWithCurvature(final Pose2d pose, double curvature) { + pose_ = pose; + curvature_ = curvature; + dcurvature_ds_ = 0.0; + } + + public Pose2dWithCurvature(final Pose2d pose, double curvature, double dcurvature_ds) { + pose_ = pose; + curvature_ = curvature; + dcurvature_ds_ = dcurvature_ds; + } + + public Pose2dWithCurvature( + final Translation2d translation, final Rotation2d254 rotation, double curvature + ) { + pose_ = new Pose2d(translation, rotation); + curvature_ = curvature; + dcurvature_ds_ = 0.0; + } + + public Pose2dWithCurvature( + final Translation2d translation, + final Rotation2d254 rotation, + double curvature, + double dcurvature_ds + ) { + pose_ = new Pose2d(translation, rotation); + curvature_ = curvature; + dcurvature_ds_ = dcurvature_ds; + } + + @Override + public final Pose2d getPose() { + return pose_; + } + + @Override + public Pose2dWithCurvature transformBy(Pose2d transform) { + return new Pose2dWithCurvature( + getPose().transformBy(transform), getCurvature(), getDCurvatureDs() + ); + } + + @Override + public Pose2dWithCurvature mirror() { + return new Pose2dWithCurvature( + getPose().mirror().getPose(), -getCurvature(), -getDCurvatureDs() + ); + } + + @Override + public double getCurvature() { + return curvature_; + } + + @Override + public double getDCurvatureDs() { + return dcurvature_ds_; + } + + @Override + public final Translation2d getTranslation() { + return getPose().getTranslation(); + } + + @Override + public final Rotation2d254 getRotation() { + return getPose().getRotation(); + } + + @Override + public Pose2dWithCurvature interpolate(final Pose2dWithCurvature other, double x) { + return new Pose2dWithCurvature( + getPose().interpolate(other.getPose(), x), + Util.interpolate(getCurvature(), other.getCurvature(), x), + Util.interpolate(getDCurvatureDs(), other.getDCurvatureDs(), x) + ); + } + + @Override + public double distance(final Pose2dWithCurvature other) { + return getPose().distance(other.getPose()); + } + + @Override + public boolean equals(final Object other) { + if (!(other instanceof Pose2dWithCurvature)) { + return false; + } + + Pose2dWithCurvature p2dwc = (Pose2dWithCurvature) other; + return getPose().equals(p2dwc.getPose()) + && Util.epsilonEquals(getCurvature(), p2dwc.getCurvature()) + && Util.epsilonEquals(getDCurvatureDs(), p2dwc.getDCurvatureDs()); + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return getPose().toString() + ", curvature: " + fmt.format(getCurvature()) + + ", dcurvature_ds: " + fmt.format(getDCurvatureDs()); + } + + @Override + public String toCSV() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return getPose().toCSV() + "," + fmt.format(getCurvature()) + "," + + fmt.format(getDCurvatureDs()); + } + + @Override + public Pose2dWithCurvature rotateBy(Rotation2d254 other) { + return new Pose2dWithCurvature(getPose().rotateBy(other), getCurvature(), getDCurvatureDs()); + } + + @Override + public Pose2dWithCurvature add(Pose2dWithCurvature other) { + return this.transformBy(other.getPose()); // todo make work + } +} diff --git a/src/main/java/com/team254/lib/geometry/Rotation2d254.java b/src/main/java/com/team254/lib/geometry/Rotation2d254.java new file mode 100644 index 0000000..bba0348 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/Rotation2d254.java @@ -0,0 +1,301 @@ +package com.team254.lib.geometry; + +import static com.team254.lib.util.Util.kEpsilon; + +import com.team254.lib.util.Util; +import java.text.DecimalFormat; + +/** + * A rotation in a 2d coordinate frame represented a point on the unit circle + * (cosine and sine). + *

+ * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) + */ +public class Rotation2d254 implements IRotation2d { + public static final Rotation2d254 kIdentity = new Rotation2d254(); + public static final Rotation2d254 kPi = new Rotation2d254(Math.PI, false); + public static final Rotation2d254 kHalfPi = new Rotation2d254(Math.PI / 2.0, false); + + public static Rotation2d254 identity() { + return kIdentity; + } + + protected double cos_angle_ = Double.NaN; + protected double sin_angle_ = Double.NaN; + protected double radians_ = Double.NaN; + + protected Rotation2d254(double x, double y, double radians) { + cos_angle_ = x; + sin_angle_ = y; + radians_ = radians; + } + + public Rotation2d254() { + this(1.0, 0.0, 0.0); + } + + public Rotation2d254(double radians, boolean normalize) { + if (normalize) { + radians = WrapRadians(radians); + } + radians_ = radians; + } + + public Rotation2d254(double x, double y, boolean normalize) { + if (normalize) { + // From trig, we know that sin^2 + cos^2 == 1, but as we do math on this object + // we might accumulate rounding errors. + // Normalizing forces us to re-scale the sin and cos to reset rounding errors. + double magnitude = Math.hypot(x, y); + if (magnitude > kEpsilon) { + sin_angle_ = y / magnitude; + cos_angle_ = x / magnitude; + } else { + sin_angle_ = 0.0; + cos_angle_ = 1.0; + } + } else { + cos_angle_ = x; + sin_angle_ = y; + } + } + + public Rotation2d254(final Rotation2d254 other) { + cos_angle_ = other.cos_angle_; + sin_angle_ = other.sin_angle_; + radians_ = other.radians_; + } + + public Rotation2d254(final edu.wpi.first.math.geometry.Rotation2d other) { + cos_angle_ = other.getCos(); + sin_angle_ = other.getSin(); + radians_ = other.getRadians(); + } + + public Rotation2d254(final Translation2d direction, boolean normalize) { + this(direction.x(), direction.y(), normalize); + } + + public static Rotation2d254 fromRadians(double angle_radians) { + return new Rotation2d254(angle_radians, true); + } + + public static Rotation2d254 fromDegrees(double angle_degrees) { + return fromRadians(Math.toRadians(angle_degrees)); + } + + public double cos() { + ensureTrigComputed(); + return cos_angle_; + } + + public double sin() { + ensureTrigComputed(); + return sin_angle_; + } + + public double tan() { + ensureTrigComputed(); + if (Math.abs(cos_angle_) < kEpsilon) { + if (sin_angle_ >= 0.0) { + return Double.POSITIVE_INFINITY; + } else { + return Double.NEGATIVE_INFINITY; + } + } + return sin_angle_ / cos_angle_; + } + + public double getRadians() { + ensureRadiansComputed(); + return radians_; + } + + /** + * Based on Team 1323's method of the same name. + * + * @return Rotation2d representing the angle of the nearest axis to the angle in standard position + */ + public Rotation2d254 nearestPole() { + double pole_sin = 0.0; + double pole_cos = 0.0; + if (Math.abs(cos_angle_) > Math.abs(sin_angle_)) { + pole_cos = Math.signum(cos_angle_); + pole_sin = 0.0; + } else { + pole_cos = 0.0; + pole_sin = Math.signum(sin_angle_); + } + return new Rotation2d254(pole_cos, pole_sin, false); + } + + public double getDegrees() { + return Math.toDegrees(getRadians()); + } + + public Rotation2d254 unaryMinus() { + return new Rotation2d254(-radians_, true); + } + + public Rotation2d254 minus(Rotation2d254 other) { + return rotateBy(other.unaryMinus()); + } + + public Rotation2d254 times(double scalar) { + return new Rotation2d254(radians_ * scalar, true); + } + + /** + * We can rotate this Rotation2d by adding together the effects of it and + * another rotation. + * + * @param other The other rotation. See: + * https://en.wikipedia.org/wiki/Rotation_matrix + * @return This rotation rotated by other. + */ + public Rotation2d254 rotateBy(final Rotation2d254 other) { + if (hasTrig() && other.hasTrig()) { + return new Rotation2d254( + cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_, + cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, + true + ); + } else { + return fromRadians(getRadians() + other.getRadians()); + } + } + + @Override + public Rotation2d254 mirror() { + return Rotation2d254.fromRadians(-radians_); + } + + public Rotation2d254 normal() { + if (hasTrig()) { + return new Rotation2d254(-sin_angle_, cos_angle_, false); + } else { + return fromRadians(getRadians() - Math.PI / 2.0); + } + } + + /** + * The inverse of a Rotation2d "undoes" the effect of this rotation. + * + * @return The inverse of this rotation. + */ + public Rotation2d254 inverse() { + if (hasTrig()) { + return new Rotation2d254(cos_angle_, -sin_angle_, false); + } else { + return fromRadians(-getRadians()); + } + } + + /** + * Obtain a Rotation2d that points in the opposite direction from this rotation. + * @return This rotation rotated by 180 degrees. + */ + public Rotation2d254 flip() { + if (hasTrig()) { + return new Rotation2d254(-cos_angle_, -sin_angle_, false); + } else { + return fromRadians(getRadians() + Math.PI); + } + } + + public boolean isParallel(final Rotation2d254 other) { + if (hasRadians() && other.hasRadians()) { + return Util.epsilonEquals(radians_, other.radians_) + || Util.epsilonEquals(radians_, WrapRadians(other.radians_ + Math.PI)); + } else if (hasTrig() && other.hasTrig()) { + return Util.epsilonEquals(sin_angle_, other.sin_angle_) + && Util.epsilonEquals(cos_angle_, other.cos_angle_); + } else { + // Use public, checked version. + return Util.epsilonEquals(getRadians(), other.getRadians()) + || Util.epsilonEquals(radians_, WrapRadians(other.radians_ + Math.PI)); + } + } + + public Translation2d toTranslation() { + ensureTrigComputed(); + return new Translation2d(cos_angle_, sin_angle_); + } + + protected double WrapRadians(double radians) { + final double k2Pi = 2.0 * Math.PI; + radians = radians % k2Pi; + radians = (radians + k2Pi) % k2Pi; + if (radians > Math.PI) + radians -= k2Pi; + return radians; + } + + private synchronized boolean hasTrig() { + return !Double.isNaN(sin_angle_) && !Double.isNaN(cos_angle_); + } + + private synchronized boolean hasRadians() { + return !Double.isNaN(radians_); + } + + private synchronized void ensureTrigComputed() { + if (!hasTrig()) { + assert (hasRadians()); + sin_angle_ = Math.sin(radians_); + cos_angle_ = Math.cos(radians_); + } + } + + private synchronized void ensureRadiansComputed() { + if (!hasRadians()) { + assert (hasTrig()); + radians_ = Math.atan2(sin_angle_, cos_angle_); + } + } + + @Override + public Rotation2d254 interpolate(final Rotation2d254 other, double x) { + if (x <= 0.0) { + return new Rotation2d254(this); + } else if (x >= 1.0) { + return new Rotation2d254(other); + } + double angle_diff = inverse().rotateBy(other).getRadians(); + return this.rotateBy(Rotation2d254.fromRadians(angle_diff * x)); + } + + @Override + public String toString() { + return "(" + new DecimalFormat("#0.000").format(getDegrees()) + " deg)"; + } + + @Override + public String toCSV() { + return new DecimalFormat("#0.000").format(getDegrees()); + } + + @Override + public double distance(final Rotation2d254 other) { + return inverse().rotateBy(other).getRadians(); + } + + @Override + public Rotation2d254 add(Rotation2d254 other) { + return this.rotateBy(other); + } + + @Override + public boolean equals(final Object other) { + if (!(other instanceof Rotation2d254)) { + return false; + } + + return distance((Rotation2d254) other) < Util.kEpsilon; + } + + @Override + public Rotation2d254 getRotation() { + return this; + } +} diff --git a/src/main/java/com/team254/lib/geometry/State.java b/src/main/java/com/team254/lib/geometry/State.java new file mode 100644 index 0000000..6c07473 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/State.java @@ -0,0 +1,16 @@ +package com.team254.lib.geometry; + +import com.team254.lib.util.CSVWritable; +import com.team254.lib.util.Interpolable; + +public interface State extends Interpolable, CSVWritable { + double distance(final S other); + + S add(S other); + + boolean equals(final Object other); + + String toString(); + + String toCSV(); +} diff --git a/src/main/java/com/team254/lib/geometry/Transform2d.java b/src/main/java/com/team254/lib/geometry/Transform2d.java new file mode 100644 index 0000000..9d8c572 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/Transform2d.java @@ -0,0 +1,143 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.team254.lib.geometry; + +import java.util.Objects; + +/** Represents a transformation for a Pose2d. */ +public class Transform2d { + private final Translation2d m_translation; + private final Rotation2d254 m_rotation; + + /** + * Constructs the transform that maps the initial pose to the final pose. + * + * @param initial The initial pose for the transformation. + * @param last The final pose for the transformation. + */ + public Transform2d(Pose2d initial, Pose2d last) { + // We are rotating the difference between the translations + // using a clockwise rotation matrix. This transforms the global + // delta into a local delta (relative to the initial pose). + m_translation = last.getTranslation() + .minus(initial.getTranslation()) + .rotateBy(initial.getRotation().unaryMinus()); + + m_rotation = last.getRotation().minus(initial.getRotation()); + } + + /** + * Constructs a transform with the given translation and rotation components. + * + * @param translation Translational component of the transform. + * @param rotation Rotational component of the transform. + */ + public Transform2d(Translation2d translation, Rotation2d254 rotation) { + m_translation = translation; + m_rotation = rotation; + } + + /** Constructs the identity transform -- maps an initial pose to itself. */ + public Transform2d() { + m_translation = new Translation2d(); + m_rotation = new Rotation2d254(); + } + + /** + * Scales the transform by the scalar. + * + * @param scalar The scalar. + * @return The scaled Transform2d. + */ + public Transform2d times(double scalar) { + return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar)); + } + + /** + * Composes two transformations. + * + * @param other The transform to compose with this one. + * @return The composition of the two transformations. + */ + public Transform2d plus(Transform2d other) { + return new Transform2d(new Pose2d(), new Pose2d().transformBy(this).transformBy(other)); + } + + /** + * Returns the translation component of the transformation. + * + * @return The translational component of the transform. + */ + public Translation2d getTranslation() { + return m_translation; + } + + /** + * Returns the X component of the transformation's translation. + * + * @return The x component of the transformation's translation. + */ + public double getX() { + return m_translation.x(); + } + + /** + * Returns the Y component of the transformation's translation. + * + * @return The y component of the transformation's translation. + */ + public double getY() { + return m_translation.y(); + } + + /** + * Returns the rotational component of the transformation. + * + * @return Reference to the rotational component of the transform. + */ + public Rotation2d254 getRotation() { + return m_rotation; + } + + /** + * Invert the transformation. This is useful for undoing a transformation. + * + * @return The inverted transformation. + */ + public Transform2d inverse() { + // We are rotating the difference between the translations + // using a clockwise rotation matrix. This transforms the global + // delta into a local delta (relative to the initial pose). + return new Transform2d( + getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()), + getRotation().unaryMinus() + ); + } + + @Override + public String toString() { + return String.format("Transform2d(%s, %s)", m_translation, m_rotation); + } + + /** + * Checks equality between this Transform2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Transform2d) { + return ((Transform2d) obj).m_translation.equals(m_translation) + && ((Transform2d) obj).m_rotation.equals(m_rotation); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_translation, m_rotation); + } +} diff --git a/src/main/java/com/team254/lib/geometry/Translation2d.java b/src/main/java/com/team254/lib/geometry/Translation2d.java new file mode 100644 index 0000000..15cf8f0 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/Translation2d.java @@ -0,0 +1,190 @@ +package com.team254.lib.geometry; + +import com.team254.lib.util.Util; +import java.text.DecimalFormat; + +/** + * A translation in a 2d coordinate frame. Translations are simply shifts in an (x, y) plane. + */ +public class Translation2d implements ITranslation2d { + protected static final Translation2d kIdentity = new Translation2d(); + + public static Translation2d identity() { + return kIdentity; + } + + protected final double x_; + protected final double y_; + + public Translation2d() { + x_ = 0; + y_ = 0; + } + + public Translation2d(double x, double y) { + x_ = x; + y_ = y; + } + + public Translation2d(final Translation2d other) { + x_ = other.x_; + y_ = other.y_; + } + + public Translation2d(final Translation2d start, final Translation2d end) { + x_ = end.x_ - start.x_; + y_ = end.y_ - start.y_; + } + + public Translation2d(final edu.wpi.first.math.geometry.Translation2d other) { + x_ = other.getX(); + y_ = other.getY(); + } + + /** + * The "norm" of a transform is the Euclidean distance in x and y. + * + * @return sqrt(x ^ 2 + y ^ 2) + */ + public double norm() { + return Math.hypot(x_, y_); + } + + public double norm2() { + return x_ * x_ + y_ * y_; + } + + public double x() { + return x_; + } + + public double y() { + return y_; + } + + /** + * We can compose Translation2d's by adding together the x and y shifts. + * + * @param other The other translation to add. + * @return The combined effect of translating by this object and the other. + */ + public Translation2d translateBy(final Translation2d other) { + return new Translation2d(x_ + other.x_, y_ + other.y_); + } + + public Translation2d plus(Translation2d other) { + return new Translation2d(x_ + other.x(), y_ + other.y()); + } + + public Translation2d minus(Translation2d other) { + return new Translation2d(x_ - other.x(), y_ - other.y()); + } + + public Translation2d unaryMinus() { + return new Translation2d(-x_, -y_); + } + + public Translation2d times(double scalar) { + return new Translation2d(x_ * scalar, y_ * scalar); + } + + /** + * We can also rotate Translation2d's. See: https://en.wikipedia.org/wiki/Rotation_matrix + * + * @param rotation The rotation to apply. + * @return This translation rotated by rotation. + */ + public Translation2d rotateBy(final Rotation2d254 rotation) { + return new Translation2d( + x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos() + ); + } + + public Rotation2d254 direction() { + return new Rotation2d254(x_, y_, true); + } + + /** + * The inverse simply means a Translation2d that "undoes" this object. + * + * @return Translation by -x and -y. + */ + public Translation2d inverse() { + return new Translation2d(-x_, -y_); + } + + @Override + public Translation2d interpolate(final Translation2d other, double x) { + if (x <= 0) { + return new Translation2d(this); + } else if (x >= 1) { + return new Translation2d(other); + } + return extrapolate(other, x); + } + + public Translation2d extrapolate(final Translation2d other, double x) { + return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_); + } + + public Translation2d scale(double s) { + return new Translation2d(x_ * s, y_ * s); + } + + public boolean epsilonEquals(final Translation2d other, double epsilon) { + return Util.epsilonEquals(x(), other.x(), epsilon) + && Util.epsilonEquals(y(), other.y(), epsilon); + } + + @Override + public String toString() { + final DecimalFormat format = new DecimalFormat("#0.000"); + return "(" + format.format(x_) + "," + format.format(y_) + ")"; + } + + @Override + public String toCSV() { + final DecimalFormat format = new DecimalFormat("#0.000"); + return format.format(x_) + "," + format.format(y_); + } + + public static double dot(final Translation2d a, final Translation2d b) { + return a.x_ * b.x_ + a.y_ * b.y_; + } + + public static Rotation2d254 getAngle(final Translation2d a, final Translation2d b) { + double cos_angle = dot(a, b) / (a.norm() * b.norm()); + if (Double.isNaN(cos_angle)) { + return new Rotation2d254(); + } + return Rotation2d254.fromRadians(Math.acos(Util.limit(cos_angle, 1.0))); + } + + public static double cross(final Translation2d a, final Translation2d b) { + return a.x_ * b.y_ - a.y_ * b.x_; + } + + @Override + public double distance(final Translation2d other) { + return inverse().translateBy(other).norm(); + } + + @Override + public Translation2d add(Translation2d other) { + return this.translateBy(other); + } + + @Override + public boolean equals(final Object other) { + if (!(other instanceof Translation2d)) { + return false; + } + + return distance((Translation2d) other) < Util.kEpsilon; + } + + @Override + public Translation2d getTranslation() { + return this; + } +} diff --git a/src/main/java/com/team254/lib/geometry/Twist2d.java b/src/main/java/com/team254/lib/geometry/Twist2d.java new file mode 100644 index 0000000..381a657 --- /dev/null +++ b/src/main/java/com/team254/lib/geometry/Twist2d.java @@ -0,0 +1,56 @@ +package com.team254.lib.geometry; + +import com.team254.lib.util.Util; +import java.text.DecimalFormat; + +/** + * A movement along an arc at constant curvature and velocity. We can use ideas from "differential + * calculus" to create new RigidTransform2d's from a Twist2d and visa versa.

A Twist can be used + * to represent a difference between two poses, a velocity, an acceleration, etc. + */ +public class Twist2d { + protected static final Twist2d kIdentity = new Twist2d(0.0, 0.0, 0.0); + + public static Twist2d identity() { + return kIdentity; + } + + public final double dx; + public final double dy; + public final double dtheta; // Radians! + + public Twist2d(double dx, double dy, double dtheta) { + this.dx = dx; + this.dy = dy; + this.dtheta = dtheta; + } + + public Twist2d scaled(double scale) { + return new Twist2d(dx * scale, dy * scale, dtheta * scale); + } + + public double norm() { + // Common case of dy == 0 + if (dy == 0.0) + return Math.abs(dx); + return Math.hypot(dx, dy); + } + + public double curvature() { + if (Math.abs(dtheta) < Util.kEpsilon && norm() < Util.kEpsilon) + return 0.0; + return dtheta / norm(); + } + + public boolean epsilonEquals(final Twist2d other, double epsilon) { + return Util.epsilonEquals(dx, other.dx, epsilon) && Util.epsilonEquals(dy, other.dy, epsilon) + && Util.epsilonEquals(dtheta, other.dtheta, epsilon); + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return "(" + fmt.format(dx) + "," + fmt.format(dy) + "," + fmt.format(Math.toDegrees(dtheta)) + + " deg)"; + } +} diff --git a/src/main/java/com/team254/lib/loops/ILooper.java b/src/main/java/com/team254/lib/loops/ILooper.java new file mode 100644 index 0000000..c23f94c --- /dev/null +++ b/src/main/java/com/team254/lib/loops/ILooper.java @@ -0,0 +1,5 @@ +package com.team254.lib.loops; + +public interface ILooper { + void register(Loop loop); +} diff --git a/src/main/java/com/team254/lib/loops/Loop.java b/src/main/java/com/team254/lib/loops/Loop.java new file mode 100644 index 0000000..8704e01 --- /dev/null +++ b/src/main/java/com/team254/lib/loops/Loop.java @@ -0,0 +1,13 @@ +package com.team254.lib.loops; + +/** + * Interface for loops, which are routine that run periodically in the robot code (such as periodic + * gyroscope calibration, etc.) + */ +public interface Loop { + void onStart(double timestamp); + + void onLoop(double timestamp); + + void onStop(double timestamp); +} diff --git a/src/main/java/com/team254/lib/loops/Looper.java b/src/main/java/com/team254/lib/loops/Looper.java new file mode 100644 index 0000000..494963a --- /dev/null +++ b/src/main/java/com/team254/lib/loops/Looper.java @@ -0,0 +1,92 @@ +package com.team254.lib.loops; + +import com.team254.lib.util.CrashTrackingRunnable; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.List; + +/** + * This code runs all of the robot's loops. Loop objects are stored in a List object. They are + * started when the robot powers up and stopped after the match. + */ +public class Looper implements ILooper { + public final double mPeriod; + + private boolean mRunning; + + private final Notifier mNotifier; + private final List mLoops; + private final Object mTaskRunningLock = new Object(); + private double mTimestamp = 0; + private double mDT = 0; + + private final CrashTrackingRunnable runnable_ = new CrashTrackingRunnable() { + @Override + public void runCrashTracked() { + synchronized (mTaskRunningLock) { + if (mRunning) { + double now = Timer.getFPGATimestamp(); + + for (Loop loop : mLoops) { + loop.onLoop(now); + } + + mDT = now - mTimestamp; + mTimestamp = now; + } + } + } + }; + + public Looper(double period) { + mNotifier = new Notifier(runnable_); + mPeriod = period; + mRunning = false; + mLoops = new ArrayList<>(); + } + + @Override + public synchronized void register(Loop loop) { + synchronized (mTaskRunningLock) { + mLoops.add(loop); + } + } + + public synchronized void start() { + if (!mRunning) { + System.out.println("Starting loops"); + + synchronized (mTaskRunningLock) { + mTimestamp = Timer.getFPGATimestamp(); + for (Loop loop : mLoops) { + loop.onStart(mTimestamp); + } + mRunning = true; + } + + mNotifier.startPeriodic(mPeriod); + } + } + + public synchronized void stop() { + if (mRunning) { + System.out.println("Stopping loops"); + mNotifier.stop(); + + synchronized (mTaskRunningLock) { + mRunning = false; + mTimestamp = Timer.getFPGATimestamp(); + for (Loop loop : mLoops) { + System.out.println("Stopping " + loop); + loop.onStop(mTimestamp); + } + } + } + } + + public void outputToSmartDashboard() { + SmartDashboard.putNumber("looper_dt", mDT); + } +} diff --git a/src/main/java/com/team254/lib/motion/IMotionProfileGoal.java b/src/main/java/com/team254/lib/motion/IMotionProfileGoal.java new file mode 100644 index 0000000..22c4d87 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/IMotionProfileGoal.java @@ -0,0 +1,52 @@ +package com.team254.lib.motion; + +/** + * A MotionProfileGoal defines a desired position and maximum velocity (at this position), along + * with the behavior that should be used to determine if we are at the goal and what to do if it is + * infeasible to reach the goal within the desired velocity bounds. + */ +public interface IMotionProfileGoal { + /** + * What should we do if we would + * reach the goal at a velocity greater than the maximum? This enum allows a user to specify a + * preference on behavior in this case.

Example use-cases of each:

OVERSHOOT - Generally + * used with a goal max_abs_vel of 0.0 to stop at the desired pos without violating any + * constraints. + *

+ * VIOLATE_MAX_ACCEL - If we absolutely do not want to pass the goal and are unwilling to violate + * the max_abs_vel (for example, there is an obstacle in front of us - slam the brakes harder than + * we'd like in order to avoid hitting it).

VIOLATE_MAX_ABS_VEL - If the max velocity is just + * a general guideline and not a hard performance limit, it's better to slightly exceed it to + * avoid skidding wheels. + */ + public enum CompletionBehavior { + // Overshoot the goal if necessary (at a velocity greater than max_abs_vel) and come back. + // Only valid if the goal velocity is 0.0 (otherwise VIOLATE_MAX_ACCEL will be used). + OVERSHOOT, + // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the max + // accel + // constraint. + VIOLATE_MAX_ACCEL, + // If we cannot slow down to the goal velocity before crossing the goal, allow exceeding the + // goal velocity. + VIOLATE_MAX_ABS_VEL + } + + public IMotionProfileGoal flipped(); + + public double pos(); + + public double max_vel(); + + public double min_vel(); + + public double pos_tolerance(); + + public double vel_tolerance(); + + public CompletionBehavior completion_behavior(); + + public boolean atGoalState(MotionState state); + + public boolean atGoalPos(double pos); +} diff --git a/src/main/java/com/team254/lib/motion/LinearTimeVaryingMotionProfileGoal.java b/src/main/java/com/team254/lib/motion/LinearTimeVaryingMotionProfileGoal.java new file mode 100644 index 0000000..0289aea --- /dev/null +++ b/src/main/java/com/team254/lib/motion/LinearTimeVaryingMotionProfileGoal.java @@ -0,0 +1,115 @@ +package com.team254.lib.motion; + +import com.team254.lib.util.Util; + +/** + * A LinearTimeVaryingMotionProfileGoal defines a goal with a constant velocity whose position is a + * function of an initial position, the velocity, and time. In other words, the goal state is + * constantly moving according to a time varying linear function. + */ +public class LinearTimeVaryingMotionProfileGoal implements IMotionProfileGoal { + protected double mT; + protected double mInitialGoalPosition; + protected double mGoalVel; + protected CompletionBehavior mCompletionBehavior = CompletionBehavior.OVERSHOOT; + protected double mPosTolerance = 1E-2; + protected double mVelTolerance = 1E-2; + + public LinearTimeVaryingMotionProfileGoal( + double t, double initial_goal_position, double goal_vel + ) { + this.mT = t; + this.mInitialGoalPosition = initial_goal_position; + this.mGoalVel = goal_vel; + } + + public LinearTimeVaryingMotionProfileGoal( + double t, + double initial_goal_position, + double goal_vel, + CompletionBehavior completion_behavior + ) { + this(t, initial_goal_position, goal_vel); + this.mCompletionBehavior = completion_behavior; + } + + public LinearTimeVaryingMotionProfileGoal( + double t, + double initial_goal_position, + double goal_vel, + CompletionBehavior completion_behavior, + double pos_tolerance, + double vel_tolerance + ) { + this(t, initial_goal_position, goal_vel, completion_behavior); + this.mPosTolerance = pos_tolerance; + this.mVelTolerance = vel_tolerance; + } + + public double t() { + return mT; + } + + @Override + public CompletionBehavior completion_behavior() { + return mCompletionBehavior; + } + + @Override + public LinearTimeVaryingMotionProfileGoal flipped() { + return new LinearTimeVaryingMotionProfileGoal( + mT, -mInitialGoalPosition, -mGoalVel, mCompletionBehavior, mPosTolerance, mVelTolerance + ); + } + + public double pos(double t) { + return mInitialGoalPosition + (t - mT) * mGoalVel; + } + + @Override + public boolean atGoalState(MotionState state) { + return Util.epsilonEquals(state.pos(), pos(state.t()), mPosTolerance) + && (Util.epsilonEquals(state.vel(), mGoalVel, mVelTolerance) + || mCompletionBehavior == CompletionBehavior.VIOLATE_MAX_ABS_VEL); + } + + /** + * Create a MotionProfileGoal that has a zero velocity goal state, for use with superposition to + * work with a moving reference frame. + * + * @return A MotionProfileGoal for use with motion profile and setpoint generation. + */ + public MotionProfileGoal toMotionProfileGoal(double t) { + return new MotionProfileGoal(pos(t), 0.0, mCompletionBehavior, mPosTolerance, mVelTolerance); + } + + @Override + public double pos() { + return mInitialGoalPosition; + } + + @Override + public double max_vel() { + return mGoalVel; + } + + @Override + public double min_vel() { + return mGoalVel; + } + + @Override + public double pos_tolerance() { + return mPosTolerance; + } + + @Override + public double vel_tolerance() { + return mVelTolerance; + } + + @Override + public boolean atGoalPos(double pos) { + return Util.epsilonEquals(pos, pos(mT), mPosTolerance); + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionProfile.java b/src/main/java/com/team254/lib/motion/MotionProfile.java new file mode 100644 index 0000000..84a1ec3 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionProfile.java @@ -0,0 +1,327 @@ +package com.team254.lib.motion; + +import static com.team254.lib.motion.MotionUtil.kEpsilon; +import static com.team254.lib.util.Util.epsilonEquals; + +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; +import java.util.Optional; + +/** + * A motion profile specifies a 1D time-parameterized trajectory. The trajectory is composed of + * successively coincident MotionSegments from which the desired state of motion at any given + * distance or time can be calculated. + */ +public class MotionProfile { + protected List mSegments; + + /** + * Create an empty MotionProfile. + */ + public MotionProfile() { + mSegments = new ArrayList<>(); + } + + /** + * Create a MotionProfile from an existing list of segments (note that validity is not checked). + * + * @param segments The new segments of the profile. + */ + public MotionProfile(List segments) { + mSegments = segments; + } + + /** + * Checks if the given MotionProfile is valid. This checks that: + *

+ * 1. All segments are valid. + *

+ * 2. Successive segments are C1 continuous in position and C0 continuous in velocity. + * + * @return True if the MotionProfile is valid. + */ + public boolean isValid() { + MotionSegment prev_segment = null; + for (MotionSegment s : mSegments) { + if (!s.isValid()) { + return false; + } + if (prev_segment != null && !s.start().coincident(prev_segment.end())) { + // Adjacent segments are not continuous. + System.err.println( + "Segments not continuous! End: " + prev_segment.end() + ", Start: " + s.start() + ); + return false; + } + prev_segment = s; + } + return true; + } + + /** + * Check if the profile is empty. + * + * @return True if there are no segments. + */ + public boolean isEmpty() { + return mSegments.isEmpty(); + } + + /** + * Get the interpolated MotionState at any given time. + * + * @param t The time to query. + * @return Empty if the time is outside the time bounds of the profile, or the resulting + * MotionState otherwise. + */ + public Optional stateByTime(double t) { + if (t < startTime() && t + kEpsilon >= startTime()) { + return Optional.of(startState()); + } + if (t > endTime() && t - kEpsilon <= endTime()) { + return Optional.of(endState()); + } + for (MotionSegment s : mSegments) { + if (s.containsTime(t)) { + return Optional.of(s.start().extrapolate(t)); + } + } + return Optional.empty(); + } + + /** + * Get the interpolated MotionState at any given time, clamping to the endpoints if time is out of + * bounds. + * + * @param t The time to query. + * @return The MotionState at time t, or closest to it if t is outside the profile. + */ + public MotionState stateByTimeClamped(double t) { + if (t < startTime()) { + return startState(); + } else if (t > endTime()) { + return endState(); + } + for (MotionSegment s : mSegments) { + if (s.containsTime(t)) { + return s.start().extrapolate(t); + } + } + // Should never get here. + return MotionState.kInvalidState; + } + + /** + * Get the interpolated MotionState by distance (the "pos()" field of MotionState). Note that + * since a profile may reverse, this method only returns the *first* instance of this position. + * + * @param pos The position to query. + * @return Empty if the profile never crosses pos or if the profile is invalid, or the resulting + * MotionState + * otherwise. + */ + public Optional firstStateByPos(double pos) { + for (MotionSegment s : mSegments) { + if (s.containsPos(pos)) { + if (epsilonEquals(s.end().pos(), pos, kEpsilon)) { + return Optional.of(s.end()); + } + final double t = Math.min(s.start().nextTimeAtPos(pos), s.end().t()); + if (Double.isNaN(t)) { + System.err.println("Error! We should reach 'pos' but we don't"); + return Optional.empty(); + } + return Optional.of(s.start().extrapolate(t)); + } + } + // We never reach pos. + return Optional.empty(); + } + + /** + * Remove all parts of the profile prior to the query time. This eliminates whole segments and + * also shortens any segments containing t. + * + * @param t The query time. + */ + public void trimBeforeTime(double t) { + for (Iterator iterator = mSegments.iterator(); iterator.hasNext();) { + MotionSegment s = iterator.next(); + if (s.end().t() <= t) { + // Segment is fully before t. + iterator.remove(); + continue; + } + if (s.start().t() <= t) { + // Segment begins before t; let's shorten the segment. + s.setStart(s.start().extrapolate(t)); + } + break; + } + } + + /** + * Remove all segments. + */ + public void clear() { + mSegments.clear(); + } + + /** + * Remove all segments and initialize to the desired state (actually a segment of length 0 that + * starts and ends at initial_state). + * + * @param initial_state The MotionState to initialize to. + */ + public void reset(MotionState initial_state) { + clear(); + mSegments.add(new MotionSegment(initial_state, initial_state)); + } + + /** + * Remove redundant segments (segments whose start and end states are coincident). + */ + public void consolidate() { + for (Iterator iterator = mSegments.iterator(); + iterator.hasNext() && mSegments.size() > 1;) { + MotionSegment s = iterator.next(); + if (s.start().coincident(s.end())) { + iterator.remove(); + } + } + } + + /** + * Add to the profile by applying an acceleration control for a given time. This is appended to + * the previous last state. + * + * @param acc The acceleration to apply. + * @param dt The period of time to apply the given acceleration. + */ + public void appendControl(double acc, double dt) { + if (isEmpty()) { + System.err.println("Error! Trying to append to empty profile"); + return; + } + MotionState last_end_state = mSegments.get(mSegments.size() - 1).end(); + MotionState new_start_state = + new MotionState(last_end_state.t(), last_end_state.pos(), last_end_state.vel(), acc); + appendSegment( + new MotionSegment(new_start_state, new_start_state.extrapolate(new_start_state.t() + dt)) + ); + } + + /** + * Add to the profile by inserting a new segment. No validity checking is done. + * + * @param segment The segment to add. + */ + public void appendSegment(MotionSegment segment) { + mSegments.add(segment); + } + + /** + * Add to the profile by inserting a new profile after the final state. No validity checking is + * done. + * + * @param profile The profile to add. + */ + public void appendProfile(MotionProfile profile) { + for (MotionSegment s : profile.segments()) { + appendSegment(s); + } + } + + /** + * @return The number of segments. + */ + public int size() { + return mSegments.size(); + } + + /** + * @return The list of segments. + */ + public List segments() { + return mSegments; + } + + /** + * @return The first state in the profile (or kInvalidState if empty). + */ + public MotionState startState() { + if (isEmpty()) { + return MotionState.kInvalidState; + } + return mSegments.get(0).start(); + } + + /** + * @return The time of the first state in the profile (or NaN if empty). + */ + public double startTime() { + return startState().t(); + } + + /** + * @return The pos of the first state in the profile (or NaN if empty). + */ + public double startPos() { + return startState().pos(); + } + + /** + * @return The last state in the profile (or kInvalidState if empty). + */ + public MotionState endState() { + if (isEmpty()) { + return MotionState.kInvalidState; + } + return mSegments.get(mSegments.size() - 1).end(); + } + + /** + * @return The time of the last state in the profile (or NaN if empty). + */ + public double endTime() { + return endState().t(); + } + + /** + * @return The pos of the last state in the profile (or NaN if empty). + */ + public double endPos() { + return endState().pos(); + } + + /** + * @return The duration of the entire profile. + */ + public double duration() { + return endTime() - startTime(); + } + + /** + * @return The total distance covered by the profile. Note that distance is the sum of absolute + * distances of all + * segments, so a reversing profile will count the distance covered in each direction. + */ + public double length() { + double length = 0.0; + for (MotionSegment s : segments()) { + length += Math.abs(s.end().pos() - s.start().pos()); + } + return length; + } + + @Override + public String toString() { + StringBuilder builder = new StringBuilder("Profile:"); + for (MotionSegment s : segments()) { + builder.append("\n\t"); + builder.append(s); + } + return builder.toString(); + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionProfileConstraints.java b/src/main/java/com/team254/lib/motion/MotionProfileConstraints.java new file mode 100644 index 0000000..42b228e --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionProfileConstraints.java @@ -0,0 +1,47 @@ +package com.team254.lib.motion; + +/** + * Constraints for constructing a MotionProfile. + */ +public class MotionProfileConstraints { + protected double max_vel = Double.POSITIVE_INFINITY; + protected double min_vel = Double.NEGATIVE_INFINITY; + protected double max_abs_acc = Double.POSITIVE_INFINITY; + + public MotionProfileConstraints(double max_vel, double min_vel, double max_acc) { + this.max_vel = Math.abs(max_vel); + this.min_vel = -Math.abs(min_vel); + this.max_abs_acc = Math.abs(max_acc); + } + + /** + * @return The (positive) maximum allowed velocity. + */ + public double max_vel() { + return max_vel; + } + + /** + * @return The (negative) minimum allowed velocity. + */ + public double min_vel() { + return min_vel; + } + + /** + * @return The (positive) maximum allowed acceleration. + */ + public double max_abs_acc() { + return max_abs_acc; + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof MotionProfileConstraints)) { + return false; + } + final MotionProfileConstraints other = (MotionProfileConstraints) obj; + return (other.max_abs_acc() == max_abs_acc()) && (other.max_vel() == max_vel()) + && (other.min_vel() == min_vel()); + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionProfileGenerator.java b/src/main/java/com/team254/lib/motion/MotionProfileGenerator.java new file mode 100644 index 0000000..bdb8de4 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionProfileGenerator.java @@ -0,0 +1,280 @@ +package com.team254.lib.motion; + +import com.team254.lib.motion.IMotionProfileGoal.CompletionBehavior; +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; +import javax.sound.sampled.Line; + +/** + * A MotionProfileGenerator generates minimum-time MotionProfiles to travel from a given MotionState + * to a given MotionProfileGoal while obeying a set of MotionProfileConstraints. + */ +public class MotionProfileGenerator { + // Static class. + private MotionProfileGenerator() {} + + protected static MotionProfile generateFlippedProfile( + MotionProfileConstraints constraints, + MotionProfileGoal goal_state, + MotionState prev_state, + boolean origFlipped + ) { + MotionProfile profile = + generateProfileImpl(constraints, goal_state.flipped(), prev_state.flipped(), !origFlipped); + for (MotionSegment s : profile.segments()) { + s.setStart(s.start().flipped()); + s.setEnd(s.end().flipped()); + } + return profile; + } + + public synchronized static MotionProfile generateProfile( + MotionProfileConstraints constraints, IMotionProfileGoal goal_state, MotionState prev_state + ) { + if (goal_state instanceof MotionProfileGoal) { + return generateProfile(constraints, (MotionProfileGoal) goal_state, prev_state); + } else { + return generateProfile( + constraints, (LinearTimeVaryingMotionProfileGoal) goal_state, prev_state + ); + } + } + + /** + * Generate a motion profile. + * + * @param constraints The constraints to use. + * @param goal_state The goal to use. + * @param prev_state The initial state to use. + * @return A motion profile from prev_state to goal_state that satisfies constraints. + */ + public synchronized static MotionProfile generateProfile( + MotionProfileConstraints constraints, MotionProfileGoal goal_state, MotionState prev_state + ) { + return generateProfileImpl(constraints, goal_state, prev_state, false); + } + + /** + * Generate a motion profile to track a linear time-varying goal. + * + * @param constraints The constraints to use. Note that max_vel must == -min_vel. + * @param goal_state The goal to use. If goal_vel is outside the constraints, it is clamped. + * @param prev_state The initial state to use. + * @return A motion profile from prev_state to goal_state that satisfies constraints. + */ + public synchronized static MotionProfile generateProfile( + MotionProfileConstraints constraints, + LinearTimeVaryingMotionProfileGoal goal_state, + MotionState prev_state + ) { + // The trick we use to solve this problem simply requires symmetrical velocity constraints. + assert (constraints.max_vel() == -constraints.min_vel()); + if (goal_state.max_vel() >= constraints.max_vel() + || goal_state.max_vel() <= constraints.min_vel()) { + // Reaching the goal exactly is impossible, so just move towards the goal as quickly as + // possible. + return generateProfile( + constraints, + new MotionProfileGoal( + Math.signum(goal_state.max_vel()) * Double.MAX_VALUE, goal_state.max_vel() + ), + prev_state + ); + } + + // Turn the linear time-varying goal into a stationary goal by superimposing the (opposite of) + // goal state motion onto the initial state and all constraints. Then add the goal state motion + // back to the result. + MotionState superposedPrevState = new MotionState( + prev_state.t(), prev_state.pos(), prev_state.vel() - goal_state.max_vel(), prev_state.acc() + ); + MotionProfileGoal superposedGoal = goal_state.toMotionProfileGoal(prev_state.t()); + MotionProfileConstraints superposedConstraints = new MotionProfileConstraints( + constraints.max_vel() - goal_state.max_vel(), + constraints.min_vel() - goal_state.max_vel(), + constraints.max_abs_acc() + ); + var profile = + generateProfileImpl(superposedConstraints, superposedGoal, superposedPrevState, false); + if (profile.size() == 0) + return profile; + + // Now add goal velocity back in. + for (MotionSegment s : profile.segments()) { + s.setStart(shiftByVelocity(s.start(), goal_state.max_vel(), prev_state.t())); + s.setEnd(shiftByVelocity(s.end(), goal_state.max_vel(), prev_state.t())); + } + + // As a result of the shift, individual segments may now (a) be able to be combined, (b) have to + // be split in order to ensure that no segment has both positive and negative velocities. + + // Combine segments with identical states after shifting. + var segments = profile.segments(); + MotionSegment cur = null; + for (Iterator iterator = segments.iterator(); + iterator.hasNext() && segments.size() > 1;) { + if (cur == null) { + cur = iterator.next(); + } + if (iterator.hasNext()) { + MotionSegment next = iterator.next(); + if (cur.end().equals(next.start())) { + // Concatenate 'next' into 'cur' and remove 'next'. + cur.setEnd(next.end()); + iterator.remove(); + } else { + cur = next; + } + } + } + + // Split segments that change direction after shifting. + List new_segments = new ArrayList(profile.size()); + for (var segment : profile.segments()) { + if (segment.start().vel() * segment.end().vel() >= 0.0) { + new_segments.add(segment); + } else { + // Velocity sign changes midway through segment. Figure out where and make two new segments + // around it. + double interp = -segment.start().vel() / (segment.end().vel() - segment.start().vel()); + double t = segment.start().t() + interp * (segment.end().t() - segment.start().t()); + var transition_state = segment.start().extrapolate(t); + new_segments.add(new MotionSegment(segment.start(), transition_state)); + new_segments.add(new MotionSegment(transition_state, segment.end())); + } + } + return new MotionProfile(new_segments); + } + + protected static MotionState shiftByVelocity( + MotionState state, double velocity_shift, double start_time + ) { + return new MotionState( + state.t(), + state.pos() + velocity_shift * (state.t() - start_time), + state.vel() + velocity_shift, + state.acc() + ); + } + + protected static MotionProfile generateProfileImpl( + MotionProfileConstraints constraints, + MotionProfileGoal goal_state, + MotionState prev_state, + boolean flipped + ) { + double delta_pos = goal_state.pos() - prev_state.pos(); + if (delta_pos < 0.0 || (delta_pos == 0.0 && prev_state.vel() < 0.0)) { + // For simplicity, we always assume the goal requires positive movement. If negative, we flip + // to solve, then flip the solution. + return generateFlippedProfile(constraints, goal_state, prev_state, flipped); + } + final double max_vel = flipped ? -constraints.min_vel() : constraints.max_vel(); + // Invariant from this point on: delta_pos >= 0.0 + // Clamp the start state to be valid. + MotionState start_state = new MotionState( + prev_state.t(), + prev_state.pos(), + Math.signum(prev_state.vel()) * Math.min(Math.abs(prev_state.vel()), max_vel), + Math.signum(prev_state.acc()) + * Math.min(Math.abs(prev_state.acc()), constraints.max_abs_acc()) + ); + MotionProfile profile = new MotionProfile(); + profile.reset(start_state); + // If our velocity is headed away from the goal, the first thing we need to do is to stop. + if (start_state.vel() < 0.0 && delta_pos > 0.0) { + final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc()); + profile.appendControl(constraints.max_abs_acc(), stopping_time); + start_state = profile.endState(); + delta_pos = goal_state.pos() - start_state.pos(); + } + // Invariant from this point on: start_state.vel() >= 0.0 + final double min_abs_vel_at_goal_sqr = + start_state.vel2() - 2.0 * constraints.max_abs_acc() * delta_pos; + final double min_abs_vel_at_goal = Math.sqrt(Math.abs(min_abs_vel_at_goal_sqr)); + final double max_abs_vel_at_goal = + Math.sqrt(start_state.vel2() + 2.0 * constraints.max_abs_acc() * delta_pos); + double goal_vel = goal_state.max_vel(); + double max_acc = constraints.max_abs_acc(); + if (min_abs_vel_at_goal_sqr > 0.0 + && min_abs_vel_at_goal > (goal_state.max_vel() + goal_state.vel_tolerance())) { + // Overshoot is unavoidable with the current constraints. Look at completion_behavior to see + // what we should do. + if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ABS_VEL) { + // Adjust the goal velocity. + goal_vel = min_abs_vel_at_goal; + } else if (goal_state.completion_behavior() == CompletionBehavior.VIOLATE_MAX_ACCEL) { + if (Math.abs(delta_pos) < goal_state.pos_tolerance()) { + // Special case: We are at the goal but moving too fast. This requires 'infinite' + // acceleration, which will result in NaNs below, so we can return the profile + // immediately. + profile.appendSegment(new MotionSegment( + new MotionState( + profile.endTime(), + profile.endPos(), + profile.endState().vel(), + Double.NEGATIVE_INFINITY + ), + new MotionState( + profile.endTime(), profile.endPos(), goal_vel, Double.NEGATIVE_INFINITY + ) + )); + profile.consolidate(); + return profile; + } + // Adjust the max acceleration. + max_acc = Math.abs(goal_vel * goal_vel - start_state.vel2()) / (2.0 * delta_pos); + } else { + // We are going to overshoot the goal, so the first thing we need to do is come to a stop. + final double stopping_time = Math.abs(start_state.vel() / constraints.max_abs_acc()); + profile.appendControl(-constraints.max_abs_acc(), stopping_time); + // Now we need to travel backwards, so generate a flipped profile. + profile.appendProfile( + generateFlippedProfile(constraints, goal_state, profile.endState(), flipped) + ); + profile.consolidate(); + return profile; + } + } + goal_vel = Math.min(goal_vel, max_abs_vel_at_goal); + // Invariant from this point forward: We can achieve goal_vel at goal_state.pos exactly using no + // more than +/- max_acc. + + // What is the maximum velocity we can reach (Vmax)? This is the intersection of two curves: one + // accelerating towards the goal from profile.finalState(), the other coming from the goal at + // max vel (in reverse). If Vmax is greater than constraints.max_abs_vel, we will clamp and + // cruise. Solve the following three equations to find Vmax (by substitution): Vmax^2 = Vstart^2 + // + 2*a*d_accel Vgoal^2 = Vmax^2 - 2*a*d_decel delta_pos = d_accel + d_decel + final double v_max = Math.min( + max_vel, Math.sqrt((start_state.vel2() + goal_vel * goal_vel) / 2.0 + delta_pos * max_acc) + ); + + // Accelerate to v_max + if (v_max > start_state.vel()) { + final double accel_time = (v_max - start_state.vel()) / max_acc; + profile.appendControl(max_acc, accel_time); + start_state = profile.endState(); + } + // Figure out how much distance will be covered during deceleration. + final double distance_decel = Math.max( + 0.0, (start_state.vel2() - goal_vel * goal_vel) / (2.0 * constraints.max_abs_acc()) + ); + final double distance_cruise = + Math.max(0.0, goal_state.pos() - start_state.pos() - distance_decel); + // Cruise at constant velocity. + if (distance_cruise > 0.0) { + final double cruise_time = distance_cruise / start_state.vel(); + profile.appendControl(0.0, cruise_time); + start_state = profile.endState(); + } + // Decelerate to goal velocity. + if (distance_decel > 0.0) { + final double decel_time = (start_state.vel() - goal_vel) / max_acc; + profile.appendControl(-max_acc, decel_time); + } + + profile.consolidate(); + return profile; + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionProfileGoal.java b/src/main/java/com/team254/lib/motion/MotionProfileGoal.java new file mode 100644 index 0000000..00f81b3 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionProfileGoal.java @@ -0,0 +1,138 @@ +package com.team254.lib.motion; + +import static com.team254.lib.util.Util.epsilonEquals; + +/** + * A MotionProfileGoal defines a desired position and maximum velocity (at this position), along + * with the behavior that should be used to determine if we are at the goal and what to do if it is + * infeasible to reach the goal within the desired velocity bounds. + */ +public class MotionProfileGoal implements IMotionProfileGoal { + protected double pos; + protected double max_vel; + protected CompletionBehavior completion_behavior = CompletionBehavior.OVERSHOOT; + protected double pos_tolerance = 1E-3; + protected double vel_tolerance = 1E-2; + + public MotionProfileGoal() {} + + public MotionProfileGoal(double pos) { + this.pos = pos; + sanityCheck(); + } + + public MotionProfileGoal(double pos, double max_abs_vel) { + this.pos = pos; + this.max_vel = max_abs_vel; + sanityCheck(); + } + + public MotionProfileGoal(double pos, double max_abs_vel, CompletionBehavior completion_behavior) { + this.pos = pos; + this.max_vel = max_abs_vel; + this.completion_behavior = completion_behavior; + sanityCheck(); + } + + public MotionProfileGoal( + double pos, + double max_abs_vel, + CompletionBehavior completion_behavior, + double pos_tolerance, + double vel_tolerance + ) { + this.pos = pos; + this.max_vel = max_abs_vel; + this.completion_behavior = completion_behavior; + this.pos_tolerance = pos_tolerance; + this.vel_tolerance = vel_tolerance; + sanityCheck(); + } + + public MotionProfileGoal(MotionProfileGoal other) { + this( + other.pos, + other.max_vel, + other.completion_behavior, + other.pos_tolerance, + other.vel_tolerance + ); + } + + /** + * @return A flipped MotionProfileGoal (where the position is negated, but all other attributes + * remain the same). + */ + @Override + public MotionProfileGoal flipped() { + return new MotionProfileGoal(-pos, max_vel, completion_behavior, pos_tolerance, vel_tolerance); + } + + @Override + public double pos() { + return pos; + } + + @Override + public double max_vel() { + return max_vel; + } + + @Override + public double min_vel() { + return -max_vel; + } + + @Override + public double pos_tolerance() { + return pos_tolerance; + } + + @Override + public double vel_tolerance() { + return vel_tolerance; + } + + @Override + public CompletionBehavior completion_behavior() { + return completion_behavior; + } + + @Override + public boolean atGoalState(MotionState state) { + return atGoalPos(state.pos()) + && (Math.abs(state.vel()) < (max_vel + vel_tolerance) + || completion_behavior == CompletionBehavior.VIOLATE_MAX_ABS_VEL); + } + + @Override + public boolean atGoalPos(double pos) { + return epsilonEquals(pos, this.pos, pos_tolerance); + } + + /** + * This method makes sure that the completion behavior is compatible with the max goal velocity. + */ + protected void sanityCheck() { + if (max_vel > vel_tolerance && completion_behavior == CompletionBehavior.OVERSHOOT) { + completion_behavior = CompletionBehavior.VIOLATE_MAX_ACCEL; + } + } + + @Override + public String toString() { + return "pos: " + pos + " (+/- " + pos_tolerance + "), max_vel: " + max_vel + " (+/- " + + vel_tolerance + "), completion behavior: " + completion_behavior.name(); + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof MotionProfileGoal)) { + return false; + } + final MotionProfileGoal other = (MotionProfileGoal) obj; + return (other.completion_behavior() == completion_behavior()) && (other.pos() == pos()) + && (other.max_vel() == max_vel()) && (other.min_vel() == min_vel()) + && (other.pos_tolerance() == pos_tolerance()) && (other.vel_tolerance() == vel_tolerance()); + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionSegment.java b/src/main/java/com/team254/lib/motion/MotionSegment.java new file mode 100644 index 0000000..148c057 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionSegment.java @@ -0,0 +1,86 @@ +package com.team254.lib.motion; + +import static com.team254.lib.motion.MotionUtil.kEpsilon; +import static com.team254.lib.util.Util.epsilonEquals; + +/** + * A MotionSegment is a movement from a start MotionState to an end MotionState with a constant + * acceleration. + */ +public class MotionSegment { + protected MotionState mStart; + protected MotionState mEnd; + + public MotionSegment(MotionState start, MotionState end) { + mStart = start; + mEnd = end; + } + + /** + * Verifies that: + *

+ * 1. All segments have a constant acceleration. + *

+ * 2. All segments have monotonic position (sign of velocity doesn't change). + *

+ * 3. The time, position, velocity, and acceleration of the profile are consistent. + */ + public boolean isValid() { + if (!epsilonEquals(start().acc(), end().acc(), kEpsilon)) { + // Acceleration is not constant within the segment. + System.err.println( + "Segment acceleration not constant! Start acc: " + start().acc() + + ", End acc: " + end().acc() + ); + return false; + } + if (Math.signum(start().vel()) * Math.signum(end().vel()) < 0.0 + && !epsilonEquals(start().vel(), 0.0, kEpsilon) + && !epsilonEquals(end().vel(), 0.0, kEpsilon)) { + // Velocity direction reverses within the segment. + System.err.println( + "Segment velocity reverses! Start vel: " + start().vel() + ", End vel: " + end().vel() + ); + return false; + } + if (!start().extrapolate(end().t()).equals(end())) { + // A single segment is not consistent. + if (start().t() == end().t() && Double.isInfinite(start().acc())) { + // One allowed exception: If acc is infinite and dt is zero. + return true; + } + System.err.println("Segment not consistent! Start: " + start() + ", End: " + end()); + return false; + } + return true; + } + + public boolean containsTime(double t) { + return t >= start().t() && t <= end().t(); + } + + public boolean containsPos(double pos) { + return pos >= start().pos() && pos <= end().pos() || pos <= start().pos() && pos >= end().pos(); + } + + public MotionState start() { + return mStart; + } + + public void setStart(MotionState start) { + mStart = start; + } + + public MotionState end() { + return mEnd; + } + + public void setEnd(MotionState end) { + mEnd = end; + } + + @Override + public String toString() { + return "Start: " + start() + ", End: " + end(); + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionState.java b/src/main/java/com/team254/lib/motion/MotionState.java new file mode 100644 index 0000000..09bbd54 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionState.java @@ -0,0 +1,169 @@ +package com.team254.lib.motion; + +import static com.team254.lib.motion.MotionUtil.kEpsilon; +import static com.team254.lib.util.Util.epsilonEquals; + +/** + * A MotionState is a completely specified state of 1D motion through time. + */ +public class MotionState { + protected final double t; + protected final double pos; + protected final double vel; + protected final double acc; + + public static MotionState kInvalidState = + new MotionState(Double.NaN, Double.NaN, Double.NaN, Double.NaN); + + public MotionState(double t, double pos, double vel, double acc) { + this.t = t; + this.pos = pos; + this.vel = vel; + this.acc = acc; + } + + public MotionState(MotionState state) { + this(state.t, state.pos, state.vel, state.acc); + } + + public double t() { + return t; + } + + public double pos() { + return pos; + } + + public double vel() { + return vel; + } + + public double vel2() { + return vel * vel; + } + + public double acc() { + return acc; + } + + /** + * Extrapolates this MotionState to the specified time by applying this MotionState's + * acceleration. + * + * @param t The time of the new MotionState. + * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this + * state. + */ + public MotionState extrapolate(double t) { + return extrapolate(t, acc); + } + + /** + * Extrapolates this MotionState to the specified time by applying a given acceleration to the (t, + * pos, vel) portion of this MotionState. + * + * @param t The time of the new MotionState. + * @param acc The acceleration to apply. + * @return A MotionState that is a valid predecessor (if t<=0) or successor (if t>=0) of this + * state (with the + * specified accel). + */ + public MotionState extrapolate(double t, double acc) { + final double dt = t - this.t; + return new MotionState(t, pos + vel * dt + .5 * acc * dt * dt, vel + acc * dt, acc); + } + + /** + * Find the next time (first time > MotionState.t()) that this MotionState will be at pos. This is + * an inverse of the extrapolate() method. + * + * @param pos The position to query. + * @return The time when we are next at pos() if we are extrapolating with a positive dt. NaN if + * we never reach pos. + */ + public double nextTimeAtPos(double pos) { + if (epsilonEquals(pos, this.pos, kEpsilon)) { + // Already at pos. + return t; + } + if (epsilonEquals(acc, 0.0, kEpsilon)) { + // Zero acceleration case. + final double delta_pos = pos - this.pos; + if (!epsilonEquals(vel, 0.0, kEpsilon) && Math.signum(delta_pos) == Math.signum(vel)) { + // Constant velocity heading towards pos. + return delta_pos / vel + t; + } + return Double.NaN; + } + + // Solve the quadratic formula. + // ax^2 + bx + c == 0 + // x = dt + // a = .5 * acc + // b = vel + // c = this.pos - pos + final double disc = vel * vel - 2.0 * acc * (this.pos - pos); + if (disc < 0.0) { + // Extrapolating this MotionState never reaches the desired pos. + return Double.NaN; + } + final double sqrt_disc = Math.sqrt(disc); + final double max_dt = (-vel + sqrt_disc) / acc; + final double min_dt = (-vel - sqrt_disc) / acc; + if (min_dt >= 0.0 && (max_dt < 0.0 || min_dt < max_dt)) { + return t + min_dt; + } + if (max_dt >= 0.0) { + return t + max_dt; + } + // We only reach the desired pos in the past. + return Double.NaN; + } + + @Override + public String toString() { + return "(t=" + t + ", pos=" + pos + ", vel=" + vel + ", acc=" + acc + ")"; + } + + /** + * Checks if two MotionStates are epsilon-equals (all fields are equal within a nominal + * tolerance). + */ + @Override + public boolean equals(Object other) { + return (other instanceof MotionState) && equals((MotionState) other, kEpsilon); + } + + /** + * Checks if two MotionStates are epsilon-equals (all fields are equal within a specified + * tolerance). + */ + public boolean equals(MotionState other, double epsilon) { + return coincident(other, epsilon) && epsilonEquals(acc, other.acc, epsilon); + } + + /** + * Checks if two MotionStates are coincident (t, pos, and vel are equal within a nominal + * tolerance, but acceleration may be different). + */ + public boolean coincident(MotionState other) { + return coincident(other, kEpsilon); + } + + /** + * Checks if two MotionStates are coincident (t, pos, and vel are equal within a specified + * tolerance, but acceleration may be different). + */ + public boolean coincident(MotionState other, double epsilon) { + return epsilonEquals(t, other.t, epsilon) && epsilonEquals(pos, other.pos, epsilon) + && epsilonEquals(vel, other.vel, epsilon); + } + + /** + * Returns a MotionState that is the mirror image of this one. Pos, vel, and acc are all negated, + * but time is not. + */ + public MotionState flipped() { + return new MotionState(t, -pos, -vel, -acc); + } +} diff --git a/src/main/java/com/team254/lib/motion/MotionUtil.java b/src/main/java/com/team254/lib/motion/MotionUtil.java new file mode 100644 index 0000000..3245b5f --- /dev/null +++ b/src/main/java/com/team254/lib/motion/MotionUtil.java @@ -0,0 +1,8 @@ +package com.team254.lib.motion; + +public class MotionUtil { + /** + * A constant for consistent floating-point equality checking within this library. + */ + public static double kEpsilon = 1e-6; +} diff --git a/src/main/java/com/team254/lib/motion/ProfileFollower.java b/src/main/java/com/team254/lib/motion/ProfileFollower.java new file mode 100644 index 0000000..e954028 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/ProfileFollower.java @@ -0,0 +1,212 @@ +package com.team254.lib.motion; + +import com.team254.lib.motion.IMotionProfileGoal.CompletionBehavior; +import com.team254.lib.util.Util; + +/** + * A controller for tracking a profile generated to attain a MotionProfileGoal. The controller uses + * feedforward on acceleration, velocity, and position; proportional feedback on velocity and + * position; and integral feedback on position. + */ +public class ProfileFollower { + protected double mKp; + protected double mKi; + protected double mKv; + protected double mKffv; + protected double mKffa; + protected double mKs; + + protected double mMinOutput = Double.NEGATIVE_INFINITY; + protected double mMaxOutput = Double.POSITIVE_INFINITY; + protected MotionState mLatestActualState; + protected MotionState mInitialState; + protected double mLatestPosError; + protected double mLatestVelError; + protected double mTotalError; + + protected MotionProfileGoal mGoal = null; + protected MotionProfileConstraints mConstraints = null; + protected SetpointGenerator mSetpointGenerator = new SetpointGenerator(); + protected SetpointGenerator.Setpoint mLatestSetpoint = null; + + /** + * Create a new ProfileFollower. + * + * @param kp The proportional gain on position error. + * @param ki The integral gain on position error. + * @param kv The proportional gain on velocity error (or derivative gain on position error). + * @param kffv The feedforward gain on velocity. Should be 1.0 if the units of the profile match + * the units of the + * output. + * @param kffa The feedforward gain on acceleration. + * @param ks The throttle required to break static friction. + */ + public ProfileFollower(double kp, double ki, double kv, double kffv, double kffa, double ks) { + resetProfile(); + setGains(kp, ki, kv, kffv, kffa, ks); + } + + public void setGains(double kp, double ki, double kv, double kffv, double kffa, double ks) { + mKp = kp; + mKi = ki; + mKv = kv; + mKffv = kffv; + mKffa = kffa; + mKs = ks; + } + + /** + * Completely clear all state related to the current profile (min and max outputs are maintained). + */ + public void resetProfile() { + mTotalError = 0.0; + mInitialState = MotionState.kInvalidState; + mLatestActualState = MotionState.kInvalidState; + mLatestPosError = Double.NaN; + mLatestVelError = Double.NaN; + mSetpointGenerator.reset(); + mGoal = null; + mConstraints = null; + resetSetpoint(); + } + + /** + * Specify a goal and constraints for achieving the goal. + */ + public void setGoalAndConstraints(MotionProfileGoal goal, MotionProfileConstraints constraints) { + if (mGoal != null && !mGoal.equals(goal) && mLatestSetpoint != null) { + // Clear the final state bit since the goal has changed. + mLatestSetpoint.final_setpoint = false; + } + mGoal = goal; + mConstraints = constraints; + } + + public void setGoal(MotionProfileGoal goal) { + setGoalAndConstraints(goal, mConstraints); + } + + /** + * @return The current goal (null if no goal has been set since the latest call to reset()). + */ + public MotionProfileGoal getGoal() { + return mGoal; + } + + public void setConstraints(MotionProfileConstraints constraints) { + setGoalAndConstraints(mGoal, constraints); + } + + public MotionState getSetpoint() { + return (mLatestSetpoint == null ? MotionState.kInvalidState : mLatestSetpoint.motion_state); + } + + /** + * Reset just the setpoint. This means that the latest_state provided to update() will be used + * rather than feeding forward the previous setpoint the next time update() is called. This almost + * always forces a MotionProfile update, and may be warranted if tracking error gets very large. + */ + public void resetSetpoint() { + mLatestSetpoint = null; + } + + public void resetIntegral() { + mTotalError = 0.0; + } + + /** + * Update the setpoint and apply the control gains to generate a control output. + * + * @param latest_state The latest *actual* state, used only for feedback purposes (unless this is + * the first iteration or + * reset()/resetSetpoint() was just called, in which case this is the new + * start state for the profile). + * @param t The timestamp for which the setpoint is desired. + * @return An output that reflects the control output to apply to achieve the new setpoint. + */ + public synchronized double update(MotionState latest_state, double t) { + mLatestActualState = latest_state; + MotionState prev_state = latest_state; + if (mLatestSetpoint != null) { + prev_state = mLatestSetpoint.motion_state; + } else { + mInitialState = prev_state; + } + final double dt = Math.max(0.0, t - prev_state.t()); + mLatestSetpoint = mSetpointGenerator.getSetpoint(mConstraints, mGoal, prev_state, t); + + // Update error. + mLatestPosError = mLatestSetpoint.motion_state.pos() - latest_state.pos(); + mLatestVelError = mLatestSetpoint.motion_state.vel() - latest_state.vel(); + + // Calculate the feedforward and proportional terms. + double output = mKp * mLatestPosError + mKv * mLatestVelError + + mKffv * mLatestSetpoint.motion_state.vel() + + (Double.isNaN(mLatestSetpoint.motion_state.acc()) + ? 0.0 + : mKffa * mLatestSetpoint.motion_state.acc()); + + if (!Util.epsilonEquals(output, 0.0)) { + output += mKs * Math.signum(output); + } + + if (output >= mMinOutput && output <= mMaxOutput) { + // Update integral. + mTotalError += mLatestPosError * dt; + output += mKi * mTotalError; + } else { + // Reset integral windup. + mTotalError = 0.0; + } + // Clamp to limits. + output = Math.max(mMinOutput, Math.min(mMaxOutput, output)); + + return output; + } + + public void setMinOutput(double min_output) { + mMinOutput = min_output; + } + + public void setMaxOutput(double max_output) { + mMaxOutput = max_output; + } + + public double getPosError() { + return mLatestPosError; + } + + public double getVelError() { + return mLatestVelError; + } + + /** + * We are finished the profile when the final setpoint has been generated. Note that this does not + * check whether we are anywhere close to the final setpoint, however. + * + * @return True if the final setpoint has been generated for the current goal. + */ + public boolean isFinishedProfile() { + return mGoal != null && mLatestSetpoint != null && mLatestSetpoint.final_setpoint; + } + + /** + * We are on target if our actual state achieves the goal (where the definition of achievement + * depends on the goal's completion behavior). + * + * @return True if we have actually achieved the current goal. + */ + public boolean onTarget() { + if (mGoal == null || mLatestSetpoint == null) { + return false; + } + // For the options that don't achieve the goal velocity exactly, also count any instance where + // we have passed the finish line. + final double goal_to_start = mGoal.pos() - mInitialState.pos(); + final double goal_to_actual = mGoal.pos() - mLatestActualState.pos(); + final boolean passed_goal_state = + Math.signum(goal_to_start) * Math.signum(goal_to_actual) < 0.0; + return mGoal.atGoalState(mLatestActualState) + || (mGoal.completion_behavior() != CompletionBehavior.OVERSHOOT && passed_goal_state); + } +} diff --git a/src/main/java/com/team254/lib/motion/SetpointGenerator.java b/src/main/java/com/team254/lib/motion/SetpointGenerator.java new file mode 100644 index 0000000..4f02008 --- /dev/null +++ b/src/main/java/com/team254/lib/motion/SetpointGenerator.java @@ -0,0 +1,119 @@ +package com.team254.lib.motion; + +import java.util.Optional; + +/** + * A SetpointGenerate does just-in-time motion profile generation to supply a stream of setpoints + * that obey the given constraints to a controller. The profile is regenerated when any of the + * inputs change, but is cached (and trimmed as we go) if the only update is to the current state. + *

+ * Note that typically for smooth control, a user will feed the last iteration's setpoint as the + * argument to getSetpoint(), and should only use a measured state directly on the first iteration + * or if a large disturbance is detected. + */ +public class SetpointGenerator { + /** + * A Setpoint is just a MotionState and an additional flag indicating whether this is setpoint + * achieves the goal (useful for higher-level logic to know that it is now time to do something + * else). + */ + public static class Setpoint { + public MotionState motion_state; + public boolean final_setpoint; + + public Setpoint(MotionState motion_state, boolean final_setpoint) { + this.motion_state = motion_state; + this.final_setpoint = final_setpoint; + } + } + + protected MotionProfile mProfile = null; + protected IMotionProfileGoal mGoal = null; + protected MotionProfileConstraints mConstraints = null; + + public SetpointGenerator() {} + + /** + * Force a reset of the profile. + */ + public void reset() { + mProfile = null; + mGoal = null; + mConstraints = null; + } + + /** + * Get a new Setpoint (and generate a new MotionProfile if necessary). + * + * @param constraints The constraints to use. + * @param goal The goal to use. + * @param prev_state The previous setpoint (or measured state of the system to do a reset). + * @param t The time to generate a setpoint for. + * @return The new Setpoint at time t. + */ + public synchronized Setpoint getSetpoint( + MotionProfileConstraints constraints, + IMotionProfileGoal goal, + MotionState prev_state, + double t + ) { + boolean regenerate = mConstraints == null || !mConstraints.equals(constraints) || mGoal == null + || !mGoal.equals(goal) || mProfile == null; + if (!regenerate && !mProfile.isEmpty()) { + Optional expected_state = mProfile.stateByTime(prev_state.t()); + regenerate = !expected_state.isPresent() || !expected_state.get().equals(prev_state); + } + if (regenerate) { + // Regenerate the profile, as our current profile does not satisfy the inputs. + mConstraints = constraints; + mGoal = goal; + mProfile = MotionProfileGenerator.generateProfile(constraints, goal, prev_state); + // System.out.println("Regenerating profile: " + mProfile); + } + + // Sample the profile at time t. + Setpoint rv = null; + if (!mProfile.isEmpty() && mProfile.isValid()) { + MotionState setpoint; + if (t > mProfile.endTime()) { + setpoint = mProfile.endState(); + } else if (t < mProfile.startTime()) { + setpoint = mProfile.startState(); + } else { + setpoint = mProfile.stateByTime(t).get(); + } + // Shorten the profile and return the new setpoint. + mProfile.trimBeforeTime(t); + rv = new Setpoint(setpoint, mProfile.isEmpty() || mGoal.atGoalState(setpoint)); + } + + // Invalid or empty profile - just output the same state again. + if (rv == null) { + rv = new Setpoint(prev_state, true); + } + + if (rv.final_setpoint) { + // Ensure the final setpoint matches the goal exactly. + rv.motion_state = new MotionState( + rv.motion_state.t(), + mGoal.pos(), + Math.signum(rv.motion_state.vel()) + * Math.max(mGoal.max_vel(), Math.abs(rv.motion_state.vel())), + 0.0 + ); + } + + return rv; + } + + /** + * Get the full profile from the latest call to getSetpoint(). Useful to check estimated time or + * distance to goal. + * + * @return The profile from the latest call to getSetpoint(), or null if there is not yet a + * profile. + */ + public MotionProfile getProfile() { + return mProfile; + } +} diff --git a/src/main/java/com/team254/lib/physics/DCMotorTransmission.java b/src/main/java/com/team254/lib/physics/DCMotorTransmission.java new file mode 100755 index 0000000..7ec6218 --- /dev/null +++ b/src/main/java/com/team254/lib/physics/DCMotorTransmission.java @@ -0,0 +1,88 @@ +package com.team254.lib.physics; + +import com.team254.lib.util.Util; + +/** + * Model of a DC motor rotating a shaft. All parameters refer to the output (e.g. should already + * consider gearing and efficiency losses). The motor is assumed to be symmetric forward/reverse. + */ +public class DCMotorTransmission { + // TODO add electrical constants? (e.g. current) + + // All units must be SI! + protected final double speed_per_volt_; // rad/s per V (no load) + protected final double torque_per_volt_; // N m per V (stall) + protected final double friction_voltage_; // V + + public DCMotorTransmission( + final double speed_per_volt, final double torque_per_volt, final double friction_voltage + ) { + speed_per_volt_ = speed_per_volt; + torque_per_volt_ = torque_per_volt; + friction_voltage_ = friction_voltage; + } + + public double speed_per_volt() { + return speed_per_volt_; + } + + public double torque_per_volt() { + return torque_per_volt_; + } + + public double friction_voltage() { + return friction_voltage_; + } + + public double free_speed_at_voltage(final double voltage) { + if (voltage > Util.kEpsilon) { + return Math.max(0.0, voltage - friction_voltage()) * speed_per_volt(); + } else if (voltage < Util.kEpsilon) { + return Math.min(0.0, voltage + friction_voltage()) * speed_per_volt(); + } else { + return 0.0; + } + } + + public double getTorqueForVoltage(final double output_speed, final double voltage) { + double effective_voltage = voltage; + if (output_speed > Util.kEpsilon) { + // Forward motion, rolling friction. + effective_voltage -= friction_voltage(); + } else if (output_speed < -Util.kEpsilon) { + // Reverse motion, rolling friction. + effective_voltage += friction_voltage(); + } else if (voltage > Util.kEpsilon) { + // System is static, forward torque. + effective_voltage = Math.max(0.0, voltage - friction_voltage()); + } else if (voltage < -Util.kEpsilon) { + // System is static, reverse torque. + effective_voltage = Math.min(0.0, voltage + friction_voltage()); + } else { + // System is idle. + return 0.0; + } + return torque_per_volt() * (-output_speed / speed_per_volt() + effective_voltage); + } + + public double getVoltageForTorque(final double output_speed, final double torque) { + double friction_voltage; + if (output_speed > Util.kEpsilon) { + // Forward motion, rolling friction. + friction_voltage = friction_voltage(); + } else if (output_speed < -Util.kEpsilon) { + // Reverse motion, rolling friction. + friction_voltage = -friction_voltage(); + } else if (torque > Util.kEpsilon) { + // System is static, forward torque. + friction_voltage = friction_voltage(); + } else if (torque < -Util.kEpsilon) { + // System is static, reverse torque. + friction_voltage = -friction_voltage(); + } else { + // System is idle. + return 0.0; + } + return torque / torque_per_volt() + output_speed / speed_per_volt() + friction_voltage; + } +} diff --git a/src/main/java/com/team254/lib/physics/SwerveDrive.java b/src/main/java/com/team254/lib/physics/SwerveDrive.java new file mode 100644 index 0000000..7ec13ba --- /dev/null +++ b/src/main/java/com/team254/lib/physics/SwerveDrive.java @@ -0,0 +1,49 @@ +package com.team254.lib.physics; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import java.text.DecimalFormat; + +public class SwerveDrive { + // All units must be SI! + + // Self-explanatory. Measure by rolling the robot a known distance and counting encoder ticks. + protected final double wheel_radius_; // m + + // "Effective" kinematic wheelbase radius. Might be larger than theoretical to compensate for + // skid steer. Measure by turning the robot in place several times and figuring out what the + // equivalent wheelbase radius is. + protected final double effective_wheelbase_radius_; // m + + public SwerveDrive(final double wheel_radius, final double effective_wheelbase_radius) { + wheel_radius_ = wheel_radius; + effective_wheelbase_radius_ = effective_wheelbase_radius; + } + + // Can refer to velocity or acceleration depending on context. + public static class ChassisState { + public Translation2d movement; + public Rotation2d254 heading; + + public ChassisState(Translation2d movement, Rotation2d254 heading) { + this.heading = heading; + this.movement = movement; + } + + public ChassisState(Translation2d movement) { + this.movement = movement; + this.heading = Rotation2d254.identity(); + } + + public ChassisState() { + this.movement = Translation2d.identity(); + this.heading = Rotation2d254.identity(); + } + + @Override + public String toString() { + DecimalFormat fmt = new DecimalFormat("#0.000"); + return fmt.format(movement.norm()) /* + ", " + fmt.format(heading.getRadians())*/; + } + } +} diff --git a/src/main/java/com/team254/lib/spline/CubicHermiteSpline.java b/src/main/java/com/team254/lib/spline/CubicHermiteSpline.java new file mode 100755 index 0000000..bcb7088 --- /dev/null +++ b/src/main/java/com/team254/lib/spline/CubicHermiteSpline.java @@ -0,0 +1,68 @@ +package com.team254.lib.spline; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; + +/** + * Temporary spline for testing + */ +public class CubicHermiteSpline extends Spline { + private final double ax, bx, cx, dx, ay, by, cy, dy; + + public CubicHermiteSpline(Pose2d p0, Pose2d p1) { + double x0, x1, dx0, dx1, y0, y1, dy0, dy1; + double scale = 2 * p0.getTranslation().distance(p1.getTranslation()); + x0 = p0.getTranslation().x(); + x1 = p1.getTranslation().x(); + dx0 = p0.getRotation().cos() * scale; + dx1 = p1.getRotation().cos() * scale; + y0 = p0.getTranslation().y(); + y1 = p1.getTranslation().y(); + dy0 = p0.getRotation().sin() * scale; + dy1 = p1.getRotation().sin() * scale; + ax = dx0 + dx1 + 2 * x0 - 2 * x1; + bx = -2 * dx0 - dx1 - 3 * x0 + 3 * x1; + cx = dx0; + dx = x0; + ay = dy0 + dy1 + 2 * y0 - 2 * y1; + by = -2 * dy0 - dy1 - 3 * y0 + 3 * y1; + cy = dy0; + dy = y0; + } + + @Override + public Translation2d getPoint(double t) { + final double x = t * t * t * ax + t * t * bx + t * cx + dx; + final double y = t * t * t * ay + t * t * by + t * cy + dy; + return new Translation2d(x, y); + } + + @Override + public Rotation2d254 getHeading(double t) { + final double dx = 3 * t * t * ax + 2 * t * bx + cx; + final double dy = 3 * t * t * ay + 2 * t * by + cy; + return new Rotation2d254(dx, dy, true); + } + + @Override + public double getVelocity(double t) { + // TODO implement this + return 1.0; + } + + @Override + public double getCurvature(double t) { + final double dx = 3 * t * t * ax + 2 * t * bx + cx; + final double dy = 3 * t * t * ay + 2 * t * by + cy; + final double ddx = 6 * t * ax + 2 * bx; + final double ddy = 6 * t * ay + 2 * by; + return (dx * ddy - dy * ddx) / ((dx * dx + dy * dy) * Math.sqrt(dx * dx + dy * dy)); + } + + @Override + public double getDCurvature(double t) { + // TODO implement this + return 0.0; + } +} diff --git a/src/main/java/com/team254/lib/spline/QuinticHermiteSpline.java b/src/main/java/com/team254/lib/spline/QuinticHermiteSpline.java new file mode 100755 index 0000000..307bc22 --- /dev/null +++ b/src/main/java/com/team254/lib/spline/QuinticHermiteSpline.java @@ -0,0 +1,410 @@ +package com.team254.lib.spline; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import java.util.List; + +public class QuinticHermiteSpline extends Spline { + private static final double kEpsilon = 1e-5; + private static final double kStepSize = 1.0; + private static final double kMinDelta = 0.001; + private static final int kSamples = 100; + private static final int kMaxIterations = 100; + + private double x0, x1, dx0, dx1, ddx0, ddx1, y0, y1, dy0, dy1, ddy0, ddy1; + private double ax, bx, cx, dx, ex, fx, ay, by, cy, dy, ey, fy; + + /** + * @param p0 The starting pose of the spline + * @param p1 The ending pose of the spline + */ + public QuinticHermiteSpline(Pose2d p0, Pose2d p1) { + double scale = 1.2 * p0.getTranslation().distance(p1.getTranslation()); + x0 = p0.getTranslation().x(); + x1 = p1.getTranslation().x(); + dx0 = p0.getRotation().cos() * scale; + dx1 = p1.getRotation().cos() * scale; + ddx0 = 0; + ddx1 = 0; + y0 = p0.getTranslation().y(); + y1 = p1.getTranslation().y(); + dy0 = p0.getRotation().sin() * scale; + dy1 = p1.getRotation().sin() * scale; + ddy0 = 0; + ddy1 = 0; + + computeCoefficients(); + } + + /** + * Used by the curvature optimization function + */ + private QuinticHermiteSpline( + double x0, + double x1, + double dx0, + double dx1, + double ddx0, + double ddx1, + double y0, + double y1, + double dy0, + double dy1, + double ddy0, + double ddy1 + ) { + this.x0 = x0; + this.x1 = x1; + this.dx0 = dx0; + this.dx1 = dx1; + this.ddx0 = ddx0; + this.ddx1 = ddx1; + + this.y0 = y0; + this.y1 = y1; + this.dy0 = dy0; + this.dy1 = dy1; + this.ddy0 = ddy0; + this.ddy1 = ddy1; + + computeCoefficients(); + } + + /** + * Re-arranges the spline into an at^5 + bt^4 + ... + f form for simpler computations + */ + private void computeCoefficients() { + ax = -6 * x0 - 3 * dx0 - 0.5 * ddx0 + 0.5 * ddx1 - 3 * dx1 + 6 * x1; + bx = 15 * x0 + 8 * dx0 + 1.5 * ddx0 - ddx1 + 7 * dx1 - 15 * x1; + cx = -10 * x0 - 6 * dx0 - 1.5 * ddx0 + 0.5 * ddx1 - 4 * dx1 + 10 * x1; + dx = 0.5 * ddx0; + ex = dx0; + fx = x0; + + ay = -6 * y0 - 3 * dy0 - 0.5 * ddy0 + 0.5 * ddy1 - 3 * dy1 + 6 * y1; + by = 15 * y0 + 8 * dy0 + 1.5 * ddy0 - ddy1 + 7 * dy1 - 15 * y1; + cy = -10 * y0 - 6 * dy0 - 1.5 * ddy0 + 0.5 * ddy1 - 4 * dy1 + 10 * y1; + dy = 0.5 * ddy0; + ey = dy0; + fy = y0; + } + + public Pose2d getStartPose() { + return new Pose2d(new Translation2d(x0, y0), new Rotation2d254(dx0, dy0, true)); + } + + public Pose2d getEndPose() { + return new Pose2d(new Translation2d(x1, y1), new Rotation2d254(dx1, dy1, true)); + } + + /** + * @param t ranges from 0 to 1 + * @return the point on the spline for that t value + */ + @Override + public Translation2d getPoint(double t) { + double x = + ax * t * t * t * t * t + bx * t * t * t * t + cx * t * t * t + dx * t * t + ex * t + fx; + double y = + ay * t * t * t * t * t + by * t * t * t * t + cy * t * t * t + dy * t * t + ey * t + fy; + return new Translation2d(x, y); + } + + private double dx(double t) { + return 5 * ax * t * t * t * t + 4 * bx * t * t * t + 3 * cx * t * t + 2 * dx * t + ex; + } + + private double dy(double t) { + return 5 * ay * t * t * t * t + 4 * by * t * t * t + 3 * cy * t * t + 2 * dy * t + ey; + } + + private double ddx(double t) { + return 20 * ax * t * t * t + 12 * bx * t * t + 6 * cx * t + 2 * dx; + } + + private double ddy(double t) { + return 20 * ay * t * t * t + 12 * by * t * t + 6 * cy * t + 2 * dy; + } + + private double dddx(double t) { + return 60 * ax * t * t + 24 * bx * t + 6 * cx; + } + + private double dddy(double t) { + return 60 * ay * t * t + 24 * by * t + 6 * cy; + } + + @Override + public double getVelocity(double t) { + return Math.hypot(dx(t), dy(t)); + } + + @Override + public double getCurvature(double t) { + return (dx(t) * ddy(t) - ddx(t) * dy(t)) + / ((dx(t) * dx(t) + dy(t) * dy(t)) * Math.sqrt((dx(t) * dx(t) + dy(t) * dy(t)))); + } + + @Override + public double getDCurvature(double t) { + double dx2dy2 = (dx(t) * dx(t) + dy(t) * dy(t)); + double num = (dx(t) * dddy(t) - dddx(t) * dy(t)) * dx2dy2 + - 3 * (dx(t) * ddy(t) - ddx(t) * dy(t)) * (dx(t) * ddx(t) + dy(t) * ddy(t)); + return num / (dx2dy2 * dx2dy2 * Math.sqrt(dx2dy2)); + } + + private double dCurvature2(double t) { + double dx2dy2 = (dx(t) * dx(t) + dy(t) * dy(t)); + double num = (dx(t) * dddy(t) - dddx(t) * dy(t)) * dx2dy2 + - 3 * (dx(t) * ddy(t) - ddx(t) * dy(t)) * (dx(t) * ddx(t) + dy(t) * ddy(t)); + return num * num / (dx2dy2 * dx2dy2 * dx2dy2 * dx2dy2 * dx2dy2); + } + + @Override + public Rotation2d254 getHeading(double t) { + return new Rotation2d254(dx(t), dy(t), true); + } + + /** + * @return integral of dCurvature^2 over the length of the spline + */ + private double sumDCurvature2() { + double dt = 1.0 / kSamples; + double sum = 0; + for (double t = 0; t < 1.0; t += dt) { + sum += (dt * dCurvature2(t)); + } + return sum; + } + + /** + * @return integral of dCurvature^2 over the length of multiple splines + */ + public static double sumDCurvature2(List splines) { + double sum = 0; + for (QuinticHermiteSpline s : splines) { + sum += s.sumDCurvature2(); + } + return sum; + } + + /** + * Makes optimization code a little more readable + */ + private static class ControlPoint { private double ddx, ddy; } + + /** + * Finds the optimal second derivative values for a set of splines to reduce the sum of the change + * in curvature squared over the path + * + * @param splines the list of splines to optimize + * @return the final sumDCurvature2 + */ + public static double optimizeSpline(List splines) { + int count = 0; + double prev = sumDCurvature2(splines); + while (count < kMaxIterations) { + runOptimizationIteration(splines); + double current = sumDCurvature2(splines); + if (prev - current < kMinDelta) + return current; + prev = current; + count++; + } + return prev; + } + + /** + * Runs a single optimization iteration + */ + private static void runOptimizationIteration(List splines) { + // can't optimize anything with less than 2 splines + if (splines.size() <= 1) { + return; + } + + ControlPoint[] controlPoints = new ControlPoint[splines.size() - 1]; + double magnitude = 0; + + for (int i = 0; i < splines.size() - 1; ++i) { + // don't try to optimize colinear points + if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) + || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) { + continue; + } + double original = sumDCurvature2(splines); + QuinticHermiteSpline temp, temp1; + + temp = splines.get(i); + temp1 = splines.get(i + 1); + controlPoints[i] = new ControlPoint(); // holds the gradient at a control point + + // calculate partial derivatives of sumDCurvature2 + splines.set( + i, + new QuinticHermiteSpline( + temp.x0, + temp.x1, + temp.dx0, + temp.dx1, + temp.ddx0, + temp.ddx1 + kEpsilon, + temp.y0, + temp.y1, + temp.dy0, + temp.dy1, + temp.ddy0, + temp.ddy1 + ) + ); + splines.set( + i + 1, + new QuinticHermiteSpline( + temp1.x0, + temp1.x1, + temp1.dx0, + temp1.dx1, + temp1.ddx0 + kEpsilon, + temp1.ddx1, + temp1.y0, + temp1.y1, + temp1.dy0, + temp1.dy1, + temp1.ddy0, + temp1.ddy1 + ) + ); + controlPoints[i].ddx = (sumDCurvature2(splines) - original) / kEpsilon; + splines.set( + i, + new QuinticHermiteSpline( + temp.x0, + temp.x1, + temp.dx0, + temp.dx1, + temp.ddx0, + temp.ddx1, + temp.y0, + temp.y1, + temp.dy0, + temp.dy1, + temp.ddy0, + temp.ddy1 + kEpsilon + ) + ); + splines.set( + i + 1, + new QuinticHermiteSpline( + temp1.x0, + temp1.x1, + temp1.dx0, + temp1.dx1, + temp1.ddx0, + temp1.ddx1, + temp1.y0, + temp1.y1, + temp1.dy0, + temp1.dy1, + temp1.ddy0 + kEpsilon, + temp1.ddy1 + ) + ); + controlPoints[i].ddy = (sumDCurvature2(splines) - original) / kEpsilon; + + splines.set(i, temp); + splines.set(i + 1, temp1); + magnitude += + controlPoints[i].ddx * controlPoints[i].ddx + controlPoints[i].ddy * controlPoints[i].ddy; + } + + magnitude = Math.sqrt(magnitude); + + // minimize along the direction of the gradient + // first calculate 3 points along the direction of the gradient + Translation2d p1, p2, p3; + p2 = new Translation2d(0, sumDCurvature2(splines)); // middle point is at the current location + + for (int i = 0; i < splines.size() - 1; + ++i) { // first point is offset from the middle location by -stepSize + if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) + || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) { + continue; + } + // normalize to step size + controlPoints[i].ddx *= kStepSize / magnitude; + controlPoints[i].ddy *= kStepSize / magnitude; + + // move opposite the gradient by step size amount + splines.get(i).ddx1 -= controlPoints[i].ddx; + splines.get(i).ddy1 -= controlPoints[i].ddy; + splines.get(i + 1).ddx0 -= controlPoints[i].ddx; + splines.get(i + 1).ddy0 -= controlPoints[i].ddy; + + // recompute the spline's coefficients to account for new second derivatives + splines.get(i).computeCoefficients(); + splines.get(i + 1).computeCoefficients(); + } + p1 = new Translation2d(-kStepSize, sumDCurvature2(splines)); + + for (int i = 0; i < splines.size() - 1; + ++i) { // last point is offset from the middle location by +stepSize + if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) + || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) { + continue; + } + // move along the gradient by 2 times the step size amount (to return to original location and + // move by 1 + // step) + splines.get(i).ddx1 += 2 * controlPoints[i].ddx; + splines.get(i).ddy1 += 2 * controlPoints[i].ddy; + splines.get(i + 1).ddx0 += 2 * controlPoints[i].ddx; + splines.get(i + 1).ddy0 += 2 * controlPoints[i].ddy; + + // recompute the spline's coefficients to account for new second derivatives + splines.get(i).computeCoefficients(); + splines.get(i + 1).computeCoefficients(); + } + + p3 = new Translation2d(kStepSize, sumDCurvature2(splines)); + + double stepSize = fitParabola( + p1, p2, p3 + ); // approximate step size to minimize sumDCurvature2 along the gradient + + for (int i = 0; i < splines.size() - 1; ++i) { + if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) + || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) { + continue; + } + // move by the step size calculated by the parabola fit (+1 to offset for the final + // transformation to find + // p3) + controlPoints[i].ddx *= 1 + stepSize / kStepSize; + controlPoints[i].ddy *= 1 + stepSize / kStepSize; + + splines.get(i).ddx1 += controlPoints[i].ddx; + splines.get(i).ddy1 += controlPoints[i].ddy; + splines.get(i + 1).ddx0 += controlPoints[i].ddx; + splines.get(i + 1).ddy0 += controlPoints[i].ddy; + + // recompute the spline's coefficients to account for new second derivatives + splines.get(i).computeCoefficients(); + splines.get(i + 1).computeCoefficients(); + } + } + + /** + * fits a parabola to 3 points + * + * @return the x coordinate of the vertex of the parabola + */ + private static double fitParabola(Translation2d p1, Translation2d p2, Translation2d p3) { + double A = + (p3.x() * (p2.y() - p1.y()) + p2.x() * (p1.y() - p3.y()) + p1.x() * (p3.y() - p2.y())); + double B = + (p3.x() * p3.x() * (p1.y() - p2.y()) + p2.x() * p2.x() * (p3.y() - p1.y()) + + p1.x() * p1.x() * (p2.y() - p3.y())); + return -B / (2 * A); + } +} diff --git a/src/main/java/com/team254/lib/spline/Spline.java b/src/main/java/com/team254/lib/spline/Spline.java new file mode 100755 index 0000000..cbe879b --- /dev/null +++ b/src/main/java/com/team254/lib/spline/Spline.java @@ -0,0 +1,33 @@ +package com.team254.lib.spline; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Pose2dWithCurvature; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; + +public abstract class Spline { + public abstract Translation2d getPoint(double t); + + public abstract Rotation2d254 getHeading(double t); + + public abstract double getCurvature(double t); + + // dk/dt + public abstract double getDCurvature(double t); + + // ds/dt + public abstract double getVelocity(double t); + + public Pose2d getPose2d(double t) { + return new Pose2d(getPoint(t), getHeading(t)); + } + + public Pose2dWithCurvature getPose2dWithCurvature(double t) { + return new Pose2dWithCurvature( + getPose2d(t), getCurvature(t), getDCurvature(t) / getVelocity(t) + ); + } + + // TODO add toString + // public abstract String toString(); +} diff --git a/src/main/java/com/team254/lib/spline/SplineGenerator.java b/src/main/java/com/team254/lib/spline/SplineGenerator.java new file mode 100755 index 0000000..51ef031 --- /dev/null +++ b/src/main/java/com/team254/lib/spline/SplineGenerator.java @@ -0,0 +1,123 @@ +package com.team254.lib.spline; + +import com.team254.lib.geometry.*; +import com.team254.lib.trajectory.TrajectoryPoint; +import java.util.ArrayList; +import java.util.List; + +public class SplineGenerator { + private static final double kMaxDX = 2.0; // inches + private static final double kMaxDY = 0.05; // inches + private static final double kMaxDTheta = 0.1; // radians! + private static final int kMinSampleSize = 1; + + /** + * Converts a spline into a list of Twist2d's. + * + * @param s the spline to parametrize + * @param t0 starting percentage of spline to parametrize + * @param t1 ending percentage of spline to parametrize + * @return list of Pose2dWithCurvature that approximates the original spline + */ + public static List> parameterizeSpline( + Spline s, + List headings, + double maxDx, + double maxDy, + double maxDTheta, + double t0, + double t1 + ) { + List> rv = new ArrayList<>(); + rv.add(new TrajectoryPoint<>(s.getPose2dWithCurvature(0.0), headings.get(0), 0)); + double dt = (t1 - t0); + for (double t = 0; t < t1; t += dt / kMinSampleSize) { + getSegmentArc(s, headings, rv, t, t + dt / kMinSampleSize, maxDx, maxDy, maxDTheta, dt); + } + return rv; + } + + /** + * Convenience function to parametrize a spline from t 0 to 1 + */ + public static List> parameterizeSpline( + Spline s, List headings + ) { + return parameterizeSpline(s, headings, kMaxDX, kMaxDY, kMaxDTheta, 0.0, 1.0); + } + + public static List> parameterizeSpline( + Spline s, List headings, double maxDx, double maxDy, double maxDTheta + ) { + return parameterizeSpline(s, headings, maxDx, maxDy, maxDTheta, 0.0, 1.0); + } + + public static List> parameterizeSplines( + List splines, List headings + ) { + return parameterizeSplines(splines, headings, kMaxDX, kMaxDY, kMaxDTheta); + } + + public static List> parameterizeSplines( + List splines, + List headings, + double maxDx, + double maxDy, + double maxDTheta + ) { + List> rv = new ArrayList<>(); + if (splines.isEmpty()) + return rv; + rv.add(new TrajectoryPoint<>( + splines.get(0).getPose2dWithCurvature(0.0), headings.get(0).getRotation(), 0 + )); + for (int i = 0; i < splines.size(); i++) { + Spline s = splines.get(i); + List spline_rots = new ArrayList<>(); + spline_rots.add(headings.get(i)); + spline_rots.add(headings.get(i + 1)); + + List> samples = + parameterizeSpline(s, spline_rots, maxDx, maxDy, maxDTheta); + samples.remove(0); + rv.addAll(samples); + } + return rv; + } + + private static void getSegmentArc( + Spline s, + List headings, + List> rv, + double t0, + double t1, + double maxDx, + double maxDy, + double maxDTheta, + double totalTime + ) { + Translation2d p0 = s.getPoint(t0); + Translation2d p1 = s.getPoint(t1); + Rotation2d254 r0 = s.getHeading(t0); + Rotation2d254 r1 = s.getHeading(t1); + Pose2d transformation = + new Pose2d(new Translation2d(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse())); + Twist2d twist = Pose2d.log(transformation); + + if (twist.dy > maxDy || twist.dx > maxDx || twist.dtheta > maxDTheta) { + getSegmentArc(s, headings, rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta, totalTime); + getSegmentArc(s, headings, rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta, totalTime); + } else { + // Interpolate heading + Rotation2d254 diff = headings.get(1).rotateBy(headings.get(0).inverse()); + if (diff.getRadians() > Math.PI) { + diff = diff.inverse().rotateBy(Rotation2d254.fromRadians(Math.PI)); + } + Rotation2d254 interpolated_heading = headings.get(0).rotateBy(diff.times(t1 / totalTime)); + + rv.add( + new TrajectoryPoint<>(s.getPose2dWithCurvature(t1), interpolated_heading, rv.size() - 1) + ); + } + } +} diff --git a/src/main/java/com/team254/lib/swerve/ChassisSpeeds.java b/src/main/java/com/team254/lib/swerve/ChassisSpeeds.java new file mode 100644 index 0000000..4c48539 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/ChassisSpeeds.java @@ -0,0 +1,95 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.team254.lib.swerve; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Twist2d; + +/** + * Represents the speed of a robot chassis. Although this struct contains similar members compared + * to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose + * w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to + * the robot frame of reference. + * + *

A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy + * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum + * will often have all three components. + */ +@SuppressWarnings("MemberName") +public class ChassisSpeeds { + /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */ + public double vxMetersPerSecond; + + /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */ + public double vyMetersPerSecond; + + /** Represents the angular velocity of the robot frame. (CCW is +) */ + public double omegaRadiansPerSecond; + + /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */ + public ChassisSpeeds() {} + + /** + * Constructs a ChassisSpeeds object. + * + * @param vxMetersPerSecond Forward velocity. + * @param vyMetersPerSecond Sideways velocity. + * @param omegaRadiansPerSecond Angular velocity. + */ + public ChassisSpeeds( + double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond + ) { + this.vxMetersPerSecond = vxMetersPerSecond; + this.vyMetersPerSecond = vyMetersPerSecond; + this.omegaRadiansPerSecond = omegaRadiansPerSecond; + } + + /** + * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds + * object. + * + * @param vxMetersPerSecond The component of speed in the x direction relative to the field. + * Positive x is away from your alliance wall. + * @param vyMetersPerSecond The component of speed in the y direction relative to the field. + * Positive y is to your left when standing behind the alliance wall. + * @param omegaRadiansPerSecond The angular rate of the robot. + * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is + * considered to be zero when it is facing directly away from your alliance station wall. + * Remember that this should be CCW positive. + * @return ChassisSpeeds object representing the speeds in the robot's frame of reference. + */ + public static ChassisSpeeds fromFieldRelativeSpeeds( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond, + Rotation2d254 robotAngle + ) { + return new ChassisSpeeds( + vxMetersPerSecond * robotAngle.cos() + vyMetersPerSecond * robotAngle.sin(), + -vxMetersPerSecond * robotAngle.sin() + vyMetersPerSecond * robotAngle.cos(), + omegaRadiansPerSecond + ); + } + + public static ChassisSpeeds fromRobotRelativeSpeeds( + double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond + ) { + return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); + } + + public Twist2d toTwist2d() { + return new Twist2d(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); + } + + @Override + public String toString() { + return String.format( + "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", + vxMetersPerSecond, + vyMetersPerSecond, + omegaRadiansPerSecond + ); + } +} diff --git a/src/main/java/com/team254/lib/swerve/DriveInput.java b/src/main/java/com/team254/lib/swerve/DriveInput.java new file mode 100644 index 0000000..361c246 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/DriveInput.java @@ -0,0 +1,97 @@ +package com.team254.lib.swerve; + +import com.team254.lib.control.RadiusController; +import com.team254.lib.control.SwerveHeadingController; +import com.team254.lib.util.LatchedBoolean; +import com.team254.lib.util.TimeDelayedBoolean; +import com.team254.lib.util.ToggleBoolean; +import com.team254.lib.util.Util; + +public class DriveInput { + private double throttle; + private double strafe; + private double rotation; + private double desiredCardinalHeading; + private boolean snapToOrigin; + private boolean engagePolarDrive; + + private LatchedBoolean mShouldChangeToMaintain = new LatchedBoolean(); + private LatchedBoolean mShouldChangeToMaintainRadius = new LatchedBoolean(); + private ToggleBoolean mShouldMaintainPolarHeading = new ToggleBoolean(); + private LatchedBoolean mShouldEngagePolarDrive = + new LatchedBoolean(); // use ToggleBoolean above if using gamepad + private TimeDelayedBoolean mShouldMaintainAzimuth = new TimeDelayedBoolean(); + private TimeDelayedBoolean mShouldSnapToOrigin = new TimeDelayedBoolean(); + private TimeDelayedBoolean mShouldMaintainRadius = new TimeDelayedBoolean(); + + private final double kTimeDelayedBooleanTimeout = 0.1; + + public DriveInput() { + throttle = 0; + strafe = 0; + rotation = 0; + desiredCardinalHeading = -1; + snapToOrigin = false; + engagePolarDrive = false; + } + + public void setDriveInput( + double x, + double y, + double omega, + double cardinalHeading, + boolean shouldSnapToOrigin, + boolean shouldEngagePolarDrive + ) { + throttle = x; + strafe = y; + rotation = omega; + desiredCardinalHeading = cardinalHeading; + snapToOrigin = shouldSnapToOrigin; + engagePolarDrive = shouldEngagePolarDrive; + } + + public double getThrottle() { + return throttle; + } + + public double getStrafe() { + return strafe; + } + + public double getRotation() { + return rotation; + } + + public double getDesiredCardinalHeading() { + return desiredCardinalHeading; + } + + public boolean maintainHeading() { + return mShouldMaintainAzimuth.update(rotation == 0, kTimeDelayedBooleanTimeout); + } + + public boolean changeHeadingSetpoint() { + return mShouldChangeToMaintain.update(maintainHeading()); + } + + public boolean maintainTargetHeading() { + return mShouldSnapToOrigin.update(!snapToOrigin, kTimeDelayedBooleanTimeout); + } + + public boolean maintainRadius() { + return mShouldMaintainRadius.update(throttle == 0, kTimeDelayedBooleanTimeout); + } + + public boolean changeRadiusSetpoint() { + return mShouldChangeToMaintainRadius.update(maintainRadius()); + } + + public boolean changeToMaintainTargetHeading() { + return mShouldMaintainPolarHeading.update(maintainTargetHeading()); + } + + public boolean shouldEngagePolarDrive() { + return mShouldEngagePolarDrive.update(engagePolarDrive); + } +} diff --git a/src/main/java/com/team254/lib/swerve/FieldRelativeController.java b/src/main/java/com/team254/lib/swerve/FieldRelativeController.java new file mode 100644 index 0000000..2eb4e2a --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/FieldRelativeController.java @@ -0,0 +1,60 @@ +package com.team254.lib.swerve; + +// import com.team254.frc2022.Constants; +// import com.team254.frc2022.Robot; +// import com.team254.frc2022.RobotState; +import com.team254.lib.control.RadiusController; +import com.team254.lib.control.SwerveHeadingController; +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.swerve.IDriveController; + +public class FieldRelativeController implements IDriveController { + public static FieldRelativeController mInstance; + public SwerveHeadingController mSwerveHeadingController = SwerveHeadingController.getInstance(); + public RadiusController mRadiusController = RadiusController.getInstance(); + + // private RobotState mRobotState = RobotState.getInstance(); + + public static FieldRelativeController getInstance() { + if (mInstance == null) { + mInstance = new FieldRelativeController(); + } + return mInstance; + } + + @Override + public ChassisSpeeds transform(DriveInput driveInput, Pose2d robotPose) { + // mRadiusController.setRadiusControllerState(RadiusController.RadiusControllerState.OFF); + // if ((mSwerveHeadingController.getHeadingControllerState() + // == SwerveHeadingController.HeadingControllerState.SNAP + // && mSwerveHeadingController.isAtGoal()) + // || driveInput.changeHeadingSetpoint()) { + // mSwerveHeadingController.setHeadingControllerState( + // SwerveHeadingController.HeadingControllerState.MAINTAIN + // ); + // mSwerveHeadingController.setGoal( + // mRobotState.getLatestFieldToVehicle().getValue().getRotation().getDegrees() + // ); + // return ChassisSpeeds.fromFieldRelativeSpeeds( + // driveInput.getThrottle() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // driveInput.getStrafe() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // mSwerveHeadingController.update(robotPose.getRotation().getDegrees()) + // * Constants.kMaxAngularVelocityRadiansPerSecond, + // robotPose.getRotation() + // ); + // } + // return ChassisSpeeds.fromFieldRelativeSpeeds( + // driveInput.getThrottle() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // driveInput.getStrafe() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // driveInput.getRotation() * Constants.kMaxAngularVelocityRadiansPerSecond + // * Constants.kScaleRotationInputs, + // robotPose.getRotation() + // ); + + return new ChassisSpeeds(); + } +} diff --git a/src/main/java/com/team254/lib/swerve/HeadingLockedFieldRelativeController.java b/src/main/java/com/team254/lib/swerve/HeadingLockedFieldRelativeController.java new file mode 100644 index 0000000..4711651 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/HeadingLockedFieldRelativeController.java @@ -0,0 +1,38 @@ +package com.team254.lib.swerve; + +// import com.team254.frc2022.Constants; +import com.team254.lib.control.RadiusController; +import com.team254.lib.control.SwerveHeadingController; +import com.team254.lib.geometry.Pose2d; + +public class HeadingLockedFieldRelativeController implements IDriveController { + public static HeadingLockedFieldRelativeController mInstance; + public SwerveHeadingController mSwerveHeadingController = SwerveHeadingController.getInstance(); + public RadiusController mRadiusController = RadiusController.getInstance(); + + public static HeadingLockedFieldRelativeController getInstance() { + if (mInstance == null) { + mInstance = new HeadingLockedFieldRelativeController(); + } + return mInstance; + } + + @Override + public ChassisSpeeds transform(DriveInput driveInput, Pose2d robotPose) { + mRadiusController.setRadiusControllerState(RadiusController.RadiusControllerState.OFF); + mSwerveHeadingController.setHeadingControllerState( + SwerveHeadingController.HeadingControllerState.SNAP + ); + mSwerveHeadingController.setGoal(driveInput.getDesiredCardinalHeading()); + // return ChassisSpeeds.fromFieldRelativeSpeeds( + // driveInput.getThrottle() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // driveInput.getStrafe() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // mSwerveHeadingController.update(robotPose.getRotation().getDegrees()) + // * Constants.kMaxAngularVelocityRadiansPerSecond, + // robotPose.getRotation() + // ); + return new ChassisSpeeds(); + } +} diff --git a/src/main/java/com/team254/lib/swerve/IDriveController.java b/src/main/java/com/team254/lib/swerve/IDriveController.java new file mode 100644 index 0000000..38a245d --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/IDriveController.java @@ -0,0 +1,7 @@ +package com.team254.lib.swerve; + +import com.team254.lib.geometry.Pose2d; + +public interface IDriveController { + ChassisSpeeds transform(DriveInput driveInput, Pose2d robotPose); +} diff --git a/src/main/java/com/team254/lib/swerve/SwerveDriveKinematics.java b/src/main/java/com/team254/lib/swerve/SwerveDriveKinematics.java new file mode 100644 index 0000000..83dbc07 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/SwerveDriveKinematics.java @@ -0,0 +1,246 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.team254.lib.swerve; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import java.util.Arrays; +import java.util.Collections; +import org.ejml.simple.SimpleMatrix; + +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual + * module states (speed and angle). + * + *

The inverse kinematics (converting from a desired chassis velocity to individual module + * states) uses the relative locations of the modules with respect to the center of rotation. The + * center of rotation for inverse kinematics is also variable. This means that you can set your set + * your center of rotation in a corner of the robot to perform special evasion maneuvers. + * + *

Forward kinematics (converting an array of module states into the overall chassis motion) is + * performs the exact opposite of what inverse kinematics does. Since this is an overdetermined + * system (more equations than variables), we use a least-squares approximation. + * + *

The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the + * Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our + * chassis speeds. + * + *

Forward kinematics is also used for odometry -- determining the position of the robot on the + * field using encoders and a gyro. + */ +public class SwerveDriveKinematics { + private final SimpleMatrix m_inverseKinematics; + private final SimpleMatrix m_forwardKinematics; + + private final int m_numModules; + private final Translation2d[] m_modules; + private Translation2d m_prevCoR = new Translation2d(); + private final Rotation2d254[] m_rotations; + + /** + * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations + * as Translation2ds. The order in which you pass in the wheel locations is the same order that + * you will receive the module states when performing inverse kinematics. It is also expected that + * you pass in the module states in the same order when calling the forward kinematics methods. + * + * @param wheelsMeters The locations of the wheels relative to the physical center of the robot. + */ + public SwerveDriveKinematics(Translation2d... wheelsMeters) { + if (wheelsMeters.length < 2) { + throw new IllegalArgumentException("A swerve drive requires at least two modules"); + } + m_numModules = wheelsMeters.length; + m_modules = Arrays.copyOf(wheelsMeters, m_numModules); + m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); + m_rotations = new Rotation2d254[m_numModules]; + + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].y()); + m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].x()); + m_rotations[i] = new Rotation2d254(m_modules[i].x(), m_modules[i].y(), true); + } + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + } + + /** + * Performs inverse kinematics to return the module states from a desired chassis velocity. This + * method is often used to convert joystick values into module speeds and angles. + * + *

This function also supports variable centers of rotation. During normal operations, the + * center of rotation is usually the same as the physical center of the robot; therefore, the + * argument is defaulted to that use case. However, if you wish to change the center of rotation + * for evasive maneuvers, vision alignment, or for any other use case, you can do so. + * + * @param chassisSpeeds The desired chassis speed. + * @param centerOfRotationMeters The center of rotation. For example, if you set the center of + * rotation at one corner of the robot and provide a chassis speed that only has a dtheta + * component, the robot will rotate around that corner. + * @return An array containing the module states. Use caution because these module states are not + * normalized. Sometimes, a user input may cause one of the module speeds to go above the + * attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double) + * DesaturateWheelSpeeds} function to rectify this issue. + */ + @SuppressWarnings("LocalVariableName") + public SwerveModuleState[] toSwerveModuleStates( + ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters + ) { + if (!centerOfRotationMeters.equals(m_prevCoR)) { + for (int i = 0; i < m_numModules; i++) { + m_inverseKinematics.setRow( + i * 2 + 0, + 0, /* Start Data */ + 1, + 0, + -m_modules[i].y() + centerOfRotationMeters.y() + ); + m_inverseKinematics.setRow( + i * 2 + 1, + 0, /* Start Data */ + 0, + 1, + +m_modules[i].x() - centerOfRotationMeters.x() + ); + } + m_prevCoR = centerOfRotationMeters; + } + + var chassisSpeedsVector = new SimpleMatrix(3, 1); + chassisSpeedsVector.setColumn( + 0, + 0, + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond + ); + + var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); + SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules]; + + for (int i = 0; i < m_numModules; i++) { + double x = moduleStatesMatrix.get(i * 2, 0); + double y = moduleStatesMatrix.get(i * 2 + 1, 0); + + double speed = Math.hypot(x, y); + Rotation2d254 angle = new Rotation2d254(x, y, true); + + moduleStates[i] = new SwerveModuleState(speed, angle); + } + + return moduleStates; + } + + /** + * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)} + * toSwerveModuleStates for more information. + * + * @param chassisSpeeds The desired chassis speed. + * @return An array containing the module states. + */ + public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) { + return toSwerveModuleStates(chassisSpeeds, new Translation2d()); + } + + /** + * Performs forward kinematics to return the resulting chassis state from the given module states. + * This method is often used for odometry -- determining the robot's position on the field using + * data from the real-world speed and angle of each module on the robot. + * + * @param wheelStates The state of the modules (as a SwerveModuleState type) as measured from + * respective encoders and gyros. The order of the swerve module states should be same as + * passed into the constructor of this class. + * @return The resulting chassis speed. + */ + public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) { + if (wheelStates.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor" + ); + } + var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1); + + for (int i = 0; i < m_numModules; i++) { + var module = wheelStates[i]; + moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.cos()); + moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.sin()); + } + + var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix); + return new ChassisSpeeds( + chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0), chassisSpeedsVector.get(2, 0) + ); + } + + public ChassisSpeeds toChasisSpeedWheelConstraints(SwerveModuleState... wheelStates) { + if (wheelStates.length != m_numModules) { + throw new IllegalArgumentException( + "Number of modules is not consistent with number of wheel locations provided in " + + "constructor" + ); + } + var constraintsMatrix = new SimpleMatrix(m_numModules * 2, 3); + for (int i = 0; i < m_numModules; i++) { + var module = wheelStates[i]; + + var beta = module.angle.rotateBy(m_rotations[i].inverse()) + .rotateBy(Rotation2d254.fromRadians(Math.PI / 2.0)); + + // System.out.println(module); + constraintsMatrix.setRow( + i * 2, 0, module.angle.cos(), module.angle.sin(), -m_modules[i].norm() * beta.cos() + ); + constraintsMatrix.setRow( + i * 2 + 1, 0, -module.angle.sin(), module.angle.cos(), m_modules[i].norm() * beta.sin() + ); + } + // System.out.println(constraintsMatrix); + + var psuedoInv = constraintsMatrix.pseudoInverse(); + + var enforcedConstraints = new SimpleMatrix(m_numModules * 2, 1); + for (int i = 0; i < m_numModules; i++) { + enforcedConstraints.setRow(i * 2, 0, wheelStates[i].speedMetersPerSecond); + enforcedConstraints.setRow(i * 2 + 1, 0, 0); + } + // System.out.println(enforcedConstraints); + + var chassisSpeedsVector = psuedoInv.mult(enforcedConstraints); + return new ChassisSpeeds( + chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0), chassisSpeedsVector.get(2, 0) + ); + } + + /** + * Renormalizes the wheel speeds if any individual speed is above the specified maximum. + * + *

Sometimes, after inverse kinematics, the requested speed from one or more modules may be + * above the max attainable speed for the driving motor on that module. To fix this issue, one can + * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the + * absolute threshold, while maintaining the ratio of speeds between modules. + * + * @param moduleStates Reference to array of module states. The array will be mutated with the + * normalized speeds! + * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach. + */ + public static void desaturateWheelSpeeds( + SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond + ) { + double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond; + if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { + for (SwerveModuleState moduleState : moduleStates) { + moduleState.speedMetersPerSecond = + moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; + } + } + } + + public final Translation2d[] getModuleLocations() { + return m_modules; + } + + public int getNumModules() { + return m_numModules; + } +} diff --git a/src/main/java/com/team254/lib/swerve/SwerveDriveOdometry.java b/src/main/java/com/team254/lib/swerve/SwerveDriveOdometry.java new file mode 100644 index 0000000..cf8b94d --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/SwerveDriveOdometry.java @@ -0,0 +1,198 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.team254.lib.swerve; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Twist2d; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field + * over a course of a match using readings from your swerve drive encoders and swerve azimuth + * encoders. + * + *

Teams can use odometry during the autonomous period for complex tasks like path following. + * Furthermore, odometry can be used for latency compensation when using computer-vision systems. + */ +public class SwerveDriveOdometry { + private final SwerveDriveKinematics m_kinematics; + private Pose2d m_poseMeters; + private ChassisSpeeds m_velocity; + private double m_prevTimeSeconds = -1; + + private Rotation2d254 m_previousAngle; + private double[] m_previousDistances; + + /** + * Constructs a SwerveDriveOdometry object. + * + * @param kinematics The swerve drive kinematics for your drivetrain. + * @param initialPose The starting position of the robot on the field. + */ + public SwerveDriveOdometry( + SwerveDriveKinematics kinematics, Pose2d initialPose, double... previousDistances + ) { + m_kinematics = kinematics; + m_velocity = new ChassisSpeeds(); + m_poseMeters = initialPose; + m_previousAngle = initialPose.getRotation(); + m_previousDistances = previousDistances; + } + + public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Pose2d initialPose) { + this(kinematics, initialPose, new double[kinematics.getNumModules()]); + } + + /** + * Constructs a SwerveDriveOdometry object with the default pose at the origin. + * + * @param kinematics The swerve drive kinematics for your drivetrain. + */ + public SwerveDriveOdometry(SwerveDriveKinematics kinematics) { + this(kinematics, new Pose2d(), new double[kinematics.getNumModules()]); + } + + /** + * Resets the robot's position on the field. + * + *

The gyroscope angle does not need to be reset here on the user's robot code. The library + * automatically takes care of offsetting the gyro angle. + * + * @param pose The position on the field that your robot is at. + */ + public void resetPosition(Pose2d pose, double... previousDistances) { + m_previousDistances = previousDistances; + resetPosition(pose); + } + + public void resetPosition(Pose2d pose) { + m_velocity = new ChassisSpeeds(); + m_poseMeters = pose; + m_previousAngle = pose.getRotation(); + } + + /** + * Returns the position of the robot on the field. + * + * @return The pose of the robot (x and y are in meters). + */ + public Pose2d getPoseMeters() { + return m_poseMeters; + } + + public ChassisSpeeds getVelocity() { + return m_velocity; + } + + public Pose2d getPoseMetersPolar() { + return new Pose2d( + Math.sqrt( + Math.pow(m_poseMeters.getTranslation().x(), 2) + + Math.pow(m_poseMeters.getTranslation().y(), 2) + ), + Math.toDegrees( + Math.atan2(m_poseMeters.getTranslation().y(), m_poseMeters.getTranslation().x()) + ), + m_poseMeters.getRotation() + ); + } + + /** + * Updates the robot's position on the field using forward kinematics and integration of the pose + * over time. This method takes in the current time as a parameter to calculate period (difference + * between two timestamps). The period is used to calculate the change in distance from a + * velocity. This also takes in an angle parameter which is used instead of the angular rate that + * is calculated from forward kinematics. + * + * @param currentTimeSeconds The current time in seconds. + * @param gyroAngle The angle reported by the gyroscope. + * @param moduleStates The current state of all swerve modules. Please provide the states in the + * same order in which you instantiated your SwerveDriveKinematics. + * @return The new pose of the robot. + */ + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d254 gyroAngle, SwerveModuleState... moduleStates + ) { + double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; + m_prevTimeSeconds = currentTimeSeconds; + + var angle = gyroAngle; + + var chassisState = m_kinematics.toChassisSpeeds(moduleStates); + var newPose = Pose2d.exp(new Twist2d( + chassisState.vxMetersPerSecond * period, + chassisState.vyMetersPerSecond * period, + angle.rotateBy(m_previousAngle.inverse()).getRadians() + )); + m_previousAngle = angle; + m_poseMeters = new Pose2d(m_poseMeters.transformBy(newPose).getTranslation(), angle); + return m_poseMeters; + } + + public Pose2d updateWithWheelConstraints( + double currentTimeSeconds, Rotation2d254 gyroAngle, SwerveModuleState... moduleStates + ) { + double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0; + // System.out.println("Time: " + currentTimeSeconds); + m_prevTimeSeconds = currentTimeSeconds; + + var angle = gyroAngle; + var chassisState = m_kinematics.toChasisSpeedWheelConstraints(moduleStates); + + var idealStates = m_kinematics.toSwerveModuleStates(chassisState); + + // Project along ideal angles. + double average = 0.0; + for (int i = 0; i < moduleStates.length; ++i) { + double ratio = moduleStates[i].angle.rotateBy(idealStates[i].angle.inverse()).cos() + * (moduleStates[i].distanceMeters - m_previousDistances[i]) + / (idealStates[i].speedMetersPerSecond * period); + if (Double.isNaN(ratio) || Double.isInfinite(ratio) + || Math.abs(idealStates[i].speedMetersPerSecond) < 0.01) { + ratio = 1.0; + } + average = average + ratio; + m_previousDistances[i] = moduleStates[i].distanceMeters; + } + average = average / 4.0; + + // System.out.println(chassisState); + SmartDashboard.putNumber("average", average); + + var newPose = Pose2d.exp(new Twist2d( + chassisState.vxMetersPerSecond * period * average, + chassisState.vyMetersPerSecond * period * average, + chassisState.omegaRadiansPerSecond * period * average + )); + // System.out.println("Translation: " + newPose); + m_velocity = chassisState; + // m_velocity.omegaRadiansPerSecond = m_previousAngle.inverse().rotateBy(gyroAngle).getRadians() + // / period; + m_poseMeters = new Pose2d(m_poseMeters.transformBy(newPose).getTranslation(), angle); + m_previousAngle = angle; + // System.out.println(m_poseMeters); + return m_poseMeters; + } + + /** + * Updates the robot's position on the field using forward kinematics and integration of the pose + * over time. This method automatically calculates the current time to calculate period + * (difference between two timestamps). The period is used to calculate the change in distance + * from a velocity. This also takes in an angle parameter which is used instead of the angular + * rate that is calculated from forward kinematics. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param moduleStates The current state of all swerve modules. Please provide the states in the + * same order in which you instantiated your SwerveDriveKinematics. + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d254 gyroAngle, SwerveModuleState[] moduleStates) { + return updateWithWheelConstraints(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates); + } +} diff --git a/src/main/java/com/team254/lib/swerve/SwerveModuleState.java b/src/main/java/com/team254/lib/swerve/SwerveModuleState.java new file mode 100644 index 0000000..15c09e4 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/SwerveModuleState.java @@ -0,0 +1,109 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.team254.lib.swerve; + +import com.team254.lib.geometry.Rotation2d254; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Objects; + +/** Represents the state of one swerve module. */ +@SuppressWarnings("MemberName") +public class SwerveModuleState implements Comparable { + /** Speed of the wheel of the module. */ + public double speedMetersPerSecond; + + /** Displacement of the wheel of the module. */ + public double distanceMeters; + + /** Angle of the module. */ + public Rotation2d254 angle = Rotation2d254.fromDegrees(0); + + public edu.wpi.first.math.kinematics.SwerveModuleState getWPI() { + return new edu.wpi.first.math.kinematics.SwerveModuleState( + speedMetersPerSecond, Rotation2d.fromDegrees(angle.getDegrees()) + ); + } + + /** Constructs a SwerveModuleState with zeros for speed and angle. */ + public SwerveModuleState() {} + + /** + * Constructs a SwerveModuleState. + * + * @param speedMetersPerSecond The speed of the wheel of the module. + * @param angle The angle of the module. + */ + public SwerveModuleState(double speedMetersPerSecond, Rotation2d254 angle) { + this.speedMetersPerSecond = speedMetersPerSecond; + this.angle = angle; + } + + public SwerveModuleState( + double speedMetersPerSecond, double distanceMeters, Rotation2d254 angle + ) { + this.speedMetersPerSecond = speedMetersPerSecond; + this.distanceMeters = distanceMeters; + this.angle = angle; + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof edu.wpi.first.math.kinematics.SwerveModuleState) { + return Double.compare( + speedMetersPerSecond, + ((edu.wpi.first.math.kinematics.SwerveModuleState) obj).speedMetersPerSecond + ) + == 0; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(speedMetersPerSecond); + } + + /** + * Compares two swerve module states. One swerve module is "greater" than the other if its speed + * is higher than the other. + * + * @param other The other swerve module. + * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. + */ + @Override + public int compareTo(SwerveModuleState other) { + return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond); + } + + @Override + public String toString() { + return String.format( + "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle + ); + } + + /** + * Minimize the change in heading the desired swerve module state would require by potentially + * reversing the direction the wheel spins. If this is used with the PIDController class's + * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees. + * + * @param desiredState The desired state. + * @param currentAngle The current module angle. + * @return Optimized swerve module state. + */ + public static SwerveModuleState optimize( + SwerveModuleState desiredState, Rotation2d254 currentAngle + ) { + var delta = desiredState.angle.rotateBy(currentAngle.inverse()); // todo check math + if (Math.abs(delta.getDegrees()) > 90.0) { + return new SwerveModuleState( + -desiredState.speedMetersPerSecond, + desiredState.angle.rotateBy(Rotation2d254.fromDegrees(180.0)) + ); + } else { + return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); + } + } +} diff --git a/src/main/java/com/team254/lib/swerve/SwerveSetpoint.java b/src/main/java/com/team254/lib/swerve/SwerveSetpoint.java new file mode 100644 index 0000000..fb7dba9 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/SwerveSetpoint.java @@ -0,0 +1,20 @@ +package com.team254.lib.swerve; + +public class SwerveSetpoint { + public ChassisSpeeds mChassisSpeeds; + public SwerveModuleState[] mModuleStates; + + public SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] initialStates) { + this.mChassisSpeeds = chassisSpeeds; + this.mModuleStates = initialStates; + } + + @Override + public String toString() { + String ret = mChassisSpeeds.toString() + "\n"; + for (int i = 0; i < mModuleStates.length; ++i) { + ret += " " + mModuleStates[i].toString() + "\n"; + } + return ret; + } +} diff --git a/src/main/java/com/team254/lib/swerve/SwerveSetpointGenerator.java b/src/main/java/com/team254/lib/swerve/SwerveSetpointGenerator.java new file mode 100644 index 0000000..1ceac75 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/SwerveSetpointGenerator.java @@ -0,0 +1,387 @@ +package com.team254.lib.swerve; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.geometry.Twist2d; +import com.team254.lib.util.Util; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +/** + * Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path + * follower), and outputs a new setpoint that respects all of the kinematic constraints on module + * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the + * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is + * kinematically infeasible (and can result in wheel slip or robot heading drift as a result). + */ +public class SwerveSetpointGenerator { + public static class KinematicLimits { + public double kMaxDriveVelocity; // m/s + public double kMaxDriveAcceleration; // m/s^2 + public double kMaxSteeringVelocity; // rad/s + } + + private final SwerveDriveKinematics mKinematics; + + public SwerveSetpointGenerator(final SwerveDriveKinematics kinematics) { + this.mKinematics = kinematics; + } + + /** + * Check if it would be faster to go to the opposite of the goal heading (and reverse drive + * direction). + * + * @param prevToGoal The rotation from the previous state to the goal state (i.e. + * prev.inverse().rotateBy(goal)). + * @return True if the shortest path to achieve this rotation involves flipping the drive + * direction. + */ + private boolean flipHeading(Rotation2d254 prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + public double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left + ) { + if (iterations_left < 0 || Util.epsilonEquals(f_0, f_1)) { + return 1.0; + } + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + protected double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation, + int max_iterations + ) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = (x, y) -> { + return unwrapAngle(f_0, Math.atan2(y, x)) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + protected double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step, + int max_iterations + ) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = (x, y) -> { + return Math.hypot(x, y) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + protected double findDriveMaxS( + double x_0, double y_0, double x_1, double y_1, double max_vel_step + ) { + // Our drive velocity between s=0 and s=1 is quadratic in s: + // v^2 = ((x_1 - x_0) * s + x_0)^2 + ((y_1 - y_0) * s + y_0)^2 + // = a * s^2 + b * s + c + // Where: + // a = (x_1 - x_0)^2 + (y_1 - y_0)^2 + // b = 2 * x_0 * (x_1 - x_0) + 2 * y_0 * (y_1 - y_0) + // c = x_0^2 + y_0^2 + // We want to find where this quadratic results in a velocity that is > max_vel_step from our + // velocity at s=0: sqrt(x_0^2 + y_0^2) +/- max_vel_step = ...quadratic... + final double dx = x_1 - x_0; + final double dy = y_1 - y_0; + final double a = dx * dx + dy * dy; + final double b = 2.0 * x_0 * dx + 2.0 * y_0 * dy; + final double c = x_0 * x_0 + y_0 * y_0; + final double v_limit_upper_2 = Math.pow(Math.hypot(x_0, y_0) + max_vel_step, 2.0); + final double v_limit_lower_2 = Math.pow(Math.hypot(x_0, y_0) - max_vel_step, 2.0); + return 0.0; + } + + /** + * Generate a new setpoint. + * + * @param limits The kinematic limits to respect for this setpoint. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual + * measured/estimated kinematic state. + * @param desiredState The desired state of motion, such as from the driver sticks or a path + * following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all of the KinematicLimits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final KinematicLimits limits, + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredState, + double dt + ) { + final Translation2d[] modules = mKinematics.getModuleLocations(); + + SwerveModuleState[] desiredModuleState = mKinematics.toSwerveModuleStates(desiredState); + // Make sure desiredState respects velocity limits. + if (limits.kMaxDriveVelocity > 0.0) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.kMaxDriveVelocity); + desiredState = mKinematics.toChassisSpeeds(desiredModuleState); + } + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (desiredState.toTwist2d().epsilonEquals(Twist2d.identity(), Util.kEpsilon)) { + need_to_steer = false; + for (int i = 0; i < modules.length; ++i) { + desiredModuleState[i].angle = prevSetpoint.mModuleStates[i].angle; + desiredModuleState[i].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[modules.length]; + double[] prev_vy = new double[modules.length]; + Rotation2d254[] prev_heading = new Rotation2d254[modules.length]; + double[] desired_vx = new double[modules.length]; + double[] desired_vy = new double[modules.length]; + Rotation2d254[] desired_heading = new Rotation2d254[modules.length]; + boolean all_modules_should_flip = true; + for (int i = 0; i < modules.length; ++i) { + prev_vx[i] = prevSetpoint.mModuleStates[i].angle.cos() + * prevSetpoint.mModuleStates[i].speedMetersPerSecond; + prev_vy[i] = prevSetpoint.mModuleStates[i].angle.sin() + * prevSetpoint.mModuleStates[i].speedMetersPerSecond; + prev_heading[i] = prevSetpoint.mModuleStates[i].angle; + if (prevSetpoint.mModuleStates[i].speedMetersPerSecond < 0.0) { + prev_heading[i] = prev_heading[i].flip(); + } + desired_vx[i] = + desiredModuleState[i].angle.cos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = + desiredModuleState[i].angle.sin() * desiredModuleState[i].speedMetersPerSecond; + desired_heading[i] = desiredModuleState[i].angle; + if (desiredModuleState[i].speedMetersPerSecond < 0.0) { + desired_heading[i] = desired_heading[i].flip(); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[i].inverse().rotateBy(desired_heading[i]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !prevSetpoint.mChassisSpeeds.toTwist2d().epsilonEquals(Twist2d.identity(), Util.kEpsilon) + && !desiredState.toTwist2d().epsilonEquals(Twist2d.identity(), Util.kEpsilon)) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired angle, and accelerate again. + return generateSetpoint(limits, prevSetpoint, new ChassisSpeeds(), dt); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then find the amount we can move from start towards goal in this cycle such + // that no kinematic limit is exceeded. + double dx = desiredState.vxMetersPerSecond - prevSetpoint.mChassisSpeeds.vxMetersPerSecond; + double dy = desiredState.vyMetersPerSecond - prevSetpoint.mChassisSpeeds.vyMetersPerSecond; + double dtheta = + desiredState.omegaRadiansPerSecond - prevSetpoint.mChassisSpeeds.omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since inverse kinematics doesn't care about angle, we can be opportunistically + // lazy). + List> overrideSteering = new ArrayList<>(modules.length); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, and then backing out the maximum interpolant between start and goal + // states. We remember the minimum across all modules, since that is the active constraint. + final double max_theta_step = dt * limits.kMaxSteeringVelocity; + for (int i = 0; i < modules.length; ++i) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.mModuleStates[i].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + if (Util.epsilonEquals(prevSetpoint.mModuleStates[i].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based purely on rotation in place. + if (Util.epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(i, Optional.of(prevSetpoint.mModuleStates[i].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint.mModuleStates[i].angle.inverse().rotateBy(desiredModuleState[i].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d254.kPi); + } + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); + // Don't limit the global min_s; + continue; + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + i, + Optional.of(prevSetpoint.mModuleStates[i].angle.rotateBy(Rotation2d254.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step + ))) + ); + min_s = 0.0; + continue; + } + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + final int kMaxIterations = 8; + double s = findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations + ); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel acceleration limits. + final double max_vel_step = dt * limits.kMaxDriveAcceleration; + for (int i = 0; i < modules.length; ++i) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + double vx_min_s = + min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = + min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster than that. + // TODO(for efficiency, do all this on v^2 to save a bunch of sqrts) + // TODO(be smarter about root finding, since this is just a quadratic in s: + // ((xf-x0)*s+x0)^2+((yf-y0)*s+y0)^2) + final int kMaxIterations = 10; + double s = min_s + * findDriveMaxS( + prev_vx[i], + prev_vy[i], + Math.hypot(prev_vx[i], prev_vy[i]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + max_vel_step, + kMaxIterations + ); + min_s = Math.min(min_s, s); + } + + ChassisSpeeds retSpeeds = new ChassisSpeeds( + prevSetpoint.mChassisSpeeds.vxMetersPerSecond + min_s * dx, + prevSetpoint.mChassisSpeeds.vyMetersPerSecond + min_s * dy, + prevSetpoint.mChassisSpeeds.omegaRadiansPerSecond + min_s * dtheta + ); + var retStates = mKinematics.toSwerveModuleStates(retSpeeds); + for (int i = 0; i < modules.length; ++i) { + final var maybeOverride = overrideSteering.get(i); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[i].angle.inverse().rotateBy(override))) { + retStates[i].speedMetersPerSecond *= -1.0; + } + retStates[i].angle = override; + } + final var deltaRotation = + prevSetpoint.mModuleStates[i].angle.inverse().rotateBy(retStates[i].angle); + if (flipHeading(deltaRotation)) { + retStates[i].angle = retStates[i].angle.flip(); + retStates[i].speedMetersPerSecond *= -1.0; + } + } + return new SwerveSetpoint(retSpeeds, retStates); + } +} diff --git a/src/main/java/com/team254/lib/swerve/TargetLockedFieldRelativeController.java b/src/main/java/com/team254/lib/swerve/TargetLockedFieldRelativeController.java new file mode 100644 index 0000000..26f2ab9 --- /dev/null +++ b/src/main/java/com/team254/lib/swerve/TargetLockedFieldRelativeController.java @@ -0,0 +1,55 @@ +package com.team254.lib.swerve; + +// import com.team254.frc2022.Constants; +// import com.team254.frc2022.RobotState; +import com.team254.lib.control.RadiusController; +import com.team254.lib.control.SwerveHeadingController; +import com.team254.lib.geometry.Pose2d; + +public class TargetLockedFieldRelativeController implements IDriveController { + public static TargetLockedFieldRelativeController mInstance; + public SwerveHeadingController mSwerveHeadingController = SwerveHeadingController.getInstance(); + public RadiusController mRadiusController = RadiusController.getInstance(); + + // private RobotState mRobotState = RobotState.getInstance(); + + public static TargetLockedFieldRelativeController getInstance() { + if (mInstance == null) { + mInstance = new TargetLockedFieldRelativeController(); + } + return mInstance; + } + + @Override + public ChassisSpeeds transform(DriveInput driveInput, Pose2d robotPose) { + // mRadiusController.setRadiusControllerState(RadiusController.RadiusControllerState.OFF); + // if ((mSwerveHeadingController.getHeadingControllerState() + // == SwerveHeadingController.HeadingControllerState.POLAR_SNAP + // && mSwerveHeadingController.isAtGoal()) + // || driveInput.changeToMaintainTargetHeading()) { + // mSwerveHeadingController.setHeadingControllerState( + // SwerveHeadingController.HeadingControllerState.POLAR_MAINTAIN + // ); + // mSwerveHeadingController.setGoal(mSwerveHeadingController.calculateAngleToOrigin( + // mRobotState.getLatestFieldToVehicle().getValue() + // )); + // } else { + // mSwerveHeadingController.setHeadingControllerState( + // SwerveHeadingController.HeadingControllerState.POLAR_SNAP + // ); + // mSwerveHeadingController.setGoal(mSwerveHeadingController.calculateAngleToOrigin( + // mRobotState.getLatestFieldToVehicle().getValue() + // )); + // } + // return ChassisSpeeds.fromFieldRelativeSpeeds( + // driveInput.getThrottle() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // driveInput.getStrafe() * Constants.kMaxVelocityMetersPerSecond + // * Constants.kScaleTranslationInputs, + // mSwerveHeadingController.update(robotPose.getRotation().getDegrees()) + // * Constants.kMaxAngularVelocityRadiansPerSecond, + // robotPose.getRotation() + // ); + return new ChassisSpeeds(); + } +} diff --git a/src/main/java/com/team254/lib/trajectory/DistanceView.java b/src/main/java/com/team254/lib/trajectory/DistanceView.java new file mode 100755 index 0000000..5bf931f --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/DistanceView.java @@ -0,0 +1,63 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.State; +import com.team254.lib.util.Util; + +public class DistanceView, T extends State> implements TrajectoryView { + protected final Trajectory trajectory_; + protected final double[] distances_; + + public DistanceView(final Trajectory trajectory) { + trajectory_ = trajectory; + distances_ = new double[trajectory_.length()]; + distances_[0] = 0.0; + for (int i = 1; i < trajectory_.length(); ++i) { + distances_[i] = distances_[i - 1] + + trajectory_.getPoint(i - 1).state().distance(trajectory_.getPoint(i).state()); + } + } + + @Override + public TrajectorySamplePoint sample(double distance) { + if (distance >= last_interpolant()) + return new TrajectorySamplePoint<>(trajectory_.getPoint(trajectory_.length() - 1)); + if (distance <= 0.0) + return new TrajectorySamplePoint<>(trajectory_.getPoint(0)); + for (int i = 1; i < distances_.length; ++i) { + final TrajectoryPoint s = trajectory_.getPoint(i); + if (distances_[i] >= distance) { + final TrajectoryPoint prev_s = trajectory_.getPoint(i - 1); + if (Util.epsilonEquals(distances_[i], distances_[i - 1])) { + return new TrajectorySamplePoint<>(s); + } else { + return new TrajectorySamplePoint<>( + prev_s.state().interpolate( + s.state(), (distance - distances_[i - 1]) / (distances_[i] - distances_[i - 1]) + ), + prev_s.heading().interpolate( + s.heading(), (distance - distances_[i - 1]) / (distances_[i] - distances_[i - 1]) + ), + i - 1, + i + ); + } + } + } + throw new RuntimeException(); + } + + @Override + public double last_interpolant() { + return distances_[distances_.length - 1]; + } + + @Override + public double first_interpolant() { + return 0.0; + } + + @Override + public Trajectory trajectory() { + return trajectory_; + } +} diff --git a/src/main/java/com/team254/lib/trajectory/IPathFollower.java b/src/main/java/com/team254/lib/trajectory/IPathFollower.java new file mode 100755 index 0000000..27d6377 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/IPathFollower.java @@ -0,0 +1,10 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Twist2d; + +public interface IPathFollower { + Twist2d steer(Pose2d current_pose); + + boolean isDone(); +} diff --git a/src/main/java/com/team254/lib/trajectory/PurePursuitController.java b/src/main/java/com/team254/lib/trajectory/PurePursuitController.java new file mode 100755 index 0000000..94c161b --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/PurePursuitController.java @@ -0,0 +1,125 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.*; +import com.team254.lib.util.Util; + +public class PurePursuitController, T extends IRotation2d> + implements IPathFollower { + protected final TrajectoryIterator iterator_; + protected final double sampling_dist_; + protected final double lookahead_; + protected final double goal_tolerance_; + protected boolean done_ = false; + + public PurePursuitController( + final DistanceView path, double sampling_dist, double lookahead, double goal_tolerance + ) { + sampling_dist_ = sampling_dist; + lookahead_ = lookahead; + goal_tolerance_ = goal_tolerance; + iterator_ = new TrajectoryIterator(path); + } + + public Twist2d steer(final Pose2d current_pose) { + done_ = done_ + || (iterator_.isDone() + && current_pose.getTranslation().distance(iterator_.getState().getTranslation()) + <= goal_tolerance_); + if (isDone()) { + return new Twist2d(0.0, 0.0, 0.0); + } + + final double remaining_progress = iterator_.getRemainingProgress(); + double goal_progress = 0.0; + // Find the first point > lookahead distance away from current_pose, or the last point + // otherwise. + for (double progress = 0.0; progress <= remaining_progress; + progress = Math.min(remaining_progress, progress + sampling_dist_)) { + double dist = current_pose.getTranslation().distance( + iterator_.preview(progress).state().getTranslation() + ); + if (dist > lookahead_) { + if (goal_progress == 0.0 && !iterator_.isDone()) { + // Make sure we don't get stuck due to numerical issues when sampling dist is large + // relative to lookahead. + goal_progress = progress; + } + break; + } + goal_progress = progress; + if (progress == remaining_progress) { + break; + } + } + iterator_.advance(goal_progress); + // final Arc arc = new Arc(current_pose, iterator_.getState()); + final Translation2d path_setpoint = + current_pose.getTranslation().inverse().translateBy(iterator_.getState().getTranslation()); + final Rotation2d254 heading_setpoint = + current_pose.getRotation().inverse().rotateBy(iterator_.getHeading().getRotation()); + if (path_setpoint.norm() < Util.kEpsilon) { + return new Twist2d(0.0, 0.0, 0.0); + } else { + return new Twist2d(path_setpoint.x(), path_setpoint.y(), heading_setpoint.getRadians()); + } + } + + public boolean isDone() { + return done_; + } + + protected static > double getDirection(Pose2d pose, S point) { + Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point.getTranslation()); + Translation2d robot = pose.getRotation().toTranslation(); + double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x(); + return (cross < 0.) ? -1. : 1.; // if robot < pose turn left + } + + public static class Arc> { + public Translation2d center; + public double radius; + public double length; + + public Arc(final Pose2d pose, final S point) { + center = findCenter(pose, point); + radius = new Translation2d(center, point.getTranslation()).norm(); + length = findLength(pose, point, center, radius); + radius *= getDirection(pose, point); + } + + protected Translation2d findCenter(Pose2d pose, S point) { + final Translation2d poseToPointHalfway = + pose.getTranslation().interpolate(point.getTranslation(), 0.5); + final Rotation2d254 normal = + pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal(); + final Pose2d perpendicularBisector = new Pose2d(poseToPointHalfway, normal); + final Pose2d normalFromPose = new Pose2d(pose.getTranslation(), pose.getRotation().normal()); + if (normalFromPose.isColinear(perpendicularBisector.normal())) { + // Special case: center is poseToPointHalfway. + return poseToPointHalfway; + } + return normalFromPose.intersection(perpendicularBisector); + } + + protected double findLength(Pose2d pose, S point, Translation2d center, double radius) { + if (radius < Double.MAX_VALUE) { + final Translation2d centerToPoint = new Translation2d(center, point.getTranslation()); + final Translation2d centerToPose = new Translation2d(center, pose.getTranslation()); + // If the point is behind pose, we want the opposite of this angle. To determine if the + // point is behind, check the sign of the cross-product between the normal vector and the + // vector from pose to point. + final boolean behind = Math.signum(Translation2d.cross( + pose.getRotation().normal().toTranslation(), + new Translation2d(pose.getTranslation(), point.getTranslation()) + )) + > 0.0; + final Rotation2d254 angle = Translation2d.getAngle(centerToPose, centerToPoint); + return radius + * (behind ? 2.0 * Math.PI - Math.abs(angle.getRadians()) : Math.abs(angle.getRadians()) + ); + } else { + return new Translation2d(pose.getTranslation(), point.getTranslation()).norm(); + } + } + } +} diff --git a/src/main/java/com/team254/lib/trajectory/TimedView.java b/src/main/java/com/team254/lib/trajectory/TimedView.java new file mode 100755 index 0000000..00e71b3 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/TimedView.java @@ -0,0 +1,63 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.State; +import com.team254.lib.trajectory.timing.TimedState; +import com.team254.lib.util.Util; + +public class TimedView, T extends State> + implements TrajectoryView, TimedState> { + protected final Trajectory, TimedState> trajectory_; + protected final double start_t_; + protected final double end_t_; + + public TimedView(Trajectory, TimedState> trajectory) { + trajectory_ = trajectory; + start_t_ = trajectory_.getPoint(0).state().t(); + end_t_ = trajectory_.getPoint(trajectory_.length() - 1).state().t(); + } + + @Override + public double first_interpolant() { + return start_t_; + } + + @Override + public double last_interpolant() { + return end_t_; + } + + @Override + public TrajectorySamplePoint, TimedState> sample(double t) { + if (t >= end_t_) { + return new TrajectorySamplePoint<>(trajectory_.getPoint(trajectory_.length() - 1)); + } + if (t <= start_t_) { + return new TrajectorySamplePoint<>(trajectory_.getPoint(0)); + } + for (int i = 1; i < trajectory_.length(); ++i) { + final TrajectoryPoint, TimedState> s = trajectory_.getPoint(i); + if (s.state().t() >= t) { + final TrajectoryPoint, TimedState> prev_s = trajectory_.getPoint(i - 1); + if (Util.epsilonEquals(s.state().t(), prev_s.state().t())) { + return new TrajectorySamplePoint<>(s); + } + return new TrajectorySamplePoint<>( + prev_s.state().interpolate( + s.state(), (t - prev_s.state().t()) / (s.state().t() - prev_s.state().t()) + ), + prev_s.heading().interpolate( + s.heading(), (t - prev_s.heading().t()) / (s.heading().t() - prev_s.heading().t()) + ), + i - 1, + i + ); + } + } + throw new RuntimeException(); + } + + @Override + public Trajectory, TimedState> trajectory() { + return trajectory_; + } +} diff --git a/src/main/java/com/team254/lib/trajectory/Trajectory.java b/src/main/java/com/team254/lib/trajectory/Trajectory.java new file mode 100644 index 0000000..21b2820 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/Trajectory.java @@ -0,0 +1,139 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.State; +import com.team254.lib.util.CSVWritable; +import java.util.ArrayList; +import java.util.List; + +public class Trajectory, T extends State> implements CSVWritable { + protected final List> points_; + protected final IndexView index_view_ = new IndexView(); + protected double default_velocity_; + + /** + * Create an empty trajectory. + */ + public Trajectory() { + points_ = new ArrayList<>(); + } + + public void setDefaultVelocity(double velocity) { + default_velocity_ = velocity; + } + + public double getDefaultVelocity() { + return default_velocity_; + } + + /** + * Create a trajectory from the given states and transforms. + * + * @param states The states of the trajectory. + */ + public Trajectory(final List states, final List headings) { + points_ = new ArrayList<>(states.size()); + for (int i = 0; i < states.size(); ++i) { + points_.add(new TrajectoryPoint<>(states.get(i), headings.get(i), i)); + } + } + + public Trajectory(final List> points) { + points_ = new ArrayList<>(points.size()); + for (int i = 0; i < points.size(); i++) { + points_.add(new TrajectoryPoint<>(points.get(i).state(), points.get(i).heading(), i)); + } + } + + public boolean isEmpty() { + return points_.isEmpty(); + } + + public int length() { + return points_.size(); + } + + public TrajectoryPoint getLastPoint() { + return points_.get(length() - 1); + } + + public TrajectoryPoint getPoint(final int index) { + return points_.get(index); + } + + public TrajectorySamplePoint getInterpolated(final double index) { + if (isEmpty()) { + return null; + } else if (index <= 0.0) { + return new TrajectorySamplePoint<>(getPoint(0)); + } else if (index >= length() - 1) { + return new TrajectorySamplePoint<>(getPoint(length() - 1)); + } + final int i = (int) Math.floor(index); + final double frac = index - i; + if (frac <= Double.MIN_VALUE) { + return new TrajectorySamplePoint<>(getPoint(i)); + } else if (frac >= 1.0 - Double.MIN_VALUE) { + return new TrajectorySamplePoint<>(getPoint(i + 1)); + } else { + return new TrajectorySamplePoint<>( + getPoint(i).state().interpolate(getPoint(i + 1).state(), frac), + getPoint(i).heading().interpolate(getPoint(i + 1).heading(), frac), + i, + i + 1 + ); + } + } + + public IndexView getIndexView() { + return index_view_; + } + + @Override + public String toString() { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < length(); ++i) { + builder.append(i); + builder.append(": "); + builder.append(getPoint(i).state()); + builder.append(getPoint(i).heading()); + builder.append(System.lineSeparator()); + } + return builder.toString(); + } + + @Override + public String toCSV() { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < length(); ++i) { + builder.append(i); + builder.append(","); + builder.append(getPoint(i).state().toCSV()); + builder.append(getPoint(i).heading().toCSV()); + builder.append(System.lineSeparator()); + } + return builder.toString(); + } + + public class IndexView implements TrajectoryView { + @Override + public TrajectorySamplePoint sample(double index) { + return Trajectory.this.getInterpolated(index); + } + + @Override + public double last_interpolant() { + return Math.max(0.0, Trajectory.this.length() - 1); + } + + @Override + public double first_interpolant() { + return 0.0; + } + + @Override + public Trajectory trajectory() { + return Trajectory.this; + } + } +} diff --git a/src/main/java/com/team254/lib/trajectory/TrajectoryIterator.java b/src/main/java/com/team254/lib/trajectory/TrajectoryIterator.java new file mode 100755 index 0000000..9c15a37 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/TrajectoryIterator.java @@ -0,0 +1,63 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.State; + +public class TrajectoryIterator, T extends State> { + protected final TrajectoryView view_; + protected double progress_ = 0.0; + protected TrajectorySamplePoint current_sample_; + + public TrajectoryIterator(final TrajectoryView view) { + view_ = view; + + // No effect if view is empty. + current_sample_ = view_.sample(view_.first_interpolant()); + progress_ = view_.first_interpolant(); + } + + public boolean isDone() { + return getRemainingProgress() == 0.0; + } + + public double getProgress() { + return progress_; + } + + public double getRemainingProgress() { + return Math.max(0.0, view_.last_interpolant() - progress_); + } + + public TrajectorySamplePoint getSample() { + return current_sample_; + } + + public S getState() { + return getSample().state(); + } + + public T getHeading() { + return getSample().heading(); + } + + public TrajectorySamplePoint advance(double additional_progress) { + progress_ = Math.max( + view_.first_interpolant(), + Math.min(view_.last_interpolant(), progress_ + additional_progress) + ); + current_sample_ = view_.sample(progress_); + return current_sample_; + } + + public TrajectorySamplePoint preview(double additional_progress) { + final double progress = Math.max( + view_.first_interpolant(), + Math.min(view_.last_interpolant(), progress_ + additional_progress) + ); + return view_.sample(progress); + } + + public Trajectory trajectory() { + return view_.trajectory(); + } +} diff --git a/src/main/java/com/team254/lib/trajectory/TrajectoryPoint.java b/src/main/java/com/team254/lib/trajectory/TrajectoryPoint.java new file mode 100755 index 0000000..db7888a --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/TrajectoryPoint.java @@ -0,0 +1,27 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.State; + +public class TrajectoryPoint, T extends State> { + protected final S state_; + protected final T heading_; + protected final int index_; + + public TrajectoryPoint(final S state, T heading, int index) { + state_ = state; + heading_ = heading; + index_ = index; + } + + public S state() { + return state_; + } + + public T heading() { + return heading_; + } + + public int index() { + return index_; + } +} diff --git a/src/main/java/com/team254/lib/trajectory/TrajectorySamplePoint.java b/src/main/java/com/team254/lib/trajectory/TrajectorySamplePoint.java new file mode 100755 index 0000000..518b078 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/TrajectorySamplePoint.java @@ -0,0 +1,40 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.State; + +public class TrajectorySamplePoint, T extends State> { + protected final S state_; + protected final T heading_; + protected final int index_floor_; + protected final int index_ceil_; + + public TrajectorySamplePoint(final S state, final T heading, int index_floor, int index_ceil) { + state_ = state; + heading_ = heading; + index_floor_ = index_floor; + index_ceil_ = index_ceil; + } + + public TrajectorySamplePoint(final TrajectoryPoint point) { + state_ = point.state(); + heading_ = point.heading(); + index_floor_ = index_ceil_ = point.index(); + } + + public S state() { + return state_; + } + + public T heading() { + return heading_; + } + + public int index_floor() { + return index_floor_; + } + + public int index_ceil() { + return index_ceil_; + } +} diff --git a/src/main/java/com/team254/lib/trajectory/TrajectoryUtil.java b/src/main/java/com/team254/lib/trajectory/TrajectoryUtil.java new file mode 100755 index 0000000..0b19d74 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/TrajectoryUtil.java @@ -0,0 +1,111 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.*; +import com.team254.lib.spline.QuinticHermiteSpline; +import com.team254.lib.spline.Spline; +import com.team254.lib.spline.SplineGenerator; +import com.team254.lib.trajectory.timing.TimedState; +import com.team254.lib.util.Util; +import java.util.ArrayList; +import java.util.List; + +public class TrajectoryUtil { + public static , T extends IRotation2d> Trajectory mirror( + final Trajectory trajectory + ) { + List waypoints = new ArrayList<>(trajectory.length()); + List headings = new ArrayList<>(trajectory.length()); + for (int i = 0; i < trajectory.length(); ++i) { + waypoints.add(trajectory.getPoint(i).state().mirror()); + headings.add(trajectory.getPoint(i).heading().mirror()); + } + return new Trajectory<>(waypoints, headings); + } + + public static , T extends IRotation2d> + Trajectory, TimedState> mirrorTimed( + final Trajectory, TimedState> trajectory + ) { // todo fix + List> waypoints = new ArrayList<>(trajectory.length()); + List> headings = new ArrayList<>(trajectory.length()); + for (int i = 0; i < trajectory.length(); ++i) { + TimedState timed_state = trajectory.getPoint(i).state(); + waypoints.add(new TimedState<>( + timed_state.state().mirror(), + timed_state.t(), + timed_state.velocity(), + timed_state.acceleration() + )); + headings.add(new TimedState<>(trajectory.getPoint(i).heading().state().mirror())); + } + return new Trajectory<>(waypoints, headings); + } + + public static , T extends IRotation2d> Trajectory transform( + final Trajectory trajectory, Pose2d transform + ) { + List waypoints = new ArrayList<>(trajectory.length()); + List headings = new ArrayList<>(trajectory.length()); + for (int i = 0; i < trajectory.length(); ++i) { + waypoints.add(trajectory.getPoint(i).state().transformBy(transform)); + headings.add(trajectory.getPoint(i).heading().rotateBy(transform.getRotation())); + } + return new Trajectory<>(waypoints, headings); + } + + /** + * Creates a Trajectory by sampling a TrajectoryView at a regular interval. + * + * @param trajectory_view + * @param interval + * @return + */ + public static , T extends State> Trajectory resample( + final TrajectoryView trajectory_view, double interval + ) { + if (interval <= Util.kEpsilon) { + return new Trajectory(); + } + final int num_states = (int) Math.ceil( + (trajectory_view.last_interpolant() - trajectory_view.first_interpolant()) / interval + ); + ArrayList states = new ArrayList(num_states); + ArrayList headings = new ArrayList<>(num_states); + + for (int i = 0; i < num_states; ++i) { + states.add(trajectory_view.sample(i * interval + trajectory_view.first_interpolant()).state() + ); + headings.add( + trajectory_view.sample(i * interval + trajectory_view.first_interpolant()).heading() + ); + } + return new Trajectory<>(states, headings); + } + + public static Trajectory trajectoryFromWaypoints( + final List waypoints, + final List headings, + double maxDx, + double maxDy, + double maxDTheta + ) { + List splines = new ArrayList<>(waypoints.size() - 1); + for (int i = 1; i < waypoints.size(); ++i) { + splines.add(new QuinticHermiteSpline(waypoints.get(i - 1), waypoints.get(i))); + } + QuinticHermiteSpline.optimizeSpline(splines); + return trajectoryFromSplinesAndHeadings(splines, headings, maxDx, maxDy, maxDTheta); + } + + public static Trajectory trajectoryFromSplinesAndHeadings( + final List splines, + final List headings, + double maxDx, + double maxDy, + double maxDTheta + ) { + return new Trajectory<>( + SplineGenerator.parameterizeSplines(splines, headings, maxDx, maxDy, maxDTheta) + ); + } +} diff --git a/src/main/java/com/team254/lib/trajectory/TrajectoryView.java b/src/main/java/com/team254/lib/trajectory/TrajectoryView.java new file mode 100755 index 0000000..240ad74 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/TrajectoryView.java @@ -0,0 +1,13 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.State; + +public interface TrajectoryView, T extends State> { + TrajectorySamplePoint sample(final double interpolant); + + double first_interpolant(); + + double last_interpolant(); + + Trajectory trajectory(); +} diff --git a/src/main/java/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java b/src/main/java/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java new file mode 100755 index 0000000..00ee3a3 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/timing/CentripetalAccelerationConstraint.java @@ -0,0 +1,23 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.Pose2dWithCurvature; + +public class CentripetalAccelerationConstraint implements TimingConstraint { + final double mMaxCentripetalAccel; + + public CentripetalAccelerationConstraint(final double max_centripetal_accel) { + mMaxCentripetalAccel = max_centripetal_accel; + } + + @Override + public double getMaxVelocity(final Pose2dWithCurvature state) { + return Math.sqrt(Math.abs(mMaxCentripetalAccel / state.getCurvature())); + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration( + final Pose2dWithCurvature state, final double velocity + ) { + return MinMaxAcceleration.kNoLimits; + } +} diff --git a/src/main/java/com/team254/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/src/main/java/com/team254/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java new file mode 100644 index 0000000..abbedfc --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java @@ -0,0 +1,29 @@ +package com.team254.lib.trajectory.timing; + +// import com.team254.frc2022.Constants; +import com.team254.lib.geometry.ICurvature; +import com.team254.lib.geometry.IPose2d; +import com.team254.lib.physics.SwerveDrive; + +public class SwerveDriveDynamicsConstraint & ICurvature> + implements TimingConstraint { + protected final SwerveDrive drive_; + protected final double abs_voltage_limit_; + + public SwerveDriveDynamicsConstraint(final SwerveDrive drive, double abs_voltage_limit) { + drive_ = drive; + abs_voltage_limit_ = abs_voltage_limit; + } + + @Override + public double getMaxVelocity(S state) { + return 4.959668 / (1 + Math.abs(4.0 * state.getCurvature())); // from 1323 TODO verify or fix + // return Constants.kMaxVelocityMetersPerSecond / (1 + Math.abs(4.0*state.getCurvature()));// + // from 1323 TODO verify or fix + } + + @Override + public MinMaxAcceleration getMinMaxAcceleration(S state, double velocity) { + return new MinMaxAcceleration(-(1867 * 0.8), (1867 * 0.8)); + } +} diff --git a/src/main/java/com/team254/lib/trajectory/timing/TimedState.java b/src/main/java/com/team254/lib/trajectory/timing/TimedState.java new file mode 100755 index 0000000..a415a5e --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/timing/TimedState.java @@ -0,0 +1,105 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.State; +import com.team254.lib.util.Util; +import java.text.DecimalFormat; + +public class TimedState> implements State> { + protected final S state_; + protected double t_; // Time we achieve this state. + protected double velocity_; // ds/dt + protected double acceleration_; // d^2s/dt^2 + + public TimedState(final S state) { + state_ = state; + } + + public TimedState(final S state, double t, double velocity, double acceleration) { + state_ = state; + t_ = t; + velocity_ = velocity; + acceleration_ = acceleration; + } + + public S state() { + return state_; + } + + public void set_t(double t) { + t_ = t; + } + + public double t() { + return t_; + } + + public void set_velocity(double velocity) { + velocity_ = velocity; + } + + public double velocity() { + return velocity_; + } + + public void set_acceleration(double acceleration) { + acceleration_ = acceleration; + } + + public double acceleration() { + return acceleration_; + } + + @Override + public String toString() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return state().toString() + ", t: " + fmt.format(t()) + ", v: " + fmt.format(velocity()) + + ", a: " + fmt.format(acceleration()); + } + + @Override + public String toCSV() { + final DecimalFormat fmt = new DecimalFormat("#0.000"); + return state().toCSV() + "," + fmt.format(t()) + "," + fmt.format(velocity()) + "," + + fmt.format(acceleration()); + } + + @Override + public TimedState interpolate(TimedState other, double x) { + final double new_t = Util.interpolate(t(), other.t(), x); + final double delta_t = new_t - t(); + if (delta_t < 0.0) { + return other.interpolate(this, 1.0 - x); + } + boolean reversing = + velocity() < 0.0 || (Util.epsilonEquals(velocity(), 0.0) && acceleration() < 0.0); + final double new_v = velocity() + acceleration() * delta_t; + final double new_s = + (reversing ? -1.0 : 1.0) * (velocity() * delta_t + .5 * acceleration() * delta_t * delta_t); + // System.out.println("x: " + x + " , new_t: " + new_t + ", new_s: " + new_s + " , distance: " + + // state() .distance(other.state())); + return new TimedState( + state().interpolate(other.state(), new_s / state().distance(other.state())), + new_t, + new_v, + acceleration() + ); + } + + @Override + public TimedState add(TimedState other) { + return new TimedState<>(this.state().add(other.state())); + } + + @Override + public double distance(TimedState other) { + return state().distance(other.state()); + } + + @Override + public boolean equals(final Object other) { + if (other == null || !(other instanceof TimedState) ) + return false; + TimedState ts = (TimedState) other; + return state().equals(ts.state()) && Util.epsilonEquals(t(), ts.t()); + } +} diff --git a/src/main/java/com/team254/lib/trajectory/timing/TimingConstraint.java b/src/main/java/com/team254/lib/trajectory/timing/TimingConstraint.java new file mode 100755 index 0000000..5809d0f --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/timing/TimingConstraint.java @@ -0,0 +1,39 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.State; + +public interface TimingConstraint> { + double getMaxVelocity(S state); + + MinMaxAcceleration getMinMaxAcceleration(S state, double velocity); + + class MinMaxAcceleration { + protected final double min_acceleration_; + protected final double max_acceleration_; + + public static MinMaxAcceleration kNoLimits = new MinMaxAcceleration(); + + public MinMaxAcceleration() { + // No limits. + min_acceleration_ = Double.NEGATIVE_INFINITY; + max_acceleration_ = Double.POSITIVE_INFINITY; + } + + public MinMaxAcceleration(double min_acceleration, double max_acceleration) { + min_acceleration_ = min_acceleration; + max_acceleration_ = max_acceleration; + } + + public double min_acceleration() { + return min_acceleration_; + } + + public double max_acceleration() { + return max_acceleration_; + } + + public boolean valid() { + return min_acceleration() <= max_acceleration(); + } + } +} diff --git a/src/main/java/com/team254/lib/trajectory/timing/TimingUtil.java b/src/main/java/com/team254/lib/trajectory/timing/TimingUtil.java new file mode 100755 index 0000000..c6bacd8 --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/timing/TimingUtil.java @@ -0,0 +1,303 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.State; +import com.team254.lib.trajectory.DistanceView; +import com.team254.lib.trajectory.Trajectory; +import java.util.ArrayList; +import java.util.List; + +public class TimingUtil { + public static , T extends State> Trajectory, TimedState> + timeParameterizeTrajectory( + boolean reverse, + final DistanceView distance_view, + double step_size, + final List> constraints, + double start_velocity, + double end_velocity, + double max_translational_velocity, + double max_abs_acceleration + ) { + final int num_states = (int) Math.ceil(distance_view.last_interpolant() / step_size + 1); + List states = new ArrayList<>(num_states); + List headings = new ArrayList<>(num_states); + for (int i = 0; i < num_states; ++i) { + states.add( + distance_view.sample(Math.min(i * step_size, distance_view.last_interpolant())).state() + ); + headings.add( + distance_view.sample(Math.min(i * step_size, distance_view.last_interpolant())).heading() + ); + } + return timeParameterizeTrajectory( + reverse, + states, + headings, + constraints, + start_velocity, + end_velocity, + max_translational_velocity, + max_abs_acceleration + ); + } + + public static , T extends State> Trajectory, TimedState> + timeParameterizeTrajectory( + boolean reverse, + final List states, + final List headings, + final List> constraints, + double start_velocity, + double end_velocity, + double max_translational_velocity, + double max_abs_acceleration + ) { + List> constraint_states = new ArrayList<>(states.size()); + final double kEpsilon = 1e-6; + + // Forward pass. We look at pairs of consecutive states, where the start state has already been + // velocity parameterized (though we may adjust the velocity downwards during the backwards + // pass). We wish to find an acceleration that is admissible at both the start and end state, as + // well as an admissible end velocity. If there is no admissible end velocity or acceleration, + // we set the end velocity to the state's maximum allowed velocity and will repair the + // acceleration during the backward pass (by slowing down the predecessor). + ConstrainedState predecessor = new ConstrainedState<>(); + predecessor.state = states.get(0); + predecessor.distance = 0.0; + predecessor.heading = headings.get(0); + predecessor.max_translational_velocity = start_velocity; + predecessor.min_translational_acceleration = -max_abs_acceleration; + predecessor.max_acceleration = max_abs_acceleration; + for (int i = 0; i < states.size(); ++i) { + // Add the new state. + constraint_states.add(new ConstrainedState<>()); + ConstrainedState constraint_state = constraint_states.get(i); + constraint_state.state = states.get(i); + constraint_state.heading = headings.get(i); + final double ds = constraint_state.state.distance(predecessor.state); + constraint_state.distance = ds + predecessor.distance; + + // We may need to iterate to find the maximum end velocity and common acceleration, since + // acceleration limits may be a function of velocity. + while (true) { + // Enforce global max velocity and max reachable velocity by global acceleration limit. + // vf = sqrt(vi^2 + 2*a*d) + constraint_state.max_translational_velocity = Math.min( + max_translational_velocity, + Math.sqrt( + predecessor.max_translational_velocity * predecessor.max_translational_velocity + + 2.0 * predecessor.max_acceleration * ds + ) + ); + if (Double.isNaN(constraint_state.max_translational_velocity)) { + throw new RuntimeException(); + } + // Enforce global max absolute acceleration. + constraint_state.min_translational_acceleration = -max_abs_acceleration; + constraint_state.max_acceleration = max_abs_acceleration; + + // At this point, the state is full constructed, but no constraints have been applied aside + // from predecessor state max accel. + + // Enforce all velocity constraints. + for (final TimingConstraint constraint : constraints) { + constraint_state.max_translational_velocity = Math.min( + constraint_state.max_translational_velocity, + constraint.getMaxVelocity(constraint_state.state) + ); + } + if (constraint_state.max_translational_velocity < 0.0) { + // This should never happen if constraints are well-behaved. + throw new RuntimeException(); + } + + // Now enforce all acceleration constraints. + for (final TimingConstraint constraint : constraints) { + final TimingConstraint.MinMaxAcceleration min_max_accel = + constraint.getMinMaxAcceleration( + constraint_state.state, + (reverse ? -1.0 : 1.0) * constraint_state.max_translational_velocity + ); + if (!min_max_accel.valid()) { + // This should never happen if constraints are well-behaved. + throw new RuntimeException(); + } + constraint_state.min_translational_acceleration = Math.max( + constraint_state.min_translational_acceleration, + reverse ? -min_max_accel.max_acceleration() : min_max_accel.min_acceleration() + ); + constraint_state.max_acceleration = Math.min( + constraint_state.max_acceleration, + reverse ? -min_max_accel.min_acceleration() : min_max_accel.max_acceleration() + ); + } + if (constraint_state.min_translational_acceleration > constraint_state.max_acceleration) { + // This should never happen if constraints are well-behaved. + throw new RuntimeException(); + } + + if (ds < kEpsilon) { + break; + } + // If the max acceleration for this constraint state is more conservative than what we had + // applied, we need to reduce the max accel at the predecessor state and try again. + // TODO: Simply using the new max acceleration is guaranteed to be valid, but may be too + // conservative. Doing a search would be better. + final double actual_acceleration = + (constraint_state.max_translational_velocity + * constraint_state.max_translational_velocity + - predecessor.max_translational_velocity * predecessor.max_translational_velocity) + / (2.0 * ds); + if (constraint_state.max_acceleration < actual_acceleration - kEpsilon) { + predecessor.max_acceleration = constraint_state.max_acceleration; + } else { + if (actual_acceleration > predecessor.min_translational_acceleration + kEpsilon) { + predecessor.max_acceleration = actual_acceleration; + } + // If actual acceleration is less than predecessor min accel, we will repair during the + // backward pass. + break; + } + // System.out.println("(intermediate) i: " + i + ", " + constraint_state.toString()); + } + // System.out.println("i: " + i + ", " + constraint_state.toString()); + predecessor = constraint_state; + } + + // Backward pass. + ConstrainedState successor = new ConstrainedState<>(); + successor.state = states.get(states.size() - 1); + successor.heading = headings.get(headings.size() - 1); + successor.distance = constraint_states.get(states.size() - 1).distance; + successor.max_translational_velocity = end_velocity; + successor.min_translational_acceleration = -max_abs_acceleration; + successor.max_acceleration = max_abs_acceleration; + for (int i = states.size() - 1; i >= 0; --i) { + ConstrainedState constraint_state = constraint_states.get(i); + final double ds = constraint_state.distance - successor.distance; // will be negative. + + while (true) { + // Enforce reverse max reachable velocity limit. + // vf = sqrt(vi^2 + 2*a*d), where vi = successor. + final double new_max_translational_velocity = Math.sqrt( + successor.max_translational_velocity * successor.max_translational_velocity + + 2.0 * successor.min_translational_acceleration * ds + ); + if (new_max_translational_velocity >= constraint_state.max_translational_velocity) { + // No new limits to impose. + break; + } + constraint_state.max_translational_velocity = new_max_translational_velocity; + if (Double.isNaN(constraint_state.max_translational_velocity)) { + throw new RuntimeException(); + } + + // Now check all acceleration constraints with the lower max velocity. + for (final TimingConstraint constraint : constraints) { + final TimingConstraint.MinMaxAcceleration min_max_accel = + constraint.getMinMaxAcceleration( + constraint_state.state, + (reverse ? -1.0 : 1.0) * constraint_state.max_translational_velocity + ); + if (!min_max_accel.valid()) { + throw new RuntimeException(); + } + constraint_state.min_translational_acceleration = Math.max( + constraint_state.min_translational_acceleration, + reverse ? -min_max_accel.max_acceleration() : min_max_accel.min_acceleration() + ); + constraint_state.max_acceleration = Math.min( + constraint_state.max_acceleration, + reverse ? -min_max_accel.min_acceleration() : min_max_accel.max_acceleration() + ); + } + if (constraint_state.min_translational_acceleration > constraint_state.max_acceleration) { + throw new RuntimeException(); + } + + if (ds > kEpsilon) { + break; + } + // If the min acceleration for this constraint state is more conservative than what we have + // applied, we need to reduce the min accel and try again. + // TODO: Simply using the new min acceleration is guaranteed to be valid, but may be too + // conservative. Doing a search would be better. + final double actual_acceleration = + (constraint_state.max_translational_velocity + * constraint_state.max_translational_velocity + - successor.max_translational_velocity * successor.max_translational_velocity) + / (2.0 * ds); + if (constraint_state.min_translational_acceleration > actual_acceleration + kEpsilon) { + successor.min_translational_acceleration = + constraint_state.min_translational_acceleration; + } else { + successor.min_translational_acceleration = actual_acceleration; + break; + } + } + successor = constraint_state; + } + + // Integrate the constrained states forward in time to obtain the TimedStates. + List> timed_states = new ArrayList<>(states.size()); + List> timed_headings = new ArrayList<>(states.size()); + double t = 0.0; + double s = 0.0; + double v = 0.0; + for (int i = 0; i < states.size(); ++i) { + final ConstrainedState constrained_state = constraint_states.get(i); + // Advance t. + final double ds = constrained_state.distance - s; + final double accel = (constrained_state.max_translational_velocity + * constrained_state.max_translational_velocity + - v * v) + / (2.0 * ds); + double dt = 0.0; + if (i > 0) { + timed_states.get(i - 1).set_acceleration(reverse ? -accel : accel); + if (Math.abs(accel) > kEpsilon) { + dt = (constrained_state.max_translational_velocity - v) / accel; + } else if (Math.abs(v) > kEpsilon) { + dt = ds / v; + } else { + throw new RuntimeException(); + } + } + t += dt; + if (Double.isNaN(t) || Double.isInfinite(t)) { + throw new RuntimeException(); + } + + v = constrained_state.max_translational_velocity; + s = constrained_state.distance; + timed_states.add( + new TimedState<>(constrained_state.state, t, reverse ? -v : v, reverse ? -accel : accel) + ); + timed_headings.add( + new TimedState<>(constrained_state.heading, t, reverse ? -v : v, reverse ? -accel : accel) + ); // todo verify + } + return new Trajectory<>(timed_states, timed_headings); + } + + protected static class ConstrainedState, T extends State> { + public S state; + public double distance; + public T heading; + public double max_translational_velocity; + public T max_angular_velocity; + public double min_translational_acceleration; + public T min_angular_acceleration; + public double max_acceleration; + public T max_angular_acceleration; + + @Override + public String toString() { + return state.toString() + ", distance: " + distance + + ", max_translational_velocity: " + max_translational_velocity + ", " + + "min_translational_acceleration: " + min_translational_acceleration + + ", max_acceleration: " + max_acceleration; + } + } +} diff --git a/src/main/java/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java b/src/main/java/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java new file mode 100755 index 0000000..b52b1ad --- /dev/null +++ b/src/main/java/com/team254/lib/trajectory/timing/VelocityLimitRegionConstraint.java @@ -0,0 +1,34 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.ITranslation2d; +import com.team254.lib.geometry.Translation2d; + +public class VelocityLimitRegionConstraint> + implements TimingConstraint { + protected final Translation2d min_corner_; + protected final Translation2d max_corner_; + protected final double velocity_limit_; + + public VelocityLimitRegionConstraint( + Translation2d min_corner, Translation2d max_corner, double velocity_limit + ) { + min_corner_ = min_corner; + max_corner_ = max_corner; + velocity_limit_ = velocity_limit; + } + + @Override + public double getMaxVelocity(S state) { + final Translation2d translation = state.getTranslation(); + if (translation.x() <= max_corner_.x() && translation.x() >= min_corner_.x() + && translation.y() <= max_corner_.y() && translation.y() >= min_corner_.y()) { + return velocity_limit_; + } + return Double.POSITIVE_INFINITY; + } + + @Override + public TimingConstraint.MinMaxAcceleration getMinMaxAcceleration(S state, double velocity) { + return MinMaxAcceleration.kNoLimits; + } +} diff --git a/src/main/java/com/team254/lib/util/CSVWritable.java b/src/main/java/com/team254/lib/util/CSVWritable.java new file mode 100644 index 0000000..eaba36e --- /dev/null +++ b/src/main/java/com/team254/lib/util/CSVWritable.java @@ -0,0 +1,5 @@ +package com.team254.lib.util; + +public interface CSVWritable { + String toCSV(); +} diff --git a/src/main/java/com/team254/lib/util/CircularBuffer.java b/src/main/java/com/team254/lib/util/CircularBuffer.java new file mode 100644 index 0000000..7cc9e8f --- /dev/null +++ b/src/main/java/com/team254/lib/util/CircularBuffer.java @@ -0,0 +1,56 @@ +package com.team254.lib.util; + +import java.util.LinkedList; + +/** + * Implements a simple circular buffer. + */ +public class CircularBuffer { + final int mWindowSize; + final LinkedList mSamples; + double mSum; + + public CircularBuffer(int window_size) { + mWindowSize = window_size; + mSamples = new LinkedList<>(); + mSum = 0.0; + } + + public void clear() { + mSamples.clear(); + mSum = 0.0; + } + + public double getAverage() { + if (mSamples.isEmpty()) + return 0.0; + return mSum / mSamples.size(); + } + + public void recomputeAverage() { + // Reset any accumulation drift. + mSum = 0.0; + if (mSamples.isEmpty()) + return; + for (Double val : mSamples) { + mSum += val; + } + mSum /= mWindowSize; + } + + public void addValue(double val) { + mSamples.addLast(val); + mSum += val; + if (mSamples.size() > mWindowSize) { + mSum -= mSamples.removeFirst(); + } + } + + public int getNumValues() { + return mSamples.size(); + } + + public boolean isFull() { + return mWindowSize == mSamples.size(); + } +} diff --git a/src/main/java/com/team254/lib/util/CircularBufferGeneric.java b/src/main/java/com/team254/lib/util/CircularBufferGeneric.java new file mode 100644 index 0000000..cadf4a3 --- /dev/null +++ b/src/main/java/com/team254/lib/util/CircularBufferGeneric.java @@ -0,0 +1,52 @@ +package com.team254.lib.util; + +import java.util.LinkedList; + +/** + * Implements a simple circular buffer. + * Can be used for any class. + */ +public class CircularBufferGeneric { + final int mWindowSize; + final LinkedList mSamples; + double mSum; + + public CircularBufferGeneric(int window_size) { + mWindowSize = window_size; + mSamples = new LinkedList<>(); + mSum = 0.0; + } + + public void clear() { + mSamples.clear(); + mSum = 0.0; + } + + public void addValue(E val) { + mSamples.addLast(val); + if (mSamples.size() > mWindowSize) { + mSamples.removeFirst(); + } + } + + public int getNumValues() { + return mSamples.size(); + } + + public boolean isFull() { + return mWindowSize == mSamples.size(); + } + + public LinkedList getLinkedList() { + /* + * NOTE: To get an Array of the specific class type which the instance is using, + * you have to use this specific code: + * specificCircularBufferGeneric.getLinkedList().toArray(new + * ClassThatIWant[specificCircularBufferGeneric .getLinkedList().size()]); The reason is that + * for some reason an array of a generic class(i.e. E[]) cannot be created because of some + * archaic data flow ambiguities + */ + + return mSamples; + } +} diff --git a/src/main/java/com/team254/lib/util/CrashTracker.java b/src/main/java/com/team254/lib/util/CrashTracker.java new file mode 100644 index 0000000..7126f3c --- /dev/null +++ b/src/main/java/com/team254/lib/util/CrashTracker.java @@ -0,0 +1,68 @@ +package com.team254.lib.util; + +import java.io.FileWriter; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.Date; +import java.util.UUID; + +/** + * Tracks start-up and caught crash events, logging them to a file which dosn't roll over + */ +public class CrashTracker { + private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID(); + + public static void logRobotConstruction() { + logMarker("robot startup"); + } + + public static void logRobotInit() { + logMarker("robot init"); + } + + public static void logTeleopInit() { + logMarker("teleop init"); + } + + public static void logAutoInit() { + logMarker("auto init"); + } + + public static void logDisabledInit() { + logMarker("disabled init"); + } + + public static void logTestInit() { + logMarker("test init"); + } + + public static void logThrowableCrash(Throwable throwable) { + logMarker("Exception", throwable); + } + + private static void logMarker(String mark) { + logMarker(mark, null); + } + + private static void logMarker(String mark, Throwable nullableException) { + try ( + PrintWriter writer = + new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true)) + ) { + writer.print(RUN_INSTANCE_UUID.toString()); + writer.print(", "); + writer.print(mark); + writer.print(", "); + writer.print(new Date().toString()); + + if (nullableException != null) { + writer.print(", "); + nullableException.printStackTrace(writer); + } + + writer.println(); + } catch (IOException e) { + e.printStackTrace(); + } + } +} diff --git a/src/main/java/com/team254/lib/util/CrashTrackingRunnable.java b/src/main/java/com/team254/lib/util/CrashTrackingRunnable.java new file mode 100644 index 0000000..48943a6 --- /dev/null +++ b/src/main/java/com/team254/lib/util/CrashTrackingRunnable.java @@ -0,0 +1,18 @@ +package com.team254.lib.util; + +/** + * Runnable class with reports all uncaught throws to CrashTracker + */ +public abstract class CrashTrackingRunnable implements Runnable { + @Override + public final void run() { + try { + runCrashTracked(); + } catch (Throwable t) { + CrashTracker.logThrowableCrash(t); + throw t; + } + } + + public abstract void runCrashTracked(); +} diff --git a/src/main/java/com/team254/lib/util/DelayedBoolean.java b/src/main/java/com/team254/lib/util/DelayedBoolean.java new file mode 100644 index 0000000..17c5e14 --- /dev/null +++ b/src/main/java/com/team254/lib/util/DelayedBoolean.java @@ -0,0 +1,32 @@ +package com.team254.lib.util; + +/** + * An iterative boolean latch that delays the transition from false to true. + */ +public class DelayedBoolean { + private boolean mLastValue; + private double mTransitionTimestamp; + private final double mDelay; + + public DelayedBoolean(double timestamp, double delay) { + mTransitionTimestamp = timestamp; + mLastValue = false; + mDelay = delay; + } + + public boolean update(double timestamp, boolean value) { + boolean result = false; + + if (value && !mLastValue) { + mTransitionTimestamp = timestamp; + } + + // If we are still true and we have transitioned. + if (value && (timestamp - mTransitionTimestamp > mDelay)) { + result = true; + } + + mLastValue = value; + return result; + } +} diff --git a/src/main/java/com/team254/lib/util/DriveOutput.java b/src/main/java/com/team254/lib/util/DriveOutput.java new file mode 100644 index 0000000..853a347 --- /dev/null +++ b/src/main/java/com/team254/lib/util/DriveOutput.java @@ -0,0 +1,39 @@ +package com.team254.lib.util; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.physics.SwerveDrive; +import com.team254.lib.swerve.SwerveModuleState; + +/** + * Represents a closed loop output to the drivebase + */ +public class DriveOutput { + public Rotation2d254[] azi_positions; // rad + public double[] drive_vels; // m/s + + public DriveOutput() { + this( + new Rotation2d254[] { + Rotation2d254.identity(), + Rotation2d254.identity(), + Rotation2d254.identity(), + Rotation2d254.identity()}, + new double[] {0, 0, 0, 0} + ); + } + + public DriveOutput(Rotation2d254[] azi_positions, double[] drive_vels) { + this.azi_positions = azi_positions; + this.drive_vels = drive_vels; + } + + public static DriveOutput fromSwerveModuleStates(SwerveModuleState[] state) { + double[] drive = new double[4]; + Rotation2d254[] azi = new Rotation2d254[4]; + for (int i = 0; i < 4; i++) { + drive[i] = state[i].speedMetersPerSecond; + azi[i] = state[i].angle; + } + return new DriveOutput(azi, drive); + } +} diff --git a/src/main/java/com/team254/lib/util/Interpolable.java b/src/main/java/com/team254/lib/util/Interpolable.java new file mode 100644 index 0000000..9f5b159 --- /dev/null +++ b/src/main/java/com/team254/lib/util/Interpolable.java @@ -0,0 +1,22 @@ +package com.team254.lib.util; + +/** + * Interpolable is an interface used by an Interpolating Tree as the Value type. Given two end + * points and an interpolation parameter on [0, 1], it calculates a new Interpolable representing + * the interpolated value. + * + * @param The Type of Interpolable + * @see InterpolatingTreeMap + */ +public interface Interpolable { + /** + * Interpolates between this value and an other value according to a given parameter. If x is 0, + * the method should return this value. If x is 1, the method should return the other value. If 0 + * < x < 1, the return value should be interpolated proportionally between the two. + * + * @param other The value of the upper bound + * @param x The requested value. Should be between 0 and 1. + * @return Interpolable The estimated average between the surrounding data + */ + T interpolate(T other, double x); +} diff --git a/src/main/java/com/team254/lib/util/InterpolatingDouble.java b/src/main/java/com/team254/lib/util/InterpolatingDouble.java new file mode 100644 index 0000000..44901e1 --- /dev/null +++ b/src/main/java/com/team254/lib/util/InterpolatingDouble.java @@ -0,0 +1,47 @@ +package com.team254.lib.util; + +/** + * A Double that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingDouble implements Interpolable, + InverseInterpolable, + Comparable { + public Double value = 0.0; + + public InterpolatingDouble(Double val) { + value = val; + } + + @Override + public InterpolatingDouble interpolate(InterpolatingDouble other, double x) { + Double dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingDouble(searchY); + } + + @Override + public double inverseInterpolate(InterpolatingDouble upper, InterpolatingDouble query) { + double upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) { + return 0; + } + double query_to_lower = query.value - value; + if (query_to_lower <= 0) { + return 0; + } + return query_to_lower / upper_to_lower; + } + + @Override + public int compareTo(InterpolatingDouble other) { + if (other.value < value) { + return 1; + } else if (other.value > value) { + return -1; + } else { + return 0; + } + } +} diff --git a/src/main/java/com/team254/lib/util/InterpolatingLong.java b/src/main/java/com/team254/lib/util/InterpolatingLong.java new file mode 100644 index 0000000..509a457 --- /dev/null +++ b/src/main/java/com/team254/lib/util/InterpolatingLong.java @@ -0,0 +1,47 @@ +package com.team254.lib.util; + +/** + * A Long that can be interpolated using the InterpolatingTreeMap. + * + * @see InterpolatingTreeMap + */ +public class InterpolatingLong implements Interpolable, + InverseInterpolable, + Comparable { + public Long value = 0L; + + public InterpolatingLong(Long val) { + value = val; + } + + @Override + public InterpolatingLong interpolate(InterpolatingLong other, double x) { + Long dydx = other.value - value; + Double searchY = dydx * x + value; + return new InterpolatingLong(searchY.longValue()); + } + + @Override + public double inverseInterpolate(InterpolatingLong upper, InterpolatingLong query) { + long upper_to_lower = upper.value - value; + if (upper_to_lower <= 0) { + return 0; + } + long query_to_lower = query.value - value; + if (query_to_lower <= 0) { + return 0; + } + return query_to_lower / (double) upper_to_lower; + } + + @Override + public int compareTo(InterpolatingLong other) { + if (other.value < value) { + return 1; + } else if (other.value > value) { + return -1; + } else { + return 0; + } + } +} diff --git a/src/main/java/com/team254/lib/util/InterpolatingTreeMap.java b/src/main/java/com/team254/lib/util/InterpolatingTreeMap.java new file mode 100644 index 0000000..0c1a329 --- /dev/null +++ b/src/main/java/com/team254/lib/util/InterpolatingTreeMap.java @@ -0,0 +1,81 @@ +package com.team254.lib.util; + +import java.util.Map; +import java.util.TreeMap; + +/** + * Interpolating Tree Maps are used to get values at points that are not defined by making a guess + * from points that are defined. This uses linear interpolation. + * + * @param The type of the key (must implement InverseInterpolable) + * @param The type of the value (must implement Interpolable) + */ +public class InterpolatingTreeMap & Comparable, V + extends Interpolable> extends TreeMap { + private static final long serialVersionUID = 8347275262778054124L; + + final int max_; + + public InterpolatingTreeMap(int maximumSize) { + max_ = maximumSize; + } + + public InterpolatingTreeMap() { + this(0); + } + + /** + * Inserts a key value pair, and trims the tree if a max size is specified + * + * @param key Key for inserted data + * @param value Value for inserted data + * @return the value + */ + @Override + public V put(K key, V value) { + if (max_ > 0 && max_ <= size()) { + // "Prune" the tree if it is oversize + K first = firstKey(); + remove(first); + } + + super.put(key, value); + + return value; + } + + @Override + public void putAll(Map map) { + System.out.println("Unimplemented Method"); + } + + /** + * @param key Lookup for a value (does not have to exist) + * @return V or null; V if it is Interpolable or exists, null if it is at a bound and cannot + * average + */ + public V getInterpolated(K key) { + V gotval = get(key); + if (gotval == null) { + // get surrounding keys for interpolation + K topBound = ceilingKey(key); + K bottomBound = floorKey(key); + + // if attempting interpolation at ends of tree, return the nearest data point + if (topBound == null && bottomBound == null) { + return null; + } else if (topBound == null) { + return get(bottomBound); + } else if (bottomBound == null) { + return get(topBound); + } + + // get surrounding values for interpolation + V topElem = get(topBound); + V bottomElem = get(bottomBound); + return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); + } else { + return gotval; + } + } +} diff --git a/src/main/java/com/team254/lib/util/InverseInterpolable.java b/src/main/java/com/team254/lib/util/InverseInterpolable.java new file mode 100644 index 0000000..1fe3e17 --- /dev/null +++ b/src/main/java/com/team254/lib/util/InverseInterpolable.java @@ -0,0 +1,23 @@ +package com.team254.lib.util; + +/** + * InverseInterpolable is an interface used by an Interpolating Tree as the Key type. Given two + * endpoint keys and a third query key, an InverseInterpolable object can calculate the + * interpolation parameter of the query key on the interval [0, 1]. + * + * @param The Type of InverseInterpolable + * @see InterpolatingTreeMap + */ +public interface InverseInterpolable { + /** + * Given this point (lower), a query point (query), and an upper point (upper), estimate how far + * (on [0, 1]) between 'lower' and 'upper' the query point lies. + * + * @param upper + * @param query + * @return The interpolation parameter on [0, 1] representing how far between this point and the + * upper point the + * query point lies. + */ + double inverseInterpolate(T upper, T query); +} diff --git a/src/main/java/com/team254/lib/util/LatchedBoolean.java b/src/main/java/com/team254/lib/util/LatchedBoolean.java new file mode 100644 index 0000000..bd34409 --- /dev/null +++ b/src/main/java/com/team254/lib/util/LatchedBoolean.java @@ -0,0 +1,19 @@ +package com.team254.lib.util; + +/** + * An iterative boolean latch. + *

+ * Returns true once if and only if the value of newValue changes from false to true. + */ +public class LatchedBoolean { + private boolean mLast = false; + + public boolean update(boolean newValue) { + boolean ret = false; + if (newValue && !mLast) { + ret = true; + } + mLast = newValue; + return ret; + } +} diff --git a/src/main/java/com/team254/lib/util/MinTimeBoolean.java b/src/main/java/com/team254/lib/util/MinTimeBoolean.java new file mode 100644 index 0000000..25533b3 --- /dev/null +++ b/src/main/java/com/team254/lib/util/MinTimeBoolean.java @@ -0,0 +1,28 @@ +package com.team254.lib.util; + +/** + * This boolean enforces a minimum time for the value to be true. It captures a rising edge and + * enforces based on timestamp. + */ +public class MinTimeBoolean { + private LatchedBoolean mLatchedBoolean; + private final double mMinTime; + private double mRisingEdgeTime; + + public MinTimeBoolean(double minTime) { + mLatchedBoolean = new LatchedBoolean(); + mMinTime = minTime; + mRisingEdgeTime = Double.NaN; + } + + public boolean update(boolean value, double timestamp) { + if (mLatchedBoolean.update(value)) { + mRisingEdgeTime = timestamp; + } + + if (!value && !Double.isNaN(mRisingEdgeTime) && (timestamp - mRisingEdgeTime < mMinTime)) { + return true; + } + return value; + } +} diff --git a/src/main/java/com/team254/lib/util/MovingAverage.java b/src/main/java/com/team254/lib/util/MovingAverage.java new file mode 100644 index 0000000..3ede9df --- /dev/null +++ b/src/main/java/com/team254/lib/util/MovingAverage.java @@ -0,0 +1,44 @@ +package com.team254.lib.util; + +import java.util.ArrayList; + +/** + * Helper class for storing and calculating a moving average + */ +public class MovingAverage { + ArrayList numbers = new ArrayList(); + private int maxSize; + + public MovingAverage(int maxSize) { + this.maxSize = maxSize; + } + + public void add(double newNumber) { + numbers.add(newNumber); + if (numbers.size() > maxSize) { + numbers.remove(0); + } + } + + public double getAverage() { + double total = 0; + + for (double number : numbers) { + total += number; + } + + return total / numbers.size(); + } + + public int getSize() { + return numbers.size(); + } + + public boolean isUnderMaxSize() { + return getSize() < maxSize; + } + + public void clear() { + numbers.clear(); + } +} diff --git a/src/main/java/com/team254/lib/util/MovingAverageTwist2d.java b/src/main/java/com/team254/lib/util/MovingAverageTwist2d.java new file mode 100644 index 0000000..7e32b8c --- /dev/null +++ b/src/main/java/com/team254/lib/util/MovingAverageTwist2d.java @@ -0,0 +1,48 @@ +package com.team254.lib.util; + +import com.team254.lib.geometry.Twist2d; +import java.util.ArrayList; + +/** + * Helper class for storing and calculating a moving average of the Twist2d class + */ +public class MovingAverageTwist2d { + ArrayList twists = new ArrayList(); + private int maxSize; + + public MovingAverageTwist2d(int maxSize) { + this.maxSize = maxSize; + } + + public synchronized void add(Twist2d twist) { + twists.add(twist); + if (twists.size() > maxSize) { + twists.remove(0); + } + } + + public synchronized Twist2d getAverage() { + double x = 0.0, y = 0.0, t = 0.0; + + for (Twist2d twist : twists) { + x += twist.dx; + y += twist.dy; + t += twist.dtheta; + } + + double size = getSize(); + return new Twist2d(x / size, y / size, t / size); + } + + public int getSize() { + return twists.size(); + } + + public boolean isUnderMaxSize() { + return getSize() < maxSize; + } + + public void clear() { + twists.clear(); + } +} diff --git a/src/main/java/com/team254/lib/util/MultiTrigger.java b/src/main/java/com/team254/lib/util/MultiTrigger.java new file mode 100644 index 0000000..ed2dfcc --- /dev/null +++ b/src/main/java/com/team254/lib/util/MultiTrigger.java @@ -0,0 +1,39 @@ +package com.team254.lib.util; + +/** + * Class to differentiate between tapping and holding a joystick button/trigger + */ +public class MultiTrigger { + private final double mTimeout; + private boolean lastPressed = false; + private final LatchedBoolean wasTapped = new LatchedBoolean(); + private final LatchedBoolean wasHeld = new LatchedBoolean(); + private boolean lastTapped = false; + private final TimeDelayedBoolean isHeld = new TimeDelayedBoolean(); + + public MultiTrigger(double timeout) { + mTimeout = timeout; + } + + public void update(boolean pressed) { + lastPressed = pressed; + lastTapped = wasTapped.update(pressed); + isHeld.update(pressed, mTimeout); + } + + public boolean wasTapped() { + return lastTapped; + } + + public boolean isPressed() { + return lastPressed; + } + + public boolean isHeld() { + return isHeld.update(lastPressed, mTimeout); + } + + public boolean holdStarted() { + return wasHeld.update(isHeld()); + } +} diff --git a/src/main/java/com/team254/lib/util/PolynomialRegression.java b/src/main/java/com/team254/lib/util/PolynomialRegression.java new file mode 100644 index 0000000..4994e6e --- /dev/null +++ b/src/main/java/com/team254/lib/util/PolynomialRegression.java @@ -0,0 +1,177 @@ +// NOTE: This file is available at +// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html +package com.team254.lib.util; + +/****************************************************************************** + * Compilation: javac -cp .:jama.jar PolynomialRegression.java + * Execution: java -cp .:jama.jar PolynomialRegression + * Dependencies: jama.jar StdOut.java + * + * % java -cp .:jama.jar PolynomialRegression + * 0.01 n^3 + -1.64 n^2 + 168.92 n + -2113.73 (R^2 = 0.997) + * + ******************************************************************************/ + +import Jama.Matrix; +import Jama.QRDecomposition; + +/** + * The {@code PolynomialRegression} class performs a polynomial regression on an set of N + * data points ( yi, xi). That is, it fits a polynomial + * y = β0 + β1 x + β2 + * x2 + ... + βd xd (where + * y is the response variable, x is the predictor variable, and the + * βi are the regression coefficients) that minimizes the sum of squared + * residuals of the multiple regression model. It also computes associated the coefficient of + * determination R2.

This implementation performs a QR-decomposition of the + * underlying Vandermonde matrix, so it is not the fastest or most numerically stable way to perform + * the polynomial regression. + * + * @author Robert Sedgewick + * @author Kevin Wayne + */ +public class PolynomialRegression { + private int degree; // degree of the polynomial regression + private Matrix beta; // the polynomial regression coefficients + private double sse; // sum of squares due to error + private double sst; // total sum of squares + + public PolynomialRegression(double[][] xy, int degree) { + double[] x = new double[xy.length]; + double[] y = new double[xy.length]; + for (int i = 0; i < xy.length; ++i) { + x[i] = xy[i][0]; + y[i] = xy[i][1]; + } + solve(x, y, degree); + } + + /** + * Performs a polynomial regression on the data points {@code (y[i], x[i])}. + * + * @param x the values of the predictor variable + * @param y the corresponding values of the response variable + * @param degree the degree of the polynomial to fit + */ + public PolynomialRegression(double[] x, double[] y, int degree) { + solve(x, y, degree); + } + + private void solve(double[] x, double[] y, int degree) { + this.degree = degree; + + int n = x.length; + QRDecomposition qr = null; + Matrix matrixX = null; + + // in case Vandermonde matrix does not have full rank, reduce degree until it does + while (true) { + // build Vandermonde matrix + double[][] vandermonde = new double[n][this.degree + 1]; + for (int i = 0; i < n; i++) { + for (int j = 0; j <= this.degree; j++) { + vandermonde[i][j] = Math.pow(x[i], j); + } + } + matrixX = new Matrix(vandermonde); + + // find least squares solution + qr = new QRDecomposition(matrixX); + if (qr.isFullRank()) + break; + + // decrease degree and try again + this.degree--; + } + + // create matrix from vector + Matrix matrixY = new Matrix(y, n); + + // linear regression coefficients + beta = qr.solve(matrixY); + + // mean of y[] values + double sum = 0.0; + for (int i = 0; i < n; i++) sum += y[i]; + double mean = sum / n; + + // total variation to be accounted for + for (int i = 0; i < n; i++) { + double dev = y[i] - mean; + sst += dev * dev; + } + + // variation not accounted for + Matrix residuals = matrixX.times(beta).minus(matrixY); + sse = residuals.norm2() * residuals.norm2(); + } + + /** + * Returns the {@code j}th regression coefficient. + * + * @param j the index + * @return the {@code j}th regression coefficient + */ + public double beta(int j) { + // to make -0.0 print as 0.0 + if (Math.abs(beta.get(j, 0)) < 1E-4) + return 0.0; + return beta.get(j, 0); + } + + /** + * Returns the degree of the polynomial to fit. + * + * @return the degree of the polynomial to fit + */ + public int degree() { + return degree; + } + + /** + * Returns the coefficient of determination R2. + * + * @return the coefficient of determination R2, which is a real number between + * 0 and 1 + */ + public double R2() { + if (sst == 0.0) + return 1.0; // constant function + return 1.0 - sse / sst; + } + + /** + * Returns the expected response {@code y} given the value of the predictor variable {@code x}. + * + * @param x the value of the predictor variable + * @return the expected response {@code y} given the value of the predictor variable {@code x} + */ + public double predict(double x) { + // horner's method + double y = 0.0; + for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } + + @Override + public String toString() { + StringBuilder s = new StringBuilder(); + int j = degree; + + // ignoring leading zero coefficients + while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; + + // create remaining terms + while (j >= 0) { + if (j == 0) + s.append(String.format("%.2f ", beta(j))); + else if (j == 1) + s.append(String.format("%.2f x + ", beta(j))); + else + s.append(String.format("%.2f x^%d + ", beta(j), j)); + j--; + } + s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); + return s.toString(); + } +} diff --git a/src/main/java/com/team254/lib/util/ReflectingCSVWriter.java b/src/main/java/com/team254/lib/util/ReflectingCSVWriter.java new file mode 100644 index 0000000..6bf4d8a --- /dev/null +++ b/src/main/java/com/team254/lib/util/ReflectingCSVWriter.java @@ -0,0 +1,76 @@ +package com.team254.lib.util; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.lang.reflect.Field; +import java.util.concurrent.ConcurrentLinkedDeque; + +/** + * Writes data to a CSV file + */ +public class ReflectingCSVWriter { + private ConcurrentLinkedDeque mLinesToWrite = new ConcurrentLinkedDeque<>(); + private PrintWriter mOutput = null; + private Field[] mFields; + + public ReflectingCSVWriter(String fileName, Class typeClass) { + mFields = typeClass.getFields(); + try { + mOutput = new PrintWriter(fileName); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + // Write field names. + StringBuilder line = new StringBuilder(); + for (Field field : mFields) { + if (line.length() != 0) { + line.append(", "); + } + line.append(field.getName()); + } + writeLine(line.toString()); + } + + public void add(T value) { + StringBuilder line = new StringBuilder(); + for (Field field : mFields) { + if (line.length() != 0) { + line.append(", "); + } + try { + if (CSVWritable.class.isAssignableFrom(field.getType())) { + line.append(((CSVWritable) field.get(value)).toCSV()); + } else { + line.append(field.get(value).toString()); + } + } catch (IllegalArgumentException | IllegalAccessException e) { + e.printStackTrace(); + } + } + mLinesToWrite.add(line.toString()); + } + + protected synchronized void writeLine(String line) { + if (mOutput != null) { + mOutput.println(line); + } + } + + // Call this periodically from any thread to write to disk. + public void write() { + while (true) { + String val = mLinesToWrite.pollFirst(); + if (val == null) { + break; + } + writeLine(val); + } + } + + public synchronized void flush() { + if (mOutput != null) { + write(); + mOutput.flush(); + } + } +} diff --git a/src/main/java/com/team254/lib/util/ShootingParameters.java b/src/main/java/com/team254/lib/util/ShootingParameters.java new file mode 100644 index 0000000..268b45d --- /dev/null +++ b/src/main/java/com/team254/lib/util/ShootingParameters.java @@ -0,0 +1,76 @@ +package com.team254.lib.util; + +import com.team254.lib.geometry.Pose2d; + +public class ShootingParameters { + private final InterpolatingTreeMap mOldBallHoodMap; + private final InterpolatingTreeMap mMediumBallHoodMap; + private final InterpolatingTreeMap mNewBallHoodMap; + private final InterpolatingTreeMap mShooterRPMMap; + private final Pose2d mVisionTargetToGoalOffset; + private final double mShooterAllowableErrorRPM; // rpm + private final double mTurretAllowableErrorMeters; // ° + private final double mHoodAllowableErrorDegrees; // ° + + public enum BallQuality { NEW_BALL, MEDIUM_BALL, OLD_BALL } + + public ShootingParameters( + InterpolatingTreeMap oldBallHoodMap, + InterpolatingTreeMap mediumBallHoodMap, + InterpolatingTreeMap newBallHoodMap, + InterpolatingTreeMap shooterRPMMap, + Pose2d visionTargetToGoalOffset, + double shooterAllowableErrorRPM, + double turretAllowableErrorMeters, + double hoodAllowableErrorDegrees + ) { + this.mOldBallHoodMap = oldBallHoodMap; + this.mMediumBallHoodMap = mediumBallHoodMap; + this.mNewBallHoodMap = newBallHoodMap; + this.mShooterRPMMap = shooterRPMMap; + this.mVisionTargetToGoalOffset = visionTargetToGoalOffset; + this.mShooterAllowableErrorRPM = shooterAllowableErrorRPM; + this.mTurretAllowableErrorMeters = turretAllowableErrorMeters; + this.mHoodAllowableErrorDegrees = hoodAllowableErrorDegrees; + } + + public InterpolatingTreeMap getHoodMap( + BallQuality ballQuality + ) { + switch (ballQuality) { + case NEW_BALL: + return mNewBallHoodMap; + case OLD_BALL: + return mOldBallHoodMap; + case MEDIUM_BALL: + default: + return mMediumBallHoodMap; + } + } + + public InterpolatingTreeMap getShooterRPMMap() { + return mShooterRPMMap; + } + + public Pose2d getVisionTargetToGoalOffset() { + return mVisionTargetToGoalOffset; + } + + public synchronized boolean + isShooterAtSetpoint(double current_shooter_rpm, double shooter_setpoint) { + return Util.epsilonEquals(current_shooter_rpm, shooter_setpoint, mShooterAllowableErrorRPM); + } + + public synchronized boolean + isTurretAtSetpoint(double current_turret_angle, double turret_setpoint, double range_meters) { + // This angle subtraction is okay because the turret is bounded to less than +/- 180 rotation. + final double angle_diff_degrees = turret_setpoint - current_turret_angle; + final double meters_error = + Math.tan(Units.degrees_to_radians(angle_diff_degrees)) * range_meters; + return Util.epsilonEquals(meters_error, 0.0, mTurretAllowableErrorMeters); + } + + public synchronized boolean isHoodAtSetpoint(double current_hood_angle, double hood_setpoint) { + return Util.epsilonEquals(current_hood_angle, hood_setpoint, mHoodAllowableErrorDegrees); + } +} diff --git a/src/main/java/com/team254/lib/util/StatFinder.java b/src/main/java/com/team254/lib/util/StatFinder.java new file mode 100644 index 0000000..0b5132a --- /dev/null +++ b/src/main/java/com/team254/lib/util/StatFinder.java @@ -0,0 +1,92 @@ +package com.team254.lib.util; + +import java.util.ArrayList; +import java.util.DoubleSummaryStatistics; +import java.util.List; + +/** + * Finds the stats (mean, standard deviation, etc.) of a list + *

+ * Example use case: finding out how long a planner takes from the average of 100 tries + */ +public class StatFinder { + private final List numbers = new ArrayList<>(); + private final int numberToIgnore; + private int numberIgnored = 0; + + private boolean stopped = false; + + /** + * Creates a new StatFinder + * + * @param numberToIgnore the number of entries to ignore before logging (like burning the top card + * of a deck before dealing) + */ + public StatFinder(int numberToIgnore) { + this.numberToIgnore = numberToIgnore; + } + + public boolean add(double number) { + if (stopped) { + return false; + } + + if (numberIgnored < numberToIgnore) { + numberIgnored++; + return false; + } + + numbers.add(number); + return true; + } + + public boolean addAndPrint(double number) { + boolean success = add(number); + + if (success) { + System.out.println("added: " + number); + } + + return success; + } + + public DoubleSummaryStatistics getStats() { + return numbers.stream().mapToDouble(x -> x).summaryStatistics(); + } + + public double getMean() { + return getStats().getAverage(); + } + + public double getStandardDeviation() { + double mean = getMean(); + return Math.sqrt( + numbers.stream().mapToDouble(x -> Math.pow(x - mean, 2)).sum() / (getSize() - 1) + ); + } + + public double getSize() { + return numbers.size(); + } + + public void printStats() { + System.out.println("mean: " + getMean()); + System.out.println("standard deviation: " + getStandardDeviation()); + System.out.println("min: " + getStats().getMin()); + System.out.println("max: " + getStats().getMax()); + System.out.println("size: " + getSize()); + } + + public void stop() { + stopped = true; + } + + public void stopAndPrint() { + if (stopped) { + return; + } + + stop(); + printStats(); + } +} diff --git a/src/main/java/com/team254/lib/util/StickyBoolean.java b/src/main/java/com/team254/lib/util/StickyBoolean.java new file mode 100644 index 0000000..4c711d2 --- /dev/null +++ b/src/main/java/com/team254/lib/util/StickyBoolean.java @@ -0,0 +1,26 @@ +package com.team254.lib.util; + +// Read a provided input +// When the input becomes true, output true until manually cleared +// Useful for latching +public class StickyBoolean { + private boolean mOn = false; + + public StickyBoolean() { + super(); + mOn = false; + } + + public boolean update(boolean input) { + mOn |= input; + return mOn; + } + + public void reset() { + mOn = false; + } + + public boolean get() { + return mOn; + } +} diff --git a/src/main/java/com/team254/lib/util/SynchronousPIDF.java b/src/main/java/com/team254/lib/util/SynchronousPIDF.java new file mode 100644 index 0000000..0ca6739 --- /dev/null +++ b/src/main/java/com/team254/lib/util/SynchronousPIDF.java @@ -0,0 +1,311 @@ +package com.team254.lib.util; + +import edu.wpi.first.hal.util.BoundaryException; +import edu.wpi.first.wpilibj.Timer; + +/** + * This class implements a PID Control Loop. + *

+ * Does all computation synchronously (i.e. the calculate() function must be + * called by the user from their own thread) + */ +public class SynchronousPIDF { + private double m_P; // factor for "proportional" control + private double m_I; // factor for "integral" control + private double m_D; // factor for "derivative" control + private double m_F; // factor for feed forward gain + private double m_maximumOutput = 1.0; // |maximum output| + private double m_minimumOutput = -1.0; // |minimum output| + private double m_maximumInput = 0.0; // maximum input - limit setpoint to this + private double m_minimumInput = 0.0; // minimum input - limit setpoint to this + private boolean m_continuous = false; // do the endpoints wrap around? eg. absolute encoder + private double m_prevError = 0.0; // the prior sensor input (used to compute velocity) + private double m_totalError = 0.0; // the sum of the errors for use in the integral calc + private double m_setpoint = 0.0; + private double m_error = 0.0; + private double m_result = 0.0; + private double m_last_input = Double.NaN; + private double m_deadband = 0.0; // If the absolute error is less than deadband then treat error + // for the proportional term as 0 + private double m_last_timestamp = Timer.getFPGATimestamp(); + + public SynchronousPIDF() {} + + /** + * Allocate a PID object with the given constants for P, I, D + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + */ + public SynchronousPIDF(double Kp, double Ki, double Kd) { + setPIDF(Kp, Ki, Kd, 0); + } + + /** + * Allocate a PID object with the given constants for P, I, D + * + * @param Kp the proportional coefficient + * @param Ki the integral coefficient + * @param Kd the derivative coefficient + * @param Kf the feed forward gain coefficient + */ + public SynchronousPIDF(double Kp, double Ki, double Kd, double Kf) { + setPIDF(Kp, Ki, Kd, Kf); + } + + public double calculate(double input) { + double timestamp = Timer.getFPGATimestamp(); + double dt = timestamp - m_last_timestamp; + m_last_timestamp = timestamp; + + return calculate(input, dt); + } + + /** + * Read the input, calculate the output accordingly, and write to the output. + * This should be called at a constant rate by the user (ex. in a timed thread) + * + * @param input the input + * @param dt time passed since previous call to calculate + */ + public double calculate(double input, double dt) { + if (dt < 1E-6) { + dt = 1E-6; + } + + m_last_input = input; + m_error = m_setpoint - input; + if (m_continuous) { + if (Math.abs(m_error) > (m_maximumInput - m_minimumInput) / 2) { + if (m_error > 0) { + m_error = m_error - m_maximumInput + m_minimumInput; + } else { + m_error = m_error + m_maximumInput - m_minimumInput; + } + } + } + + if (Util.inRange(m_error * m_P, m_minimumOutput, m_maximumOutput)) { + m_totalError += m_error * dt; + } else { + m_totalError = 0; + } + + // Don't blow away m_error so as to not break derivative + double proportionalError = Math.abs(m_error) < m_deadband ? 0 : m_error; + + m_result = + (m_P * proportionalError + m_I * m_totalError + m_D * (m_error - m_prevError) / dt + + m_F * m_setpoint); + m_prevError = m_error; + + return Util.limit(m_result, m_minimumOutput, m_maximumOutput); + } + + /** + * Set the PID controller gain parameters. Set the proportional, integral, and + * differential coefficients. + * + * @param p Proportional coefficient + * @param i Integral coefficient + * @param d Differential coefficient + */ + public void setPID(double p, double i, double d) { + m_P = p; + m_I = i; + m_D = d; + } + + /** + * Set the PID controller gain parameters. Set the proportional, integral, and + * differential coefficients. + * + * @param p Proportional coefficient + * @param i Integral coefficient + * @param d Differential coefficient + * @param f Feed forward coefficient + */ + public void setPIDF(double p, double i, double d, double f) { + setPID(p, i, d); + m_F = f; + } + + /** + * Get the Proportional coefficient + * + * @return proportional coefficient + */ + public double getP() { + return m_P; + } + + /** + * Get the Integral coefficient + * + * @return integral coefficient + */ + public double getI() { + return m_I; + } + + /** + * Get the Differential coefficient + * + * @return differential coefficient + */ + public double getD() { + return m_D; + } + + /** + * Get the Feed forward coefficient + * + * @return feed forward coefficient + */ + public double getF() { + return m_F; + } + + /** + * Return the current PID result This is always centered on zero and constrained + * the the max and min outs + * + * @return the latest calculated output + */ + public double get() { + return m_result; + } + + /** + * Set the PID controller to consider the input to be continuous, Rather then + * using the max and min in as constraints, it considers them to be the same + * point and automatically calculates the shortest route to the setpoint. + * + * @param continuous Set to true turns on continuous, false turns off continuous + */ + public void setContinuous(boolean continuous) { + m_continuous = continuous; + } + + public void setDeadband(double deadband) { + m_deadband = deadband; + } + + /** + * Set the PID controller to consider the input to be continuous, Rather then + * using the max and min in as constraints, it considers them to be the same + * point and automatically calculates the shortest route to the setpoint. + */ + public void setContinuous() { + this.setContinuous(true); + } + + /** + * Sets the maximum and minimum values expected from the input. + * + * @param minimumInput the minimum value expected from the input + * @param maximumInput the maximum value expected from the output + */ + public void setInputRange(double minimumInput, double maximumInput) { + if (minimumInput > maximumInput) { + throw new BoundaryException("Lower bound is greater than upper bound"); + } + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + setSetpoint(m_setpoint); + } + + /** + * Sets the minimum and maximum values to write. + * + * @param minimumOutput the minimum value to write to the output + * @param maximumOutput the maximum value to write to the output + */ + public void setOutputRange(double minimumOutput, double maximumOutput) { + if (minimumOutput > maximumOutput) { + throw new BoundaryException("Lower bound is greater than upper bound"); + } + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; + } + + public void setMaxAbsoluteOutput(double maxAbsoluteOutput) { + setOutputRange(-maxAbsoluteOutput, maxAbsoluteOutput); + } + + /** + * Set the setpoint for the PID controller + * + * @param setpoint the desired setpoint + */ + public void setSetpoint(double setpoint) { + if (m_maximumInput > m_minimumInput) { + if (setpoint > m_maximumInput) { + m_setpoint = m_maximumInput; + } else if (setpoint < m_minimumInput) { + m_setpoint = m_minimumInput; + } else { + m_setpoint = setpoint; + } + } else { + m_setpoint = setpoint; + } + } + + /** + * Returns the current setpoint of the PID controller + * + * @return the current setpoint + */ + public double getSetpoint() { + return m_setpoint; + } + + /** + * Returns the current difference of the input from the setpoint + * + * @return the current error + */ + public double getError() { + return m_error; + } + + /** + * Return true if the error is within the tolerance + * + * @return true if the error is less than the tolerance + */ + public boolean onTarget(double tolerance) { + return !Double.isNaN(m_last_input) && Math.abs(m_last_input - m_setpoint) < tolerance; + } + + /** + * Reset all internal terms. + */ + public void reset() { + m_last_input = Double.NaN; + m_prevError = 0; + m_totalError = 0; + m_result = 0; + m_setpoint = 0; + } + + public void resetIntegrator() { + m_totalError = 0; + } + + public String getState() { + String lState = ""; + + lState += "Kp: " + m_P + "\n"; + lState += "Ki: " + m_I + "\n"; + lState += "Kd: " + m_D + "\n"; + + return lState; + } + + public String getType() { + return "PIDController"; + } +} diff --git a/src/main/java/com/team254/lib/util/TimeDelayedBoolean.java b/src/main/java/com/team254/lib/util/TimeDelayedBoolean.java new file mode 100644 index 0000000..c510f3e --- /dev/null +++ b/src/main/java/com/team254/lib/util/TimeDelayedBoolean.java @@ -0,0 +1,22 @@ +package com.team254.lib.util; + +import edu.wpi.first.wpilibj.Timer; + +/** + * This class contains a boolean value and a timer. It can set its boolean value and return whether + * the timer is within a set timeout. This returns true if the stored value is true and the timeout + * has expired. + */ +public class TimeDelayedBoolean { + private Timer t = new Timer(); + private boolean m_old = false; + + public boolean update(boolean value, double timeout) { + if (!m_old && value) { + t.reset(); + t.start(); + } + m_old = value; + return value && t.get() >= timeout; + } +} diff --git a/src/main/java/com/team254/lib/util/ToggleBoolean.java b/src/main/java/com/team254/lib/util/ToggleBoolean.java new file mode 100644 index 0000000..3dca474 --- /dev/null +++ b/src/main/java/com/team254/lib/util/ToggleBoolean.java @@ -0,0 +1,17 @@ +package com.team254.lib.util; + +public class ToggleBoolean { + private boolean released = true; + private boolean retVal = false; + + public boolean update(boolean newValue) { + if (newValue && released) { + released = false; + retVal = !retVal; + } + if (!newValue) { + released = true; + } + return retVal; + } +} diff --git a/src/main/java/com/team254/lib/util/TuningLogger.java b/src/main/java/com/team254/lib/util/TuningLogger.java new file mode 100644 index 0000000..cd4cd41 --- /dev/null +++ b/src/main/java/com/team254/lib/util/TuningLogger.java @@ -0,0 +1,31 @@ +package com.team254.lib.util; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileWriter; +import java.io.IOException; +import java.io.PrintWriter; +import java.text.DecimalFormat; + +public class TuningLogger { + private PrintWriter mWriter = null; + + public TuningLogger(String filePath) { + try { + mWriter = new PrintWriter(new FileWriter(filePath)); + } catch (IOException ignored) { + } + } + + public void logData(String mapName, double range, double value) { + if (mWriter != null) { + String line = mapName + ".put(new InterpolatingDouble(" + range + + "), new InterpolatingDouble(" + value + "));\n"; + mWriter.write(line); + mWriter.flush(); + System.out.println("Wrote: " + line); + } else { + System.err.println("No print writer created to write " + mapName); + } + } +} diff --git a/src/main/java/com/team254/lib/util/Units.java b/src/main/java/com/team254/lib/util/Units.java new file mode 100644 index 0000000..68c94c7 --- /dev/null +++ b/src/main/java/com/team254/lib/util/Units.java @@ -0,0 +1,38 @@ +package com.team254.lib.util; + +public class Units { + public static final double kGravityInPerSecSq = 386.09; + public static final double kInchesToMeters = 0.0254; + + public static double rpm_to_rads_per_sec(double rpm) { + return rpm * 2.0 * Math.PI / 60.0; + } + + public static double rads_per_sec_to_rpm(double rads_per_sec) { + return rads_per_sec * 60.0 / (2.0 * Math.PI); + } + + public static double inches_to_meters(double inches) { + return inches * kInchesToMeters; + } + + public static double meters_to_inches(double meters) { + return meters / kInchesToMeters; + } + + public static double feet_to_meters(double feet) { + return inches_to_meters(feet * 12.0); + } + + public static double meters_to_feet(double meters) { + return meters_to_inches(meters) / 12.0; + } + + public static double degrees_to_radians(double degrees) { + return Math.toRadians(degrees); + } + + public static double radians_to_degrees(double radians) { + return Math.toDegrees(radians); + } +} diff --git a/src/main/java/com/team254/lib/util/Util.java b/src/main/java/com/team254/lib/util/Util.java new file mode 100644 index 0000000..2102dff --- /dev/null +++ b/src/main/java/com/team254/lib/util/Util.java @@ -0,0 +1,82 @@ +package com.team254.lib.util; + +import java.util.List; + +/** + * Contains basic functions that are used often. + */ +public class Util { + public static final double kEpsilon = 1e-12; + + /** + * Prevent this class from being instantiated. + */ + private Util() {} + + /** + * Limits the given input to the given magnitude. + */ + public static double limit(double v, double maxMagnitude) { + return limit(v, -maxMagnitude, maxMagnitude); + } + + public static double limit(double v, double min, double max) { + return Math.min(max, Math.max(min, v)); + } + + public static boolean inRange(double v, double maxMagnitude) { + return inRange(v, -maxMagnitude, maxMagnitude); + } + + /** + * Checks if the given input is within the range (min, max), both exclusive. + */ + public static boolean inRange(double v, double min, double max) { + return v > min && v < max; + } + + public static double interpolate(double a, double b, double x) { + x = limit(x, 0.0, 1.0); + return a + (b - a) * x; + } + + public static String joinStrings(final String delim, final List strings) { + StringBuilder sb = new StringBuilder(); + for (int i = 0; i < strings.size(); ++i) { + sb.append(strings.get(i).toString()); + if (i < strings.size() - 1) { + sb.append(delim); + } + } + return sb.toString(); + } + + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, kEpsilon); + } + + public static boolean epsilonEquals(int a, int b, int epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean allCloseTo(final List list, double value, double epsilon) { + boolean result = true; + for (Double value_in : list) { + result &= epsilonEquals(value_in, value, epsilon); + } + return result; + } + + public static double handleDeadband(double value, double deadband) { + deadband = Math.abs(deadband); + if (deadband == 1) { + return 0; + } + double scaledValue = (value + (value < 0 ? deadband : -deadband)) / (1 - deadband); + return (Math.abs(value) > Math.abs(deadband)) ? scaledValue : 0; + } +} diff --git a/src/main/java/com/team254/lib/vision/AimingParameters.java b/src/main/java/com/team254/lib/vision/AimingParameters.java new file mode 100644 index 0000000..4188f1f --- /dev/null +++ b/src/main/java/com/team254/lib/vision/AimingParameters.java @@ -0,0 +1,53 @@ +package com.team254.lib.vision; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; + +public class AimingParameters { + private final double range; + private final Pose2d field_to_vehicle; + private final Pose2d field_to_goal; + private final Rotation2d254 robot_to_goal_rotation; + private final int track_id; + private final boolean is_latest; + + public AimingParameters( + Pose2d field_to_vehicle, Pose2d field_to_goal, int track_id, boolean is_latest + ) { + this.field_to_vehicle = field_to_vehicle; + this.field_to_goal = field_to_goal; + final Pose2d vehicle_to_goal = field_to_vehicle.inverse().transformBy(field_to_goal); + this.range = vehicle_to_goal.getTranslation().norm(); + this.robot_to_goal_rotation = vehicle_to_goal.getTranslation().direction(); + this.track_id = track_id; + this.is_latest = is_latest; + } + + public boolean getIsLatest() { + return this.is_latest; + } + + public Pose2d getFieldToVehicle() { + return field_to_vehicle; + } + + public Pose2d getFieldToGoal() { + return field_to_goal; + } + + public double getRange() { + return range; + } + + public Pose2d getVehicleToGoal() { + return field_to_vehicle.inverse().transformBy(field_to_goal); + } + + public Rotation2d254 getRobotToGoalRotation() { + return robot_to_goal_rotation; + } + + public int getTrackId() { + return track_id; + } +} diff --git a/src/main/java/com/team254/lib/vision/GoalTrack.java b/src/main/java/com/team254/lib/vision/GoalTrack.java new file mode 100644 index 0000000..13e575f --- /dev/null +++ b/src/main/java/com/team254/lib/vision/GoalTrack.java @@ -0,0 +1,145 @@ +package com.team254.lib.vision; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import edu.wpi.first.wpilibj.Timer; +import java.util.Map; +import java.util.TreeMap; + +/** + * A class that is used to keep track of all goals detected by the vision system. As goals are + * detected/not detected anymore by the vision system, function calls will be made to create, + * destroy, or update a goal track.

This helps in the goal ranking process that determines which + * goal to fire into, and helps to smooth measurements of the goal's location over time. + * + * @see GoalTracker + */ +public class GoalTrack { + final GoalTracker.Configuration mConfiguration; + TreeMap mObservedPositions = new TreeMap<>(); + Pose2d mSmoothedPosition = null; + int mId; + + private GoalTrack(GoalTracker.Configuration config) { + mConfiguration = config; + } + + public synchronized void emptyUpdate() { + pruneByTime(); + } + + /** + * Attempts to update the track with a new observation. + * + * @return True if the track was updated + */ + public synchronized boolean tryUpdate(double timestamp, Pose2d new_observation) { + if (!isAlive()) { + return false; + } + double distance = + mSmoothedPosition.inverse() + .transformBy(new_observation) + .getTranslation() + .norm(); // Distance between new observation and average position of Goal Tracker + if (distance < mConfiguration.kMaxTrackerDistance) { + mObservedPositions.put(timestamp, new_observation); + pruneByTime(); + return true; + } else { + emptyUpdate(); + return false; + } + } + + public synchronized boolean isAlive() { + return mObservedPositions.size() > 0; + } + + /** + * Removes the track if it is older than the set "age" described in the Constants file. + * + * @see Constants + */ + synchronized void pruneByTime() { + double delete_before = Timer.getFPGATimestamp() - mConfiguration.kMaxGoalTrackAge; + mObservedPositions.entrySet().removeIf(entry -> entry.getKey() < delete_before); + if (mObservedPositions.isEmpty()) { + mSmoothedPosition = null; + } else { + smooth(); + } + } + + /** + * Makes a new track based on the timestamp and the goal's coordinates (from vision) + */ + public static GoalTrack makeNewTrack( + GoalTracker.Configuration config, double timestamp, Pose2d first_observation, int id + ) { + GoalTrack rv = new GoalTrack(config); + rv.mObservedPositions.put(timestamp, first_observation); + rv.mSmoothedPosition = first_observation; + rv.mId = id; + return rv; + } + + /** + * Averages out the observed positions based on an set of observed positions + */ + synchronized void smooth() { + if (isAlive()) { + double x = 0; + double y = 0; + double s = 0; // sin of angle + double c = 0; // cos of angle + double t_now = Timer.getFPGATimestamp(); + int num_samples = 0; + for (Map.Entry entry : mObservedPositions.entrySet()) { + if (t_now - entry.getKey() > mConfiguration.kMaxGoalTrackSmoothingTime) { + continue; + } + ++num_samples; + x += entry.getValue().getTranslation().x(); + y += entry.getValue().getTranslation().y(); + c += entry.getValue().getRotation().cos(); + s += entry.getValue().getRotation().sin(); + } + x /= num_samples; + y /= num_samples; + s /= num_samples; + c /= num_samples; + + if (num_samples == 0) { + // Handle the case that all samples are older than kMaxGoalTrackSmoothingTime. + mSmoothedPosition = mObservedPositions.lastEntry().getValue(); + } else { + mSmoothedPosition = new Pose2d(x, y, new Rotation2d254(c, s, true)); + } + } + } + + public synchronized Pose2d getSmoothedPosition() { + return mSmoothedPosition; + } + + public synchronized Pose2d getLatestPosition() { + return mObservedPositions.lastEntry().getValue(); + } + + public synchronized double getLatestTimestamp() { + return mObservedPositions.keySet().stream().max(Double::compareTo).orElse(0.0); + } + + public synchronized double getStability() { + return Math.min( + 1.0, + mObservedPositions.size() + / (mConfiguration.kCameraFrameRate * mConfiguration.kMaxGoalTrackAge) + ); + } + + public synchronized int getId() { + return mId; + } +} diff --git a/src/main/java/com/team254/lib/vision/GoalTracker.java b/src/main/java/com/team254/lib/vision/GoalTracker.java new file mode 100644 index 0000000..46d6bfa --- /dev/null +++ b/src/main/java/com/team254/lib/vision/GoalTracker.java @@ -0,0 +1,156 @@ +package com.team254.lib.vision; + +import com.team254.lib.geometry.Pose2d; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; + +/** + * This is used in the event that multiple goals are detected to judge all goals based on timestamp, + * stability, and continuation of previous goals (i.e. if a goal was detected earlier and has + * changed locations). This allows the robot to make consistent decisions about which goal to aim at + * and to smooth out jitter from vibration of the camera. + * + * @see GoalTrack + */ +public class GoalTracker { + /** + * Track reports contain all of the relevant information about a given goal track. + */ + public static class TrackReport { + // Transform from the field frame to the vision target. + public Pose2d field_to_target; + + // The timestamp of the latest time that the goal has been observed + public double latest_timestamp; + + // The percentage of the goal tracking time during which this goal has + // been observed (0 to 1) + public double stability; + + // The track id + public int id; + + public TrackReport(GoalTrack track) { + this.field_to_target = track.getSmoothedPosition(); + this.latest_timestamp = track.getLatestTimestamp(); + this.stability = track.getStability(); + this.id = track.getId(); + } + } + + /** + * TrackReportComparators are used in the case that multiple tracks are active (e.g. we see or + * have recently seen multiple goals). They contain heuristics used to pick which track we should + * aim at by calculating a score for each track (highest score wins). + */ + public static class TrackReportComparator implements Comparator { + final Configuration mConfiguration; + double mCurrentTimestamp; + int mLastTrackId; + + protected TrackReportComparator( + Configuration config, int last_track_id, double current_timestamp + ) { + this.mConfiguration = config; + this.mLastTrackId = last_track_id; + this.mCurrentTimestamp = current_timestamp; + } + + double score(TrackReport report) { + double stability_score = mConfiguration.kStabilityWeight * report.stability; + double age_score = mConfiguration.kAgeWeight + * Math.max( + 0, + (mConfiguration.kMaxGoalTrackAge - (mCurrentTimestamp - report.latest_timestamp)) + / mConfiguration.kMaxGoalTrackAge + ); + double switching_score = (report.id == mLastTrackId ? mConfiguration.kSwitchingWeight : 0); + return stability_score + age_score + switching_score; + } + + @Override + public int compare(TrackReport o1, TrackReport o2) { + double diff = score(o1) - score(o2); + // Greater than 0 if o1 is better than o2 + if (diff < 0) { + return 1; + } else if (diff > 0) { + return -1; + } else { + return 0; + } + } + } + + public static class Configuration { + public double kMaxTrackerDistance = 0.0; + public double kMaxGoalTrackAge = 1.5; // s + public double kMaxGoalTrackSmoothingTime = 0.1; // s + public double kCameraFrameRate = 90.0; // Hz + + // Reward tracks for being more stable (seen in more frames) + public double kStabilityWeight = 1.0; + // Reward tracks for being recently observed + public double kAgeWeight = 1.0; + // Reward tracks for being continuations of tracks that we are already + // tracking + public double kSwitchingWeight = 1.0; + } + + List mCurrentTracks = new ArrayList<>(); + int mNextId = 0; + final Configuration mConfiguration; + + public GoalTracker(Configuration config) { + mConfiguration = config; + } + + public synchronized void reset() { + mCurrentTracks.clear(); + } + + public synchronized TrackReportComparator getComparator(int last_track_id, double timestamp) { + return new TrackReportComparator(mConfiguration, last_track_id, timestamp); + } + + public synchronized void update(double timestamp, List field_to_goals) { + // Try to update existing tracks + for (Pose2d target : field_to_goals) { + boolean hasUpdatedTrack = false; + for (GoalTrack track : mCurrentTracks) { + if (!hasUpdatedTrack) { + if (track.tryUpdate(timestamp, target)) { + hasUpdatedTrack = true; + } + } else { + track.emptyUpdate(); + } + } + if (!hasUpdatedTrack) { + // Add a new track. + // System.out.println("Created new track"); + mCurrentTracks.add(GoalTrack.makeNewTrack(mConfiguration, timestamp, target, mNextId)); + ++mNextId; + } + } + + maybePruneTracks(); + } + + public synchronized void maybePruneTracks() { + mCurrentTracks.removeIf(track -> !track.isAlive()); + } + + public synchronized boolean hasTracks() { + return !mCurrentTracks.isEmpty(); + } + + public synchronized List getTracks() { + List rv = new ArrayList<>(); + for (GoalTrack track : mCurrentTracks) { + rv.add(new TrackReport(track)); + } + return rv; + } +} diff --git a/src/main/java/com/team254/lib/vision/TargetInfo.java b/src/main/java/com/team254/lib/vision/TargetInfo.java new file mode 100644 index 0000000..4733ac0 --- /dev/null +++ b/src/main/java/com/team254/lib/vision/TargetInfo.java @@ -0,0 +1,37 @@ +package com.team254.lib.vision; + +/** + * A container class for Targets detected by the vision system, containing the location in + * three-dimensional space. + */ +public class TargetInfo { + protected double x = 1.0; + protected double y; + protected double z; + protected double skew; + + public TargetInfo(double y, double z) { + this.y = y; + this.z = z; + } + + public void setSkew(double skew) { + this.skew = skew; + } + + public double getX() { + return x; + } + + public double getY() { + return y; + } + + public double getZ() { + return z; + } + + public double getSkew() { + return skew; + } +} diff --git a/src/main/java/com/team254/lib/wpilib/IterativeRobotBase.java b/src/main/java/com/team254/lib/wpilib/IterativeRobotBase.java new file mode 100644 index 0000000..c326f0b --- /dev/null +++ b/src/main/java/com/team254/lib/wpilib/IterativeRobotBase.java @@ -0,0 +1,321 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.team254.lib.wpilib; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Watchdog; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +// import edu.wpi.first.wpilibj.DriverStation; + +/** + * IterativeRobotBase implements a specific type of robot program framework, + * extending the RobotBase class. + * + *

+ * The IterativeRobotBase class does not implement startCompetition(), so it + * should not be used by teams directly. + * + *

+ * This class provides the following functions which are called by the main + * loop, startCompetition(), at the appropriate times: + * + *

+ * robotInit() -- provide for initialization at robot power-on + * + *

+ * init() functions -- each of the following functions is called once when the + * appropriate mode is entered: - disabledInit() -- called each and every time + * disabled is entered from another mode - autonomousInit() -- called each and + * every time autonomous is entered from another mode - teleopInit() -- called + * each and every time teleop is entered from another mode - testInit() -- + * called each and every time test is entered from another mode + * + *

+ * periodic() functions -- each of these functions is called on an interval: - + * robotPeriodic() - disabledPeriodic() - autonomousPeriodic() - + * teleopPeriodic() - testPeriodic() + */ +@SuppressWarnings("PMD.TooManyMethods") +public abstract class IterativeRobotBase extends RobotBase { + protected double m_period; + + private enum Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest } + + private Mode m_lastMode = Mode.kNone; + private final Watchdog m_watchdog; + + /** + * Constructor for IterativeRobotBase. + * + * @param period Period in seconds. + */ + protected IterativeRobotBase(double period) { + m_period = period; + m_watchdog = new Watchdog(period, this::printLoopOverrunMessage); + m_watchdog.suppressTimeoutMessage(true); + } + + /** + * Provide an alternate "main loop" via startCompetition(). + */ + @Override public abstract void startCompetition(); + + /* ----------- Overridable initialization code ----------------- */ + + /** + * Robot-wide initialization code should go here. + * + *

+ * Users should override this method for default Robot-wide initialization which + * will be called when the robot is first powered on. It will be called exactly + * one time. + * + *

+ * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" + * indicators will be off until RobotInit() exits. Code in RobotInit() that + * waits for enable will cause the robot to never indicate that the code is + * ready, causing the robot to be bypassed in a match. + */ + public void robotInit() { + System.out.println("Default robotInit() method... Override me!"); + } + + /** + * Robot-wide simulation initialization code should go here. + * + *

+ * Users should override this method for default Robot-wide simulation related + * initialization which will be called when the robot is first started. It will + * be called exactly one time after RobotInit is called only when the robot is + * in simulation. + */ + public void simulationInit() { + System.out.println("Default simulationInit() method... Override me!"); + } + + /** + * Initialization code for disabled mode should go here. + * + *

+ * Users should override this method for initialization code which will be + * called each time the robot enters disabled mode. + */ + public void disabledInit() { + System.out.println("Default disabledInit() method... Override me!"); + } + + /** + * Initialization code for autonomous mode should go here. + * + *

+ * Users should override this method for initialization code which will be + * called each time the robot enters autonomous mode. + */ + public void autonomousInit() { + System.out.println("Default autonomousInit() method... Override me!"); + } + + /** + * Initialization code for teleop mode should go here. + * + *

+ * Users should override this method for initialization code which will be + * called each time the robot enters teleop mode. + */ + public void teleopInit() { + System.out.println("Default teleopInit() method... Override me!"); + } + + /** + * Initialization code for test mode should go here. + * + *

+ * Users should override this method for initialization code which will be + * called each time the robot enters test mode. + */ + @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") + public void testInit() { + System.out.println("Default testInit() method... Override me!"); + } + + /* ----------- Overridable periodic code ----------------- */ + + private boolean m_rpFirstRun = true; + + /** + * Periodic code for all robot modes should go here. + */ + public void robotPeriodic() { + if (m_rpFirstRun) { + System.out.println("Default robotPeriodic() method... Override me!"); + m_rpFirstRun = false; + } + } + + private boolean m_spFirstRun = true; + + /** + * Periodic simulation code should go here. + * + *

+ * This function is called in a simulated robot after user code executes. + */ + public void simulationPeriodic() { + if (m_spFirstRun) { + System.out.println("Default simulationPeriodic() method... Override me!"); + m_spFirstRun = false; + } + } + + private boolean m_dpFirstRun = true; + + /** + * Periodic code for disabled mode should go here. + */ + public void disabledPeriodic() { + if (m_dpFirstRun) { + System.out.println("Default disabledPeriodic() method... Override me!"); + m_dpFirstRun = false; + } + } + + private boolean m_apFirstRun = true; + + /** + * Periodic code for autonomous mode should go here. + */ + public void autonomousPeriodic() { + if (m_apFirstRun) { + System.out.println("Default autonomousPeriodic() method... Override me!"); + m_apFirstRun = false; + } + } + + private boolean m_tpFirstRun = true; + + /** + * Periodic code for teleop mode should go here. + */ + public void teleopPeriodic() { + if (m_tpFirstRun) { + System.out.println("Default teleopPeriodic() method... Override me!"); + m_tpFirstRun = false; + } + } + + private boolean m_tmpFirstRun = true; + + /** + * Periodic code for test mode should go here. + */ + @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") + public void testPeriodic() { + if (m_tmpFirstRun) { + System.out.println("Default testPeriodic() method... Override me!"); + m_tmpFirstRun = false; + } + } + + @SuppressWarnings("PMD.CyclomaticComplexity") + protected void loopFunc() { + m_watchdog.reset(); + + // Call the appropriate function depending upon the current robot mode + if (isDisabled()) { + // Call DisabledInit() if we are now just entering disabled mode from either a + // different mode + // or from power-on. + if (m_lastMode != Mode.kDisabled) { + LiveWindow.setEnabled(false); + Shuffleboard.disableActuatorWidgets(); + disabledInit(); + m_watchdog.addEpoch("disabledInit()"); + m_lastMode = Mode.kDisabled; + } + + // HAL.observeUserProgramDisabled(); + disabledPeriodic(); + m_watchdog.addEpoch("disablePeriodic()"); + } else if (isAutonomous()) { + // Call AutonomousInit() if we are now just entering autonomous mode from either + // a different + // mode or from power-on. + if (m_lastMode != Mode.kAutonomous) { + LiveWindow.setEnabled(false); + Shuffleboard.disableActuatorWidgets(); + autonomousInit(); + m_watchdog.addEpoch("autonomousInit()"); + m_lastMode = Mode.kAutonomous; + } + + // HAL.observeUserProgramAutonomous(); + autonomousPeriodic(); + m_watchdog.addEpoch("autonomousPeriodic()"); + } else if (isTeleop()) { + // Call TeleopInit() if we are now just entering teleop mode from either a + // different mode or + // from power-on. + if (m_lastMode != Mode.kTeleop) { + LiveWindow.setEnabled(false); + Shuffleboard.disableActuatorWidgets(); + teleopInit(); + m_watchdog.addEpoch("teleopInit()"); + m_lastMode = Mode.kTeleop; + } + + // HAL.observeUserProgramTeleop(); + teleopPeriodic(); + m_watchdog.addEpoch("teleopPeriodic()"); + } else { + // Call TestInit() if we are now just entering test mode from either a different + // mode or from + // power-on. + if (m_lastMode != Mode.kTest) { + LiveWindow.setEnabled(true); + Shuffleboard.enableActuatorWidgets(); + testInit(); + m_watchdog.addEpoch("testInit()"); + m_lastMode = Mode.kTest; + } + + // HAL.observeUserProgramTest(); + testPeriodic(); + m_watchdog.addEpoch("testPeriodic()"); + } + + robotPeriodic(); + m_watchdog.addEpoch("robotPeriodic()"); + + SmartDashboard.updateValues(); + m_watchdog.addEpoch("SmartDashboard.updateValues()"); + LiveWindow.updateValues(); + m_watchdog.addEpoch("LiveWindow.updateValues()"); + Shuffleboard.update(); + m_watchdog.addEpoch("Shuffleboard.update()"); + + if (isSimulation()) { + simulationPeriodic(); + m_watchdog.addEpoch("simulationPeriodic()"); + } + + m_watchdog.disable(); + + // Warn on loop time overruns + if (m_watchdog.isExpired()) { + // m_watchdog.printEpochs(); + } + } + + private void printLoopOverrunMessage() { + // DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false); + } +} diff --git a/src/main/java/com/team254/lib/wpilib/TimedRobot.java b/src/main/java/com/team254/lib/wpilib/TimedRobot.java new file mode 100644 index 0000000..095ce14 --- /dev/null +++ b/src/main/java/com/team254/lib/wpilib/TimedRobot.java @@ -0,0 +1,117 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.team254.lib.wpilib; + +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.NotifierJNI; +import edu.wpi.first.wpilibj.RobotController; + +/** + * TimedRobot implements the IterativeRobotBase robot program framework. + * + *

+ * The TimedRobot class is intended to be subclassed by a user creating a robot + * program. + * + *

+ * periodic() functions from the base class are called on an interval by a + * Notifier instance. + */ +public class TimedRobot extends IterativeRobotBase { + public static final double kDefaultPeriod = 0.02; + + // The C pointer to the notifier object. We don't use it directly, it is + // just passed to the JNI bindings. + private final int m_notifier = NotifierJNI.initializeNotifier(); + + // The absolute expiration time + private double m_expirationTime; + + /** + * Constructor for TimedRobot. + */ + protected TimedRobot() { + this(kDefaultPeriod); + } + + /** + * Constructor for TimedRobot. + * + * @param period Period in seconds. + */ + protected TimedRobot(double period) { + super(period); + NotifierJNI.setNotifierName(m_notifier, "TimedRobot"); + + HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed); + } + + @Override + @SuppressWarnings("NoFinalizer") + protected void finalize() { + NotifierJNI.stopNotifier(m_notifier); + NotifierJNI.cleanNotifier(m_notifier); + } + + /** + * Provide an alternate "main loop" via startCompetition(). + */ + @Override + @SuppressWarnings("UnsafeFinalization") + public void startCompetition() { + robotInit(); + + if (isSimulation()) { + simulationInit(); + } + + // Tell the DS that the robot is ready to be enabled + // HAL.observeUserProgramStarting(); + + m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period; + updateAlarm(); + + // Loop forever, calling the appropriate mode-dependent function + while (true) { + long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); + if (curTime == 0) { + break; + } + + m_expirationTime += m_period; + updateAlarm(); + + loopFunc(); + } + } + + /** + * Ends the main loop in startCompetition(). + */ + @Override + public void endCompetition() { + NotifierJNI.stopNotifier(m_notifier); + } + + /** + * Get time period between calls to Periodic() functions. + */ + public double getPeriod() { + return m_period; + } + + /** + * Update the alarm hardware to reflect the next alarm. + */ + @SuppressWarnings("UnsafeFinalization") + private void updateAlarm() { + NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6)); + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..fe215d7 --- /dev/null +++ b/src/main/java/frc/robot/Main.java @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +public final class Main { + private Main() {} + + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java new file mode 100644 index 0000000..6730171 --- /dev/null +++ b/src/main/java/frc/robot/Ports.java @@ -0,0 +1,134 @@ +package frc.robot; + +import edu.wpi.first.wpilibj.PneumaticsModuleType; + +public class Ports { + public static final int DRIVER_XBOX_USB_PORT = 0; + public static final int OPERATOR_PS4_USB_PORT = 1; + public static final int PIT_XBOX_USB_PORT = 2; + + public enum ClawPorts { + COMP_PORTS(24, 25, 9), + PRACTICE_PORTS(24, 25, 9), + WRISTED_KLAW_PORTS(24, 25, 9); + + public final int leftRoller; + public final int rightRoller; + public final int breakBeam; + + // Unused until wrist motorized + // public final int wrist; + // public final int wristEncoder; + + ClawPorts(int leftRoller, int rightRoller, int breakBeam) { + this.leftRoller = leftRoller; + this.rightRoller = rightRoller; + this.breakBeam = breakBeam; + } + } + + public enum DrivetrainPorts { + COMP_PORTS( + "Default Name", + new int[] {10, 16, 12, 14}, + new int[] {11, 17, 13, 15}, + new int[] {1, 3, 0, 2}, + "Default Name", + 35 + ), + PRACTICE_PORTS( + "Default Name", + new int[] {10, 16, 12, 14}, + new int[] {11, 17, 13, 15}, + new int[] {1, 2, 0, 3}, + "Default Name", + 35 + ), + SWERVE_BASE_PORTS( + "rio", + new int[] {11, 16, 12, 14}, + new int[] {10, 17, 13, 15}, + new int[] {30, 33, 31, 32}, + "rio", + 35 + ); + + public final String moduleCanBus; + public final int[] drive; + public final int[] steer; + public final int[] encoder; + public final String pigeonCanBus; + public final int pigeonID; + + DrivetrainPorts( + String moduleCanBus, + int[] drive, + int[] steer, + int[] encoder, + String pigeonCanBus, + int pigeonID + ) { + this.moduleCanBus = moduleCanBus; + this.drive = drive; + this.steer = steer; + this.encoder = encoder; + this.pigeonCanBus = pigeonCanBus; + this.pigeonID = pigeonID; + } + } + + public enum ElevatorPorts { + COMP_PORTS(20, 21, -1), + PRACTICE_PORTS(20, 21, -1); + + public final int primaryMotor; + public final int followerMotor; + public final int limitSwitch; + + ElevatorPorts(int primary, int follower, int limitSwitch) { + primaryMotor = primary; + followerMotor = follower; + this.limitSwitch = limitSwitch; + } + } + + public enum LedPorts { + COMP_PORTS("", 50), + PRACTICE_PORTS("", 50), + SWERVE_BASE_PORTS("", 50); + + public final int candle; + public final String candleCanBus; + + LedPorts(String candleCanBus, int candle) { + this.candle = candle; + this.candleCanBus = candleCanBus; + } + } + + public enum PneumaticPorts { + COMP_PORTS(PneumaticsModuleType.CTREPCM, 42, 1, 6, 0, 5), + PRACTICE_PORTS(PneumaticsModuleType.CTREPCM, 42, 4, 2, 1, 3); + + public final PneumaticsModuleType moduleType; + public final int moduleID; + public final int clawForward, clawReverse; + public final int intakeForward, intakeReverse; + + PneumaticPorts( + PneumaticsModuleType type, + int moduleID, + int clawForward, + int clawReverse, + int intakeForward, + int intakeReverse + ) { + this.moduleType = type; + this.moduleID = moduleID; + this.clawForward = clawForward; + this.clawReverse = clawReverse; + this.intakeForward = intakeForward; + this.intakeReverse = intakeReverse; + } + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..d8f1f34 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,111 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; 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.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotAltModes; +import frc.robot.utils.LoopTimer; + +public class Robot extends TimedRobot { + private Command _autonomousCommand; + + private final RobotContainer _robotContainer; + private final CommandScheduler _scheduler; + + public Robot() { + super(Constants.LOOP_PERIOD_S); + + LoopTimer.markLoopStart(); + + _scheduler = CommandScheduler.getInstance(); + _robotContainer = new RobotContainer(); + } + + @Override + public void robotInit() { + _robotContainer.configFMSData(); + _robotContainer.robotInit(); + } + + @Override + public void robotPeriodic() { + _scheduler.run(); + _robotContainer.periodic(); + if (RobotAltModes.isLoopTiming) { + System.out.println(); + } + } + + @Override + public void disabledInit() { + _robotContainer.onDisable(); + ; + } + + @Override + public void disabledPeriodic() { + _robotContainer.disabledPeriodic(); + } + + @Override + public void disabledExit() {} + + @Override + public void autonomousInit() { + _robotContainer.configFMSData(); + _robotContainer.autonomousInit(); + _autonomousCommand = _robotContainer._curAutoSelected.command; + + if (_autonomousCommand != null) { + _autonomousCommand.schedule(); + } + } + + @Override + public void autonomousPeriodic() { + _robotContainer.log(); + } + + @Override + public void autonomousExit() {} + + @Override + public void teleopInit() { + _robotContainer.configFMSData(); + _robotContainer.teleopInit(); + if (_autonomousCommand != null) { + _autonomousCommand.cancel(); + _autonomousCommand = null; + } + } + + @Override + public void teleopPeriodic() { + _robotContainer.log(); + } + + @Override + public void teleopExit() {} + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + public void simulationInit() { + _robotContainer.simulationInit(); + } + + public void simulationPeriodic() {} + + @Override + public void testPeriodic() {} + + @Override + public void testExit() {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..97942f2 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,743 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; 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.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleArrayLogEntry; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.*; +import frc.robot.commands.base.ClawOpenToCommand; +import frc.robot.commands.base.ClawOuttakeCommand; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.commands.base.ElevatorZeroCommand; +import frc.robot.commands.base.GoToPoseCommand; +import frc.robot.commands.score.TeleopScoreCommands; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.Constants; +import frc.robot.constants.LedConfs; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.AutoCommand; +import frc.robot.constants.enums.AutonomousAction; +import frc.robot.constants.enums.AutonomousRoutines; +import frc.robot.constants.enums.AutonomousRoutines.Builders; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.ClawScoreLevels; +import frc.robot.constants.enums.DriveModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.LedColors; +import frc.robot.constants.enums.LedModes; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.subsystems.*; +import frc.robot.utils.LoopTimer; +import java.util.Map; + +public class RobotContainer { + /* --- Shared Resources --- */ + private Field2d _field = new Field2d(); + + /* --- Subsystems --- */ + public Controlboard _controlboard = new Controlboard(_field); + public Drivetrain _drivetrain = new Drivetrain(_field); + public Claw _claw = new Claw(); + public Elevator _elevator = new Elevator(); + public LedController _ledController = new LedController(); + + /* --- Commands --- */ + // Synchronization commands + private CommandBase _resetAllCommand = new ResetAllCommand(_claw, _drivetrain, _elevator); + + // Driver controller feedback + private InstantCommand _xboxRumbleCommand = _controlboard.driverRumbleCommand(); + private InstantCommand _xboxResetRumbleCommand = _controlboard.driverResetRumbleCommand(); + + // Operator Commands + private CommandBase _swapGamePieceCommand = _controlboard.swapDesiredGamePieceCommand(); + private CommandBase _swapIntakeLocationCommand = _controlboard.swapIntakeLocationCommand(); + + // Manual operator overrides commands + private InstantCommand _setHeldGamePieceCommand = _controlboard.setHeldGamePieceCommand(); + private InstantCommand _unsetHeldGamePieceCommand = _controlboard.unsetHeldGamePieceCommand(); + private CommandBase _opOverride = + new InstantCommand(() -> { + _ledController.setIntakeMode( + _controlboard.getDesiredIntakeLocation(), GamePieceType.UNKNOWN_GAME_PIECE + ); + _controlboard.unsetHeldGamePiece(); + }).ignoringDisable(true); + + // Drivetrain + private TeleopSwerveCommand _teleopSwerveCommand = + new TeleopSwerveCommand(_drivetrain, _elevator, _controlboard, DriveModes.FIELD_RELATIVE); + private InstantCommand _zeroGyroCommand = _drivetrain.zeroGyroCommand(); + private InstantCommand _zeroPoseCommand = _drivetrain.zeroPoseCommand(); + private CommandBase _forceChargingWheelDirection = + _drivetrain.forceChargingWheelDirectionCommand(); + + // LED + private LedChargingCommand _ledChargingCommand = + new LedChargingCommand(_ledController, _drivetrain, _controlboard); + private TeleopLedCommand _teleopLEDCommand = new TeleopLedCommand(_ledController, _controlboard); + + /* Base/complex commands ONLY for testing, not bound to any buttons in a real match */ + private InstantCommand _incrementLedCommand = _ledController.incrementAnimationCommand(); + + // Scoring + private CommandBase _manualScoreCommand = TeleopScoreCommands.manual( + ScoringLevels.UNKNOWN_SCORING_LEVEL, + _claw, + _drivetrain, + _elevator, + _ledController, + _controlboard + ); + + // Go to pose testing + private GoToPoseCommand _goToPoseCommandPos1 = + new GoToPoseCommand(_drivetrain, new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(180.0))); + private GoToPoseCommand _goToPoseCommandPos2 = + new GoToPoseCommand(_drivetrain, new Pose2d(-2.0, -2.0, Rotation2d.fromDegrees(0.0))); + private CommandBase _dynamicGoToScoringLocation = new DynamicGoToScoringLocationCommand( + _drivetrain, + _controlboard, + false, + Constants.POST_SCORING_RED_OFFSET, + ScoringLocations.UNKNOWN_SCORING_LOCATION + ); + private CommandBase _platformGoToScoringLocation = new DynamicGoToIntakeLocationCommand( + _drivetrain, _controlboard, new Translation2d(Constants.PLATFORM_PREP_DIST_M, 0.0) + ); + private InstantCommand _forceSetInitialPose = _drivetrain.forceSetInitalPoseCommand(); + + // Intake + private CommandBase _groundIntake = + GroundIntakeCommands.teleop(_claw, _elevator, _ledController, _controlboard); + private CommandBase _manualPlatformIntake = + PlatformIntakeCommand.manual(_claw, _drivetrain, _elevator, _ledController, _controlboard); + private CommandBase _manualSingleStationIntake = SingleStationIntakeCommand.manual( + _claw, _drivetrain, _elevator, _ledController, _controlboard + ); + + // Elevator + private ElevatorZeroCommand _zeroElevator = new ElevatorZeroCommand(_elevator); + private ElevatorRaiseToCommand _elevatorDynamic = new ElevatorRaiseToCommand( + _elevator, _controlboard, ElevatorPositions.ELEVATOR_UNKNOWN_POSITION + ); + + private ElevatorRaiseToCommand _elevatorMin = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_MIN_TICKS); + private ElevatorRaiseToCommand _elevatorMax = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_MAX_TICKS); + + // Elevator Cone + private ElevatorRaiseToCommand _elevator0Cone = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_LEVEL_0_CONE); + private ElevatorRaiseToCommand _elevator1Cone = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_LEVEL_1_CONE); + private ElevatorRaiseToCommand _elevator2Cone = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_LEVEL_2_CONE); + private ElevatorRaiseToCommand _elevatorConePlatformIntake = new ElevatorRaiseToCommand( + _elevator, _controlboard, ElevatorPositions.ELEVATOR_PLATFORM_CONE_INTAKE + ); + + // Elevator Cube + private ElevatorRaiseToCommand _elevator0Cube = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_LEVEL_0_CUBE); + private ElevatorRaiseToCommand _elevator1Cube = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_LEVEL_1_CUBE); + private ElevatorRaiseToCommand _elevator2Cube = + new ElevatorRaiseToCommand(_elevator, _controlboard, ElevatorPositions.ELEVATOR_LEVEL_2_CUBE); + private ElevatorRaiseToCommand _elevatorCubePlatformIntake = new ElevatorRaiseToCommand( + _elevator, _controlboard, ElevatorPositions.ELEVATOR_PLATFORM_CUBE_INTAKE + ); + + // Claw + + // Outtake + private CommandBase _shootCone1 = + new ClawOuttakeCommand(_claw, _controlboard, false, ClawScoreLevels.CONE_LEVEL_1); + private CommandBase _shootCone2 = + new ClawOuttakeCommand(_claw, _controlboard, false, ClawScoreLevels.CONE_LEVEL_2); + private CommandBase _shootCube0 = + new ClawOuttakeCommand(_claw, _controlboard, false, ClawScoreLevels.CUBE_LEVEL_0); + private CommandBase _shootCube1 = + new ClawOuttakeCommand(_claw, _controlboard, false, ClawScoreLevels.CUBE_LEVEL_1); + private CommandBase _shootCube2 = + new ClawOuttakeCommand(_claw, _controlboard, false, ClawScoreLevels.CUBE_LEVEL_2); + + // Claw intake command (complex) + private CommandBase _clawIntake = + ClawIntakeCommand.ClawIntake(_claw, _controlboard, _ledController); + + /* Shuffleboard */ + // Auto and Path Planner + private Alliance _alliance = Alliance.Invalid; + private SendableChooser _autoChooser = new SendableChooser(); + public AutonomousRoutines _curAutoSelected = AutonomousRoutines.DEFAULT_AUTO; + + private Map _eventMap; + private AutonomousRoutines[] _autonModes; + + // Verbose mode + // private double[] _yawPitchRoll = new double[3]; + private double[] _curPoses = new double[4]; + private double[] _visionPoses = new double[4]; + + private DataLog _log = DataLogManager.getLog(); + private DoubleArrayLogEntry _poseLog = new DoubleArrayLogEntry(_log, "/Swerve/RobotPose"); + private DoubleArrayLogEntry _visionPoseLog = new DoubleArrayLogEntry(_log, "/Swerve/VisionPose"); + private DoubleArrayLogEntry _gyroLog = new DoubleArrayLogEntry(_log, "/Swerve/Gyro"); + private DoubleArrayLogEntry _swerveSetpoints = new DoubleArrayLogEntry(_log, "/Swerve/Setpoints"); + private DoubleArrayLogEntry _swerveOutputs = new DoubleArrayLogEntry(_log, "/Swerve/RealOutputs"); + + private DoubleLogEntry _posXLog = new DoubleLogEntry(_log, "/Drive/X"); + private DoubleLogEntry _posYLog = new DoubleLogEntry(_log, "/Drive/Y"); + private DoubleLogEntry _posThetaLog = new DoubleLogEntry(_log, "/Drive/Theta"); + + public RobotContainer() { + var setupTab = Shuffleboard.getTab("Setup"); + + // Auto Chooser + setupTab.add("Auto Chooser", _autoChooser).withSize(3, 2); + _autoChooser.setDefaultOption("NO AUTONOMOUS", AutonomousRoutines.DEFAULT_AUTO.ordinal()); + + // Default commands + _drivetrain.setDefaultCommand(_teleopSwerveCommand); + _ledController.setDefaultCommand(_teleopLEDCommand); + + registerAutonomousCommands(); + registerAutonomousRoutines(); + + configureBindings(); + configShuffleboard(); + + LoopTimer.markCompletion("\n Robot Initialized: "); + } + + private void registerAutonomousUtilityCommands() { + /* --- Print marker commands --- */ + // Use these to log data + AutonomousAction.registerAutonomousAction("printStart", "Start Auto Sequence"); + + /* --- Wait Commands --- */ + // Use these for parallel deadline stop points to stop the drivetrain from moving + AutonomousAction.registerAutonomousAction("waitScore", 1.75); + AutonomousAction.registerAutonomousAction("waitScoreCone1", 0.2); + AutonomousAction.registerAutonomousAction("waitScoreCube1", 0.2); + AutonomousAction.registerAutonomousAction("waitScoreCube2", 0.2); + AutonomousAction.registerAutonomousAction("waitIntake", 0.05); + AutonomousAction.registerAutonomousAction("waitWireScore", 1.0); + AutonomousAction.registerAutonomousAction("waitCharge", 1.0); + + /* --- LED Commands --- */ + AutonomousAction.registerAutonomousAction("redOrangeLed", _ledController, LedColors.RED_ORANGE); + AutonomousAction.registerAutonomousAction("yellowLed", _ledController, LedColors.YELLOW); + AutonomousAction.registerAutonomousAction("pinkLed", _ledController, LedColors.PINK); + AutonomousAction.registerAutonomousAction("greenLed", _ledController, LedColors.GREEN); + AutonomousAction.registerAutonomousAction("blueLed", _ledController, LedColors.BLUE); + AutonomousAction.registerAutonomousAction("purpleLed", _ledController, LedColors.PURPLE); + } + + private void registerAutonomousScoreCommands() { + /* --- Score Commands --- */ + // prep + AutonomousAction.registerAutonomousAction( + "prepScoreCone", + AutonomousScoreCommands.prep( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + GamePieceType.CONE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepScoreCone2", + AutonomousScoreCommands.prep( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + GamePieceType.CONE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepScoreCone0", + AutonomousScoreCommands.prep( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_0_CONE, + GamePieceType.CONE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepScoreCube2", + AutonomousScoreCommands.prep( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE, + GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepScoreCube1", + AutonomousScoreCommands.prep( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_1_CUBE, + GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepScoreCube0", + AutonomousScoreCommands.prep( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_0_CUBE, + GamePieceType.CUBE_GAME_PIECE + ) + ); + + // action + AutonomousAction.registerAutonomousAction( + "scoreCube0", + AutonomousScoreCommands.score( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_0_CUBE, + ClawScoreLevels.CUBE_LEVEL_0, + GamePieceType.CUBE_GAME_PIECE, + 5 + ) + ); + AutonomousAction.registerAutonomousAction( + "scoreCube1", + AutonomousScoreCommands.score( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_1_CUBE, + ClawScoreLevels.CUBE_LEVEL_1, + GamePieceType.CUBE_GAME_PIECE, + 5 + ) + ); + AutonomousAction.registerAutonomousAction( + "scoreCube2", + AutonomousScoreCommands.score( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE, + ClawScoreLevels.CUBE_LEVEL_2, + GamePieceType.CUBE_GAME_PIECE, + 5 + ) + ); + AutonomousAction.registerAutonomousAction( + "scoreCone2", + AutonomousScoreCommands.score( + _elevator, + _controlboard, + _claw, + _ledController, + _drivetrain, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + ClawScoreLevels.CONE_LEVEL_2, + GamePieceType.CONE_GAME_PIECE, + 5 + ) + ); + + // post + AutonomousAction.registerAutonomousAction( + "postScore", AutonomousScoreCommands.post(_elevator, _controlboard, _claw, _ledController) + ); + + AutonomousAction.registerAutonomousAction( + "forceConeShot", + ClawRollersCommands.on(_claw, ClawDirections.CLAW_ROLLERS_CONE_OUT, 1.0) + .andThen(Commands.waitSeconds(ScoringLevels.Consts.AUTO_CONE_SHOT_TIMEOUT_S)) + ); + AutonomousAction.registerAutonomousAction( + "forceCubeShot", + ClawRollersCommands.on(_claw, ClawDirections.CLAW_ROLLERS_CUBE_OUT, 1.0) + .andThen(Commands.waitSeconds(ScoringLevels.Consts.AUTO_CUBE_SHOT_TIMEOUT_S)) + ); + // TODO: missing + // AutonomousAction.registerAutonomousAction( + // "wireScore", WireCoverScoreCommands.score(_claw, _controlboard, _elevator, -1.0, 10000) + // ); + // AutonomousAction.registerAutonomousAction( + // "wirePrepScore", + // WireCoverPrepScoreCommands.prep( + // _claw, _controlboard, _elevator, GamePieceType.CONE_GAME_PIECE + // ) + // ); + // AutonomousAction.registerAutonomousAction( + // "wirePostScore", WireCoverPostScoreCommands.post(_claw, _controlboard, _elevator) + // ); + } + + private void registerAutonomousIntakeCommands() { + /* --- Intake Commands --- */ + // prep + AutonomousAction.registerAutonomousAction( + "prepGroundIntake", + GroundIntakeCommands.autonPrep( + _claw, _elevator, _controlboard, _ledController, GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepGroundIntakeCube", + GroundIntakeCommands.autonPrep( + _claw, _elevator, _controlboard, _ledController, GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "prepGroundIntakeCone", + GroundIntakeCommands.autonPrep( + _claw, _elevator, _controlboard, _ledController, GamePieceType.CONE_GAME_PIECE + ) + ); + + // action + AutonomousAction.registerAutonomousAction( + "groundIntake", + GroundIntakeCommands.auton( + _claw, _elevator, _ledController, _controlboard, 3, GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "groundIntakeCube", + GroundIntakeCommands.auton( + _claw, _elevator, _ledController, _controlboard, 3, GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "groundIntakeCone", + GroundIntakeCommands.auton( + _claw, _elevator, _ledController, _controlboard, 3, GamePieceType.CONE_GAME_PIECE + ) + ); + + // post + AutonomousAction.registerAutonomousAction( + "postGroundIntake", + GroundIntakeCommands.autonPost( + _claw, _elevator, _controlboard, _ledController, GamePieceType.CUBE_GAME_PIECE + ) + ); + AutonomousAction.registerAutonomousAction( + "postGroundIntakeCone", + GroundIntakeCommands.autonPost( + _claw, _elevator, _controlboard, _ledController, GamePieceType.CONE_GAME_PIECE + ) + ); + } + + private void registerAutonomousCommands() { + registerAutonomousUtilityCommands(); + registerAutonomousScoreCommands(); + registerAutonomousIntakeCommands(); + + _eventMap = AutonomousAction.getEventMap(); + } + + private void registerAutonomousRoutines() { + if (_autonModes == null) { + for (Builders builder : Builders.values()) { + builder.initialize(_drivetrain, _eventMap); + } + + _autonModes = AutonomousRoutines.values(); + for (AutonomousRoutines routine : _autonModes) { + if (routine.showInDashboard) { + routine.build(_drivetrain, _controlboard, _elevator, _claw, _ledController); + _autoChooser.addOption(routine.name, routine.ordinal()); + } + } + } + } + + public AutonomousRoutines getAutonomousRoutineSelection() { + var result = _autoChooser.getSelected(); + return _autonModes[result == null ? 0 : result.intValue()]; + } + + public void configFMSData() { + _alliance = DriverStation.getAlliance(); + _drivetrain.updateAlliance(_alliance); + _controlboard.updateAlliance(_alliance); + } + + public void configShuffleboard() { + /* Commands for pit testing */ + ShuffleboardTab tuningTab = Shuffleboard.getTab("Tuning tab"); + + // Claw + // tuningTab.add("Claw open", _clawOpenCommand).withPosition(3, 1); + // tuningTab.add("Claw close", _clawCloseCommand).withPosition(4, 1); + + // Shoot + tuningTab.add("Shoot cone lv2", _shootCone2).withPosition(3, 2); + tuningTab.add("Shoot cube lv2", _shootCube2).withPosition(4, 2); + + // // Shoot sequence + // tuningTab.add("Manual platform intake", _manualPlatformIntakeCommand).withPosition(3, 4); + // tuningTab.add("Claw intake", _clawIntake).withPosition(4, 4); + // tuningTab.add("Manualprep to score", _manualPrepToScore).withPosition(5, 4); + // tuningTab.add("Manual shoot", _manualShoot).withPosition(6, 4); + + // Elevator + tuningTab.add("Elevator Cone lv0", _elevator0Cone).withPosition(7, 0); + tuningTab.add("Elevator Cone lv1", _elevator1Cone).withPosition(8, 0); + tuningTab.add("Elevator Cone lv2", _elevator2Cone).withPosition(9, 0); + tuningTab.add("Elevator Cube lv0", _elevator0Cube).withPosition(7, 1); + tuningTab.add("Elevator Cube lv1", _elevator1Cube).withPosition(8, 1); + tuningTab.add("Elevator Cube lv2", _elevator2Cube).withPosition(9, 1); + tuningTab.add("Elevator ZERO", _zeroElevator).withPosition(9, 4); + + if (RobotAltModes.isSim) { + SmartDashboard.putData("Set Held", _setHeldGamePieceCommand); + SmartDashboard.putData("Unset Held", _unsetHeldGamePieceCommand); + } + } + + public void robotInit() { + DataLogManager.start(); + DataLogManager.logNetworkTables(false); + _ledController.setDisabled(true); + if (RobotAltModes.isVerboseMode) { + DriverStation.startDataLog(_log); + } + } + public void autonomousInit() { + _drivetrain.enableAuto(); + _drivetrain.resetModulesToAbsolute(); + _drivetrain.autonomousDriveMode(true); + _elevator.onEnable(); + } + + public void teleopInit() { + _elevator.onEnable(); + _drivetrain.disableAuto(); + _drivetrain.resetModulesToAbsolute(); + _drivetrain.autonomousDriveMode(false); + _teleopSwerveCommand.teleopInit(); + _resetAllCommand.schedule(); + _ledController.setDisabled(false); + _ledController.changeAnimation(LedModes.RAINBOW); + } + + public void simulationInit() { + _drivetrain.simulationInit(); + } + + public void log() { + if (RobotAltModes.isVerboseMode) { + Pose2d curPose = _drivetrain.getPose(); + _curPoses[0] = curPose.getX(); + _curPoses[1] = curPose.getY(); + _curPoses[2] = curPose.getRotation().getDegrees(); + _poseLog.append(_curPoses); + + Pose2d visionPose = _drivetrain.getVisionPose(); + _visionPoses[0] = visionPose.getX(); + _visionPoses[1] = visionPose.getY(); + _visionPoses[2] = visionPose.getRotation().getDegrees(); + _visionPoseLog.append(_visionPoses); + + // might be problematic, look into + // _drivetrain.getYawPitchRoll(_yawPitchRoll); + // _gyroLog.append(_yawPitchRoll); + + // Module Config: + // Swerve chassis FL, BL, BR, FR + // Practice bot FL, FR, BL, BR + _swerveOutputs.append(_drivetrain.swerveMeasuredIO()); + _swerveSetpoints.append(_drivetrain.swerveSetpointsIO()); + + _posXLog.append(curPose.getX()); + _posYLog.append(curPose.getY()); + _posThetaLog.append(curPose.getRotation().getDegrees()); + } + } + + public void periodic() { + _controlboard.updateOperatorSelections(); + _controlboard.updateShuffleboard(); + } + + public void disabledPeriodic() { + AutonomousRoutines prev = _curAutoSelected; + _curAutoSelected = getAutonomousRoutineSelection(); + + Alliance prevAlliance = _alliance; + _alliance = DriverStation.getAlliance(); + + if (_curAutoSelected == AutonomousRoutines.DEFAULT_AUTO) { + // No auto selected, turn entire strip red + _ledController.setSolidColor(LedColors.RED_ORANGE, LedSections.ALL); + } else { + // For initial pose aligning + if (prev != _curAutoSelected || prevAlliance != _alliance) { + _drivetrain.updateAlliance(_alliance); + _controlboard.updateAlliance(_alliance); + + _ledController.setSolidColor( + 0, + 0, + 0, + // _curAutoSelected.color.r, _curAutoSelected.color.g, _curAutoSelected.color.b, + LedConfs.LED_WHITE_LEVEL, + LedSections.CANDLE + ); + } + + _ledController.disabledPeriodic( + _drivetrain.getVisionPose(), + _curAutoSelected.getInitialPose(_alliance), + _alliance == Alliance.Blue + ); + } + } + + public void onEnable() {} + + public void onDisable() { + resetAll(); + _elevator.onDisable(); + _controlboard.driverResetRumble(); + _ledController.turnOffLeds(LedSections.ALL); + } + + public void resetAll() { + // Upon disabling, open all control loops + _resetAllCommand.schedule(); + } + + public void setLedColor(LedColors ledColor, LedSections ledSection) { + // does this schedule? + _ledController.setSolidColorCommand(ledColor, ledSection).schedule(); + } + + public void setLedColor(LedColors ledColor) { + // does this schedule? + _ledController.setSolidColorCommand(ledColor).schedule(); + } + + public void runAutonomousCommand(AutoCommand command) {} + + private void configureBindings() { + // DRIVER + _controlboard._xboxDrive.x().onTrue(_zeroGyroCommand); + _controlboard._xboxDrive.start().onTrue(_zeroPoseCommand); + + // intake + _controlboard._xboxDrive.rightTrigger().whileTrue(_groundIntake); + _controlboard._xboxDrive.y().toggleOnTrue(_manualPlatformIntake); + _controlboard._xboxDrive.rightBumper().whileTrue(_manualSingleStationIntake); + + // score + _controlboard._xboxDrive.leftTrigger().onTrue(_manualScoreCommand); + + // OPERATOR + _controlboard._ps4Op.L1().onTrue(_swapGamePieceCommand); + _controlboard._ps4Op.R1().onTrue(_swapIntakeLocationCommand); + _controlboard._ps4Op.R2().onTrue(_opOverride); + + // ------------------------------------------------------------------------ + + /* + BUTTON KEY + expected button = actual ps4 button + cross = circle + square = cross + circle = square + triangle = triangle + L1 = L1 + R1 = R1 + L2 = share + R2 = options + share = can’t use bc it only uses boolean events + options = right joystick + no L3 + no R3 + no PS + no touchpad (edited) + */ + + // PIT + // _controlboard._xboxDrive.leftStick().onTrue(); + // _controlboard._xboxDrive.rightStick().onTrue(); + // _controlboard._xboxDrive.leftTrigger().onTrue(); + // _controlboard._xboxDrive.rightTrigger().onTrue(); + // _controlboard._xboxDrive.a().onTrue(); + // _controlboard._xboxDrive.b().onTrue(); + // _controlboard._xboxDrive.x().onTrue(); + // _controlboard._xboxDrive.y().onTrue(); + // _controlboard._xboxDrive.povUp().onTrue(); + // _controlboard._xboxDrive.povDown().onTrue(); + // _controlboard._xboxDrive.povLeft().onTrue(); + // _controlboard._xboxDrive.povRight().onTrue(); + // _controlboard._xboxDrive.back().onTrue(); + // _controlboard._xboxDrive.start().onTrue(); + + // ------------------------------------------------------------------------ + + // // elevator bring up 09/13/2023 #439 + // _controlboard._xboxDrive.a().onTrue(newElevatorRaiseToCommand( + // _elevator, _controlboard, ElevatorPositions.ELEVATOR_UNKNOWN_POSITION + // )); // 0 + // _controlboard._xboxDrive.b().onTrue( + // new ElevatorRaiseToCommand(_elevator, _controlboard, + // ElevatorPositions.ELEVATOR_STOW_TICKS) + // ); // 1000 + // _controlboard._xboxDrive.leftBumper().onTrue(_elevator1Cone); // 46400 + // _controlboard._xboxDrive.rightBumper().onTrue(_elevator2Cone); // 66550 + } +} diff --git a/src/main/java/frc/robot/commands/AutonomousScoreCommands.java b/src/main/java/frc/robot/commands/AutonomousScoreCommands.java new file mode 100644 index 0000000..42c6aef --- /dev/null +++ b/src/main/java/frc/robot/commands/AutonomousScoreCommands.java @@ -0,0 +1,122 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.base.ClawOuttakeCommand; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.ClawScoreLevels; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.LedColors; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.LedController; + +public class AutonomousScoreCommands { + public static CommandBase score( + Elevator elevator, + Controlboard controlboard, + Claw claw, + LedController ledController, + Drivetrain drivetrain, + ElevatorPositions position, + ClawScoreLevels clawScoreLevel, + GamePieceType piece, + double timeout_SEC + ) { + if (RobotAltModes.isAutoTuning) { + return Commands.none(); + } else { + return Commands + .sequence( + ledController.setSolidColorCommand(LedColors.BLUE), + PrepToScoreCommands.autonomous(claw, controlboard, elevator, position, piece), + Commands.either( + claw.setClawRollersPowerCommand(1.0).andThen( + Commands.waitSeconds(ScoringLevels.Consts.AUTO_CUBE_SHOT_TIMEOUT_S) + ), + new ClawOuttakeCommand(claw, controlboard, true, clawScoreLevel) + .withTimeout(ScoringLevels.Consts.AUTO_CONE_SHOT_TIMEOUT_S) + .andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S)), + () -> piece.isCube() + ), + Commands.parallel( + claw.clawRollersOffCommand(), + piece.isCube() ? Commands.none() + : Commands.waitSeconds(0.15).andThen( + StowCommands.auto(claw, controlboard, elevator) + ) + ) + + ) + .withTimeout(timeout_SEC) + .finallyDo((boolean interrupted) -> { + controlboard.unsetHeldGamePiece(); + claw.setClawRollersPower(0.0); + }); + } + } + + public static CommandBase prep( + Elevator elevator, + Controlboard controlboard, + Claw claw, + LedController ledController, + Drivetrain drivetrain, + ElevatorPositions position, + GamePieceType piece + ) { + // TODO update this to optimize auto. + if (RobotAltModes.isAutoTuning) { + return Commands.none(); + } + return Commands + .sequence( + ClawRollersCommands.autonIn( + claw, piece, ClawConfs.CLAW_CUBE_AUTO_STALL_POWER, ClawConfs.CLAW_CONE_STALL_POWER + ), + Commands.parallel( + // todo do according to held game piece + ledController.setSolidColorCommand(LedColors.GREEN), + PrepToScoreCommands.autonomous(claw, controlboard, elevator, position, piece) + ) + ) + .beforeStarting(() -> drivetrain.setAutoPrepScore(true)); + } + + public static CommandBase post( + Elevator elevator, Controlboard controlboard, Claw claw, LedController ledController + ) { + if (RobotAltModes.isAutoTuning) { + return Commands.none(); + } else { + // TODO update this to optimize auto + // All subsystems passed as parameters, just so anything that is running is canceled. + return Commands.sequence( + Commands.waitSeconds(0.15), + StowCommands.auto(claw, controlboard, elevator), + ledController.setSolidColorCommand(LedColors.PURPLE) + ); + } + } + + public static CommandBase yeet( + Elevator elevator, Controlboard controlboard, Claw claw, ClawScoreLevels clawPower + ) { + return Commands.sequence( + new ElevatorRaiseToCommand(elevator, controlboard, ElevatorPositions.ELEVATOR_LEVEL_1_CUBE) + .asProxy(), + claw.setClawRollersPowerCommand(clawPower.power).asProxy(), + Commands.waitSeconds(0.2).asProxy(), + new ElevatorRaiseToCommand(elevator, controlboard, ElevatorPositions.ELEVATOR_STOW_TICKS) + .asProxy() + ); + } +} diff --git a/src/main/java/frc/robot/commands/ClawIntakeCommand.java b/src/main/java/frc/robot/commands/ClawIntakeCommand.java new file mode 100644 index 0000000..2d3ad57 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClawIntakeCommand.java @@ -0,0 +1,41 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.base.ClawOpenToCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.ClawConfs.IntakeSpeed; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.LedController; + +public class ClawIntakeCommand { + public static CommandBase ClawIntake( + Claw claw, Controlboard controlboard, LedController ledController + ) { + // REQUIRES DESIRED GAME PIECE TO BE CORRECT + return Commands + .sequence( + // Fold claw out + new ClawOpenToCommand(claw, ClawPositions.CLAW_OPEN_POS_TICKS), + // Different intake speeds per game piece type + ClawRollersCommands.desiredPieceCommand( + claw, + controlboard, + IntakeSpeed.CUBE_POWER, + ClawDirections.CLAW_ROLLERS_CUBE_IN, + IntakeSpeed.CONE_POWER, + ClawDirections.CLAW_ROLLERS_CONE_IN + ), + // wait for rollers to start + ClawRollersCommands.waitSequenceCommand(claw, controlboard), + LedCommands.dynamicBlinkLedCommand(ledController, controlboard), + ClawRollersCommands.desiredPieceStallCommand(claw, controlboard) + ) + .finallyDo((boolean interrupted) -> { + claw.setClawRollersStall(controlboard.getDesiredGamePiece()); + }); + } +} diff --git a/src/main/java/frc/robot/commands/ClawRollersCommands.java b/src/main/java/frc/robot/commands/ClawRollersCommands.java new file mode 100644 index 0000000..5caebc6 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClawRollersCommands.java @@ -0,0 +1,117 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.subsystems.*; +import java.util.function.BooleanSupplier; + +public class ClawRollersCommands { + public static CommandBase on(Claw claw, ClawDirections clawDirection, double speed) { + if (Constants.isKlaw) { + switch (clawDirection) { + case CLAW_ROLLERS_CONE_IN: + case CLAW_ROLLERS_CUBE_OUT: + return claw.setClawRollersPowerCommand(speed); + case CLAW_ROLLERS_CONE_OUT: + case CLAW_ROLLERS_CUBE_IN: + return claw.setClawRollersPowerCommand(-speed); + default: + return claw.setClawRollersPowerCommand(0.0); + } + } + return Commands.print("No Claw Rollers on this robot"); + } + + public static CommandBase conditionalCommand( + Claw claw, + Controlboard controlboard, + double cubeSpeed, + ClawDirections cubeDirection, + double coneSpeed, + ClawDirections coneDirection, + BooleanSupplier condition + ) { + return Commands.either( + ClawRollersCommands.on(claw, cubeDirection, cubeSpeed), + ClawRollersCommands.on(claw, coneDirection, coneSpeed), + condition + ); + } + + public static CommandBase desiredPieceCommand( + Claw claw, + Controlboard controlboard, + double cubeSpeed, + ClawDirections cubeDirection, + double coneSpeed, + ClawDirections coneDirection + ) { + return ClawRollersCommands.conditionalCommand( + claw, + controlboard, + cubeSpeed, + cubeDirection, + coneSpeed, + coneDirection, + () -> controlboard.getDesiredGamePiece().isCube() + ); + } + + public static CommandBase desiredPieceStallCommand(Claw claw, Controlboard controlboard) { + return desiredPieceCommand( + claw, + controlboard, + ClawConfs.CLAW_CUBE_STALL_POWER, + ClawDirections.CLAW_ROLLERS_CUBE_IN, + ClawConfs.CLAW_CONE_STALL_POWER, + ClawDirections.CLAW_ROLLERS_CONE_IN + ); + } + + public static CommandBase heldPieceCommand( + Claw claw, + Controlboard controlboard, + double cubeSpeed, + ClawDirections cubeDirection, + double coneSpeed, + ClawDirections coneDirection + ) { + return conditionalCommand( + claw, + controlboard, + cubeSpeed, + cubeDirection, + coneSpeed, + coneDirection, + () -> controlboard.getHeldGamePiece().isCube() + ); + } + + public static CommandBase waitSequenceCommand(Claw claw, Controlboard controlboard) { + return Commands.sequence( + Commands.waitSeconds(ClawConfs.CLAW_ROLLER_SPINUP_TIME_S), + Commands.waitUntil( + () -> controlboard.isPieceInIntake(claw.getBeamBreak(), claw.coneDetected()) + ), + Commands.either( + Commands.waitSeconds(ClawConfs.CLAW_CUBE_SETTLE_TIME_S), + Commands.waitSeconds(ClawConfs.CLAW_CONE_SETTLE_TIME_S), + () -> controlboard.getDesiredGamePiece().isCube() + ) + ); + } + + public static CommandBase autonIn( + Claw claw, GamePieceType piece, double cubePower, double conePower + ) { + // If an unknown piece is here, we're calling this in a dynamic command (be worried) + assert (piece != GamePieceType.UNKNOWN_GAME_PIECE); + return piece.isCube() + ? ClawRollersCommands.on(claw, ClawDirections.CLAW_ROLLERS_CUBE_IN, cubePower) + : ClawRollersCommands.on(claw, ClawDirections.CLAW_ROLLERS_CONE_IN, conePower); + } +} diff --git a/src/main/java/frc/robot/commands/ClawScoreCommand.java b/src/main/java/frc/robot/commands/ClawScoreCommand.java new file mode 100644 index 0000000..f2949d7 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClawScoreCommand.java @@ -0,0 +1,25 @@ +package frc.robot.commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.constants.enums.ClawScoreLevels; +import frc.robot.subsystems.Claw; + +public class ClawScoreCommand extends SequentialCommandGroup { + private final Claw _claw; + private final ClawScoreLevels _scoringLevel; + private final double _scoringDuration; + public ClawScoreCommand(Claw claw, double scoringDuration, ClawScoreLevels scoringLevel) { + _claw = claw; + _scoringDuration = scoringDuration; + _scoringLevel = scoringLevel; + + addCommands(_claw.openClawCommand() + .andThen(new WaitUntilCommand(_claw::isExtended)) + .andThen(_claw.setClawPowerCommand(_scoringLevel.power)) + .andThen(new WaitCommand(_scoringDuration)) + .andThen(_claw.clawRollersOffCommand().alongWith( + _claw.closeClawCommand().alongWith(new WaitUntilCommand(_claw::isRetracted)) + ))); + } +} diff --git a/src/main/java/frc/robot/commands/CommandUtils.java b/src/main/java/frc/robot/commands/CommandUtils.java new file mode 100644 index 0000000..cba36fb --- /dev/null +++ b/src/main/java/frc/robot/commands/CommandUtils.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Elevator; + +public class CommandUtils { + public static final class ElevatorUtils { + public static CommandBase pieceSpecificElevatorRaiseToCommand( + Claw claw, + Controlboard controlboard, + Elevator elevator, + ElevatorPositions cubeElevatorPositions, + ElevatorPositions coneElevatorPositions + ) { + return Commands.either( + new ElevatorRaiseToCommand(elevator, controlboard, cubeElevatorPositions), + new ElevatorRaiseToCommand(elevator, controlboard, coneElevatorPositions), + () -> controlboard.getDesiredGamePiece().isCube() + ); + } + } +} diff --git a/src/main/java/frc/robot/commands/DynamicGoToIntakeLocationCommand.java b/src/main/java/frc/robot/commands/DynamicGoToIntakeLocationCommand.java new file mode 100644 index 0000000..aed54be --- /dev/null +++ b/src/main/java/frc/robot/commands/DynamicGoToIntakeLocationCommand.java @@ -0,0 +1,180 @@ +package frc.robot.commands; + +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.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.CameraSets; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.utils.MultiCameraController; + +public class DynamicGoToIntakeLocationCommand extends CommandBase { + private final Drivetrain _drivetrain; + private final Controlboard _controlboard; + private final Transform2d _redOffset; + private final Transform2d _blueOffset; + private final HPIntakeStations _intakeLocation; + private final double _xToleranceM; + private final double _yToleranceM; + private Rotation2d _thetaTolerance; + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + Translation2d redOffset, + HPIntakeStations intakeLocation, + double xToleranceM, + double yToleranceM, + Rotation2d thetaTolerance + ) { + _drivetrain = drivetrain; + _controlboard = controlboard; + _redOffset = new Transform2d(redOffset, Rotation2d.fromDegrees(0.0)); + _intakeLocation = intakeLocation; + _xToleranceM = xToleranceM; + _yToleranceM = yToleranceM; + _thetaTolerance = thetaTolerance; + _blueOffset = _redOffset.inverse(); + + addRequirements(drivetrain); + } + + public DynamicGoToIntakeLocationCommand(Drivetrain drivetrain, Controlboard controlboard) { + this(drivetrain, controlboard, new Translation2d(0, 0)); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, Controlboard controlboard, Translation2d redOffset + ) { + this(drivetrain, controlboard, redOffset, HPIntakeStations.UNKNOWN_INTAKE_STATION); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + Translation2d redOffset, + HPIntakeStations intakeLocation + ) { + this( + drivetrain, + controlboard, + redOffset, + intakeLocation, + Constants.CRobot.drive.control.xy.toleranceM, + Constants.CRobot.drive.control.xy.toleranceM + ); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, Controlboard controlboard, Translation2d redOffset, double xyTolerance + ) { + this(drivetrain, controlboard, redOffset, HPIntakeStations.UNKNOWN_INTAKE_STATION, xyTolerance); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + Translation2d redOffset, + HPIntakeStations intakeLocation, + double xyTolerance + ) { + this(drivetrain, controlboard, redOffset, intakeLocation, xyTolerance, xyTolerance); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + Translation2d redOffset, + double xTolerance, + double yTolerance + ) { + this( + drivetrain, + controlboard, + redOffset, + HPIntakeStations.UNKNOWN_INTAKE_STATION, + xTolerance, + yTolerance + ); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + Translation2d redOffset, + HPIntakeStations intakeLocation, + double xTolerance, + double yTolerance + ) { + this( + drivetrain, + controlboard, + redOffset, + intakeLocation, + xTolerance, + yTolerance, + Constants.CRobot.drive.control.theta.tolerance + ); + } + + public DynamicGoToIntakeLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + Translation2d redOffset, + double xToleranceM, + double yToleranceM, + Rotation2d thetaTolerance + ) { + this( + drivetrain, + controlboard, + redOffset, + HPIntakeStations.UNKNOWN_INTAKE_STATION, + xToleranceM, + yToleranceM, + thetaTolerance + ); + } + + @Override + public void initialize() { + boolean isRedAlliance = _drivetrain.isRedAlliance(); + Pose2d pose = _intakeLocation == HPIntakeStations.UNKNOWN_INTAKE_STATION + ? _controlboard.getDesiredIntakeLocation().pose.get(isRedAlliance) + : _intakeLocation.pose.get(isRedAlliance); + Pose2d offsetPose = pose.plus(isRedAlliance ? _redOffset : _blueOffset); + + if (_intakeLocation == HPIntakeStations.DOUBLE_STATION_INNER) { // inner + _drivetrain.setDesiredCameras(isRedAlliance ? CameraSets.CAMERA_1 : CameraSets.CAMERA_0); + } else { + _drivetrain.setDesiredCameras(isRedAlliance ? CameraSets.CAMERA_0 : CameraSets.CAMERA_1); + } + + _drivetrain.setTolerance(_xToleranceM, _yToleranceM, _thetaTolerance); + _drivetrain.setStaticTarget(offsetPose); + _drivetrain.chaseStaticTargetDrive(); + } + + @Override + public void execute() { + _drivetrain.chaseStaticTargetDrive(); + } + + @Override + public void end(boolean interrupted) { + _drivetrain.resetDefaultTolerance(); + _drivetrain.fieldRelativeDrive(0.0, 0.0, 0.0); + _drivetrain.setDesiredCameras(Constants.CRobot.vision.setup); + } + + @Override + public boolean isFinished() { + return _drivetrain.inRange(); + // TODO: fill in with actual value; + } +} diff --git a/src/main/java/frc/robot/commands/DynamicGoToScoringLocationCommand.java b/src/main/java/frc/robot/commands/DynamicGoToScoringLocationCommand.java new file mode 100644 index 0000000..81fbcba --- /dev/null +++ b/src/main/java/frc/robot/commands/DynamicGoToScoringLocationCommand.java @@ -0,0 +1,127 @@ +package frc.robot.commands; + +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.wpilibj2.command.CommandBase; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.CameraSets; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; + +public class DynamicGoToScoringLocationCommand extends CommandBase { + private final Drivetrain _drivetrain; + private final Controlboard _controlboard; + private final boolean _forceWheel; + private final Transform2d _redOffset; + private final Transform2d _blueOffset; + private final ScoringLocations _scoringLocation; + private final double _xToleranceM; + private final double _yToleranceM; + private final Rotation2d _thetaTolerance; + private Pose2d _pose; + private boolean _thetaAlignedDeg; + private double _moduleAngleDeg; + + public DynamicGoToScoringLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + boolean forceWheel, + Transform2d redOffset, + ScoringLocations scoringLocation, + double xToleranceM, + double yToleranceM, + Rotation2d thetaTolerance + ) { + _drivetrain = drivetrain; + _controlboard = controlboard; + _forceWheel = forceWheel; + _redOffset = redOffset; + _blueOffset = _redOffset.inverse(); + _scoringLocation = scoringLocation; + _xToleranceM = xToleranceM; + _yToleranceM = yToleranceM; + _thetaTolerance = thetaTolerance; + + addRequirements(drivetrain); + } + + public DynamicGoToScoringLocationCommand( + Drivetrain drivetrain, + Controlboard controlboard, + boolean forceWheel, + Transform2d redOffset, + ScoringLocations scoringLocation + ) { + this( + drivetrain, + controlboard, + forceWheel, + redOffset, + scoringLocation, + Constants.CRobot.drive.control.xy.toleranceM, + Constants.CRobot.drive.control.xy.toleranceM, + Constants.CRobot.drive.control.theta.tolerance + ); + } + + @Override + public void initialize() { + _controlboard.driverRumble(); + + boolean isRed = _drivetrain.isRedAlliance(); + ScoringLocations location = _scoringLocation == ScoringLocations.UNKNOWN_SCORING_LOCATION + ? _controlboard.getDesiredScoringLocation() + : _scoringLocation; + _pose = location.pose.get(isRed); + + _drivetrain.setDesiredCameras(location.getScoringCamera(isRed)); + + _drivetrain.setTolerance(_xToleranceM, _yToleranceM, _thetaTolerance); + _drivetrain.setStaticTarget(_pose); + + if (_forceWheel) { + _thetaAlignedDeg = false; + _drivetrain.setSnapAngle(_pose.getRotation()); + _drivetrain.snapToAngleDrive(0.0, 0.0); + } else { + _drivetrain.chaseStaticTargetDrive(); + } + } + + @Override + public void execute() { + if (_forceWheel) { + if (!_thetaAlignedDeg) { + if (_drivetrain.thetaInRange()) { + _thetaAlignedDeg = true; + var curr = _drivetrain.getPose(); + _moduleAngleDeg = Math.atan2(_pose.getX() - curr.getX(), _pose.getY() - curr.getY()); + _drivetrain.forceScoreWheelDirection(_moduleAngleDeg); + } else { + _drivetrain.snapToAngleDrive(0.0, 0.0); + } + } else { + _drivetrain.forceWheelDirectionDrive( + _moduleAngleDeg, Constants.FORCED_WHEEL_ALIGNMENT_SPEED_MPS + ); + } + } else { + _drivetrain.chaseStaticTargetDrive(); + } + } + + @Override + public void end(boolean interrupted) { + _drivetrain.resetDefaultTolerance(); + _drivetrain.fieldRelativeDrive(0.0, 0.0, 0.0); + _drivetrain.setDesiredCameras(CameraSets.BOTH_CAMERAS); + _controlboard.driverResetRumble(); + } + + @Override + public boolean isFinished() { + return _drivetrain.inRange(); + } +} diff --git a/src/main/java/frc/robot/commands/DynamicGoToScoringPoseCommands.java b/src/main/java/frc/robot/commands/DynamicGoToScoringPoseCommands.java new file mode 100644 index 0000000..44b9cb3 --- /dev/null +++ b/src/main/java/frc/robot/commands/DynamicGoToScoringPoseCommands.java @@ -0,0 +1,52 @@ +package frc.robot.commands; + +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.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.LedController; + +public class DynamicGoToScoringPoseCommands { + public static CommandBase manual( + Drivetrain drivetrain, Controlboard controlboard, LedController ledController + ) { + return Commands + .either( + new DynamicGoToScoringLocationCommand( + drivetrain, + controlboard, + false, + new Transform2d(new Translation2d(-0.1, 0.0), Rotation2d.fromDegrees(0)), + ScoringLocations.UNKNOWN_SCORING_LOCATION + ), + Commands.either( + new DynamicGoToScoringLocationCommand( + drivetrain, + controlboard, + false, + new Transform2d(new Translation2d(-0.25, 0.0), Rotation2d.fromDegrees(0)), + ScoringLocations.UNKNOWN_SCORING_LOCATION + ), + new DynamicGoToScoringLocationCommand( + drivetrain, + controlboard, + false, + new Transform2d(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(0)), + ScoringLocations.UNKNOWN_SCORING_LOCATION + ), + () -> { + return controlboard.getDesiredScoringLevel() == ScoringLevels.SCORING_LEVEL_1; + } + ), + () -> { return controlboard.getHeldGamePiece() == GamePieceType.CUBE_GAME_PIECE; } + ) + .withTimeout(Constants.ALIGN_SHOT_TIMEOUT_S); + } +} diff --git a/src/main/java/frc/robot/commands/GroundIntakeCommands.java b/src/main/java/frc/robot/commands/GroundIntakeCommands.java new file mode 100644 index 0000000..de01433 --- /dev/null +++ b/src/main/java/frc/robot/commands/GroundIntakeCommands.java @@ -0,0 +1,129 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.CommandUtils.ElevatorUtils; +import frc.robot.commands.base.ClawOpenToCommand; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.ClawConfs.IntakeSpeed; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.LedColors; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.LedController; + +public class GroundIntakeCommands { + public static CommandBase teleop( + Claw claw, Elevator elevator, LedController ledController, Controlboard controlboard + ) { + return Commands + .sequence( + new ClawOpenToCommand(claw, ClawPositions.CLAW_OPEN_POS_TICKS), + Commands.parallel( + Commands.either( + ledController.setSolidColorCommand(LedColors.PURPLE), + ledController.setSolidColorCommand(LedColors.YELLOW), + () -> controlboard.getDesiredGamePiece() == GamePieceType.CUBE_GAME_PIECE + ), + ElevatorUtils.pieceSpecificElevatorRaiseToCommand( + claw, + controlboard, + elevator, + ElevatorPositions.ELEVATOR_GROUND_CUBE_INTAKE, + ElevatorPositions.ELEVATOR_GROUND_CONE_INTAKE + ) + ), + ClawIntakeCommand.ClawIntake(claw, controlboard, ledController) + ) + .finallyDo((boolean interrupted) -> { + claw.closeClawSolenoid(); + claw.setClawRollersStall(controlboard.getDesiredGamePiece()); + ledController.turnOffLeds(LedSections.ALL); + }); + } + + public static CommandBase auton( + Claw claw, + Elevator elevator, + LedController ledController, + Controlboard controlboard, + double timeout, + GamePieceType piece + ) { + // if autotesting return none + return Commands.parallel( + ledController.setSolidColorCommand(LedColors.PURPLE), + Commands + .sequence( + ClawRollersCommands.autonIn( + claw, piece, IntakeSpeed.CUBE_POWER, IntakeSpeed.CONE_POWER + ), + Commands.waitSeconds(ClawConfs.CLAW_ROLLER_SPINUP_TIME_S), + Commands.waitUntil(() -> { + return controlboard.isPieceInIntake( + claw.getBeamBreak(), + claw.coneDetected(IntakeSpeed.CONE_DETECTION_CURRENT_THRESHOLD_A) + ); + }), + ClawRollersCommands.autonIn( + claw, piece, IntakeSpeed.CUBE_POWER, IntakeSpeed.CONE_POWER + ) + ) + .withTimeout(timeout) + .beforeStarting(() -> { controlboard.setDesiredGamePiece(piece); }) + .finallyDo((boolean interrupted) -> { controlboard.setHeldGamePiece(); }) + ); + } + + public static CommandBase autonPrep( + Claw claw, + Elevator elevator, + Controlboard controlboard, + LedController ledController, + GamePieceType piece + ) { + if (RobotAltModes.isAutoTuning) + return Commands.none(); + + return Commands.parallel( + ledController.setSolidColorCommand(LedColors.RED_ORANGE), + Commands.sequence( + ClawRollersCommands.autonIn( + claw, piece, IntakeSpeed.CUBE_POWER, IntakeSpeed.CONE_POWER + ), + new InstantCommand(() -> claw.openClawSolenoid()) + ), + new ElevatorRaiseToCommand( + elevator, + controlboard, + piece.isCube() ? ElevatorPositions.ELEVATOR_GROUND_CUBE_INTAKE + : ElevatorPositions.ELEVATOR_GROUND_CONE_INTAKE + ) + .beforeStarting(() -> controlboard.setDesiredGamePiece(piece)) + ); + } + + public static CommandBase autonPost( + Claw claw, + Elevator elevator, + Controlboard controlboard, + LedController ledController, + GamePieceType piece + ) { + return Commands.parallel( + ledController.setSolidColorCommand(LedColors.YELLOW), + ClawRollersCommands.autonIn( + claw, piece, ClawConfs.CLAW_CUBE_INTAKE_RESETTLE, ClawConfs.CLAW_CONE_INTAKE_RESETTLE + ), + new InstantCommand(() -> claw.closeClawSolenoid()), + StowCommands.auto(claw, controlboard, elevator) + ); + } +} diff --git a/src/main/java/frc/robot/commands/LedChargingCommand.java b/src/main/java/frc/robot/commands/LedChargingCommand.java new file mode 100644 index 0000000..c2d07a1 --- /dev/null +++ b/src/main/java/frc/robot/commands/LedChargingCommand.java @@ -0,0 +1,76 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.LedConfs; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.enums.LedColors; +import frc.robot.constants.enums.LedModes; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.LedController; + +public class LedChargingCommand extends CommandBase { + private final LedController _ledController; + private final Drivetrain _drivetrain; + private final Controlboard _controlboard; + private boolean _redAlliance; + private boolean _facingSubstationRed; + private boolean _useRoll; + private double _currAngle; + + public LedChargingCommand( + LedController ledController, Drivetrain drivetrain, Controlboard controlboard + ) { + _ledController = ledController; + _drivetrain = drivetrain; + _controlboard = controlboard; + + addRequirements(ledController); + } + + @Override + public void initialize() { + _redAlliance = _drivetrain.isRedAlliance(); + } + + @Override + public void execute() { + calculateRobotPosition(); + + // robot closer to alliance: blue LEDs, closer to center: yellow LEDs + if (_currAngle < -LedConfs.LEVEL_THRESHOLD) { + boolean temp = _facingSubstationRed ? _redAlliance : !_redAlliance; + _ledController.setSolidColor(temp ? LedColors.YELLOW : LedColors.BLUE, LedSections.ALL); + } else if (_currAngle > LedConfs.LEVEL_THRESHOLD) { + boolean temp = _facingSubstationRed ? !_redAlliance : _redAlliance; + _ledController.setSolidColor(temp ? LedColors.YELLOW : LedColors.BLUE, LedSections.ALL); + } else { + _ledController.changeAnimation(LedModes.RAINBOW); + } + } + + @Override + public void end(boolean interrupted) { + _ledController.turnOffLeds(LedSections.ALL); + } + + @Override + public boolean isFinished() { + return false; + } + + public void calculateRobotPosition() { + double yaw = _drivetrain.getYaw().getDegrees() % 360.0; + yaw = yaw + yaw < 0.0 ? 360.0 : 0.0; + _useRoll = (yaw >= LedConfs.ROLL_1_LOWER_DEG && yaw <= LedConfs.ROLL_1_UPPER_DEG) + || (yaw >= LedConfs.ROLL_2_LOWER_DEG && yaw <= LedConfs.ROLL_2_UPPER_DEG); + _facingSubstationRed = + ((yaw >= LedConfs.ORIENTATION_BLUE_LOWER_DEG && yaw <= LedConfs.ORIENTATION_BLUE_UPPER_DEG) + && !_redAlliance) + || ((yaw >= LedConfs.ORIENTATION_RED_LOWER_DEG || yaw <= LedConfs.ORIENTATION_RED_UPPER_DEG) + && _redAlliance); + _currAngle = + _useRoll ? _drivetrain.getRoll().getDegrees() : _drivetrain.getPitch().getDegrees(); + } +} diff --git a/src/main/java/frc/robot/commands/LedCommands.java b/src/main/java/frc/robot/commands/LedCommands.java new file mode 100644 index 0000000..fba9cf5 --- /dev/null +++ b/src/main/java/frc/robot/commands/LedCommands.java @@ -0,0 +1,29 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.enums.LedModes; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.LedController; + +// Raising elevator: blue +// Raising intake: pink +// Intake rollers: purple +// Retract intake: green + +public class LedCommands { + public static CommandBase dynamicBlinkLedCommand( + LedController ledController, Controlboard controlboard + ) { + return Commands.sequence( + new InstantCommand(() -> ledController.turnOffLeds(LedSections.ALL)), + Commands.either( + ledController.changeAnimationCommand(LedModes.BLINK_CUBE), + ledController.changeAnimationCommand(LedModes.BLINK_CONE), + () -> controlboard.getDesiredGamePiece().isCube() + ) + ); + } +} diff --git a/src/main/java/frc/robot/commands/PlatformIntakeCommand.java b/src/main/java/frc/robot/commands/PlatformIntakeCommand.java new file mode 100644 index 0000000..9b6dd27 --- /dev/null +++ b/src/main/java/frc/robot/commands/PlatformIntakeCommand.java @@ -0,0 +1,189 @@ +package frc.robot.commands; + +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.util.Units; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.base.PrepForPlatformIntakeCommand; +import frc.robot.commands.base.SetClawGivenElevatorCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.Constants; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.DriveModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.constants.enums.LedColors; +import frc.robot.subsystems.*; + +public class PlatformIntakeCommand { + public static CommandBase dynamic( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard, + Pose2d platformPose + ) { + return Commands.sequence( + Commands.parallel( + ledController.setSolidColorCommand(LedColors.PURPLE), + new DynamicGoToIntakeLocationCommand( + drivetrain, + controlboard, + new Translation2d(Constants.PLATFORM_PREP_DIST_M, 0.0), + HPIntakeStations.UNKNOWN_INTAKE_STATION, + Units.inchesToMeters(5.0) + ) + ), + // Potentially parallelize with go previous got to intake location + new PrepForPlatformIntakeCommand(claw, controlboard, elevator), + + // Allow the driver to reposition as long as claw beam break not triggered + Commands.race( + // Integrate intake retraction if using partial stow + Commands + .sequence( + Commands.either( + new DynamicGoToIntakeLocationCommand(drivetrain, controlboard), + new DynamicGoToIntakeLocationCommand( + drivetrain, + controlboard, + new Translation2d(Constants.PLATFORM_INTAKE_CONE_OFFSET_M, 0.0) + ), + () -> controlboard.getDesiredGamePiece().isCube() + ), + // Turn rumble off while driver has control + new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.FIELD_RELATIVE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + .beforeStarting(controlboard.driverResetRumbleCommand()) + ) + .finallyDo((boolean interrupted) -> controlboard.driverRumble()), + ClawIntakeCommand.ClawIntake(claw, controlboard, ledController) + ), + Commands.deadline( + Commands.waitSeconds(ClawConfs.CLAW_PLATFORM_CLEARANCE_WAIT_S) + .andThen(Commands.parallel( + new SetClawGivenElevatorCommand( + claw, + controlboard, + elevator, + ClawPositions.CLAW_MIN_TICKS, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE.ticks, + false + ), + StowCommands.tele(claw, controlboard, elevator) + )), + Commands.sequence( + new DynamicGoToIntakeLocationCommand( + drivetrain, + controlboard, + new Translation2d(Constants.PLATFORM_BACKOUT_DIST_M, 0.0), + HPIntakeStations.UNKNOWN_INTAKE_STATION, + Units.inchesToMeters(10.0), + Units.inchesToMeters(10.0), + Rotation2d.fromDegrees(20.0) + ) + .finallyDo((boolean interrupted) -> controlboard.driverResetRumble()) + .withTimeout(2.0), + new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.FIELD_RELATIVE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + ) + ) + // , + // #ifdef NEW_CLAW + // DesiredPieceStallRollers(claw, controlboard)) + // #else + // ClawRollersOnCommand(claw, CLAW_ROLLERS_IN, + // CLAW_CONE_STALL_POWER).ToPtr()) + // #endif + // .BeforeStarting([&]() { + // controlboard.DriverRumble(); + // ledController.SetOverride(true); + // }) + // .FinallyDo([&](bool interrupted) { + // controlboard.DriverResetRumble(); + // ledController.SetOverride(false); + // ledController.TurnOffLeds(ALL); + // }) + // .Unless([&]() { + // auto pose = drivetrain.GetPose(); + // auto inRange = + // (pose.Y() > HALF_FIELD_Y && (drivetrain.GetAlliance() == + // frc::DriverStation::kRed + // ? (pose.X() < LOADING_ZONE_RED_MAX_X) + // : (pose.X() > + // LOADING_ZONE_BLUE_MIN_X))); + // if (!inRange) { ledController.SetSolidColor(RED_ORANGE, ALL); } + // return !inRange; + // }) + ); + } + + public static CommandBase manual( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands + .sequence( + Commands.deadline( + Commands.sequence( + Commands.either( + ledController.setSolidColorCommand(LedColors.PURPLE), + ledController.setSolidColorCommand(LedColors.YELLOW), + () -> controlboard.getDesiredGamePiece().isCube() + ), + new PrepForPlatformIntakeCommand(claw, controlboard, elevator), + ClawIntakeCommand.ClawIntake(claw, controlboard, ledController) + ), + new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.SNAP_TO_ANGLE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + .beforeStarting(() -> { + drivetrain.setSnapAngle( + drivetrain.isRedAlliance() + ? HPIntakeStations.LocationMath.RED_PLATFORM_ANGLE + : HPIntakeStations.LocationMath.BLUE_PLATFORM_ANGLE + ); + }) + ), + drivetrain + .forceAllianceBasedFieldRelativeMovementCommand( + -2.0, 0.0, Units.millisecondsToSeconds(200) + ) + .beforeStarting(claw.closeClawCommand()), + Commands.deadline( + StowCommands.tele(claw, controlboard, elevator), + new TeleopSwerveCommand( + drivetrain, elevator, controlboard, DriveModes.FIELD_RELATIVE + ) + ) + ) + .finallyDo((boolean interrupted) -> { ledController.turnOffLeds(LedSections.ALL); }); + } +} diff --git a/src/main/java/frc/robot/commands/PrepToScoreCommands.java b/src/main/java/frc/robot/commands/PrepToScoreCommands.java new file mode 100644 index 0000000..21a5fa4 --- /dev/null +++ b/src/main/java/frc/robot/commands/PrepToScoreCommands.java @@ -0,0 +1,90 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.subsystems.*; + +public class PrepToScoreCommands { + public static CommandBase teleop( + Claw claw, Controlboard controlboard, Elevator elevator, LedController ledController + ) { + return teleop( + claw, controlboard, elevator, ledController, ElevatorPositions.ELEVATOR_UNKNOWN_POSITION + ); + } + + public static CommandBase teleop( + Claw claw, + Controlboard controlboard, + Elevator elevator, + LedController ledController, + ElevatorPositions position + ) { + return teleop( + claw, controlboard, elevator, ledController, position, GamePieceType.UNKNOWN_GAME_PIECE + ); + } + + // In paralell reseat game piece, raise elevator --> deploy claw, then wait claw extension time + public static CommandBase teleop( + Claw claw, + Controlboard controlboard, + Elevator elevator, + LedController ledController, + ElevatorPositions position, + GamePieceType piece + ) { + return Commands.parallel( + // Deploy claw once elevator is at full height + new ElevatorRaiseToCommand(elevator, controlboard, position) + .finallyDo((boolean interrupted) -> { + if (controlboard.getDesiredScoringLevel() != ScoringLevels.SCORING_LEVEL_0) + claw.openClawSolenoid(); + }), + ClawRollersCommands.conditionalCommand( + claw, + controlboard, + ClawConfs.CLAW_CUBE_INTAKE_RESETTLE, + ClawDirections.CLAW_ROLLERS_CUBE_IN, + ClawConfs.CLAW_CONE_INTAKE_RESETTLE, + ClawDirections.CLAW_ROLLERS_CONE_IN, + (() -> { + return piece != GamePieceType.UNKNOWN_GAME_PIECE + ? piece.isCube() + : controlboard.getHeldGamePiece().isCube(); + }) + ) + ); + } + + public static CommandBase autonomous( + Claw claw, + Controlboard controlboard, + Elevator elevator, + ElevatorPositions position, + GamePieceType piece + ) { + return Commands.sequence( + Commands.deadline( + new ElevatorRaiseToCommand(elevator, controlboard, position), + ClawRollersCommands.autonIn( + claw, + piece, + ClawConfs.CLAW_CUBE_INTAKE_RESETTLE, + ClawConfs.CLAW_CONE_INTAKE_RESETTLE + ) + ), + // because it's autonomous we can do a simple ternary + piece.isCube() + ? Commands.none() + : claw.openClawCommand().andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_OUT_TIME_S)) + ); + } +} diff --git a/src/main/java/frc/robot/commands/ResetAllCommand.java b/src/main/java/frc/robot/commands/ResetAllCommand.java new file mode 100644 index 0000000..11d303b --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetAllCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.enums.DriveModes; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; + +public class ResetAllCommand extends CommandBase { + private final Claw _claw; + private final Drivetrain _drivetrain; + private final Elevator _elevator; + + public ResetAllCommand(Claw claw, Drivetrain drivetrain, Elevator elevator) { + _drivetrain = drivetrain; + _claw = claw; + _elevator = elevator; + + addRequirements(drivetrain, claw, elevator); + } + + @Override + public void initialize() { + _drivetrain.drive(0.0, 0.0, 0.0, DriveModes.FIELD_RELATIVE); + + _claw.setClawRollersPower(0.0); + // _claw.setClawPower(0.0) for motorClaw + _claw.closeClawSolenoid(); + + _elevator.setPower(0.0); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SingleStationIntakeCommand.java b/src/main/java/frc/robot/commands/SingleStationIntakeCommand.java new file mode 100644 index 0000000..8ff041f --- /dev/null +++ b/src/main/java/frc/robot/commands/SingleStationIntakeCommand.java @@ -0,0 +1,109 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.base.ClawOpenToCommand; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.ClawConfs.IntakeSpeed; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.DriveModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.subsystems.*; + +public class SingleStationIntakeCommand { + public static CommandBase dynamic( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands + .sequence( + Commands.deadline( + Commands.sequence( + new ClawOpenToCommand(claw, ClawPositions.CLAW_CLOSED_POS_TICKS), + ClawRollersCommands.desiredPieceCommand( + claw, + controlboard, + IntakeSpeed.CUBE_POWER, + ClawDirections.CLAW_ROLLERS_CUBE_IN, + IntakeSpeed.CONE_POWER, + ClawDirections.CLAW_ROLLERS_CONE_IN + ), + ClawRollersCommands.waitSequenceCommand(claw, controlboard) + ), + new DynamicGoToIntakeLocationCommand( + drivetrain, + controlboard, + new Translation2d(0.0, 0.0), + HPIntakeStations.SINGLE_STATION, + Units.inchesToMeters(2), + Units.inchesToMeters(9), + Rotation2d.fromDegrees(1.0) + ), + new ElevatorRaiseToCommand( + elevator, controlboard, ElevatorPositions.ELEVATOR_DROP_CONE_INTAKE + ) + ), + ClawRollersCommands.desiredPieceStallCommand(claw, controlboard), + StowCommands.tele(claw, controlboard, elevator) + ) + .finallyDo((boolean interrupted) -> { + if (interrupted) + claw.setClawRollersPower(ClawConfs.CLAW_CONE_STALL_POWER); + }); + } + public static CommandBase manual( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands + .sequence( + Commands.deadline( + Commands.sequence( + new ClawOpenToCommand(claw, ClawPositions.CLAW_CLOSED_POS_TICKS), + ClawRollersCommands.desiredPieceCommand( + claw, + controlboard, + IntakeSpeed.CUBE_POWER, + ClawDirections.CLAW_ROLLERS_CUBE_IN, + IntakeSpeed.CONE_POWER, + ClawDirections.CLAW_ROLLERS_CONE_IN + ), + ClawRollersCommands.waitSequenceCommand(claw, controlboard) + ), + LedCommands.dynamicBlinkLedCommand(ledController, controlboard), + new ElevatorRaiseToCommand( + elevator, controlboard, ElevatorPositions.ELEVATOR_DROP_CONE_INTAKE + ) + ), + ClawRollersCommands.desiredPieceStallCommand(claw, controlboard), + StowCommands.tele(claw, controlboard, elevator) + ) + .alongWith( + new TeleopSwerveCommand(drivetrain, elevator, controlboard, DriveModes.SNAP_TO_ANGLE) + .beforeStarting(() -> drivetrain.setSnapAngleDeg(90.0)) + ) + .beforeStarting(() -> { + ledController.setOverride(true); + ledController.turnOffLeds(LedSections.ALL); + }) + .finallyDo((boolean interrupted) -> { + if (interrupted) + claw.setClawRollersPower(ClawConfs.CLAW_CONE_STALL_POWER); + ledController.setOverride(false); + ledController.turnOffLeds(LedSections.ALL); + }); + } +} diff --git a/src/main/java/frc/robot/commands/StowCommands.java b/src/main/java/frc/robot/commands/StowCommands.java new file mode 100644 index 0000000..3a191eb --- /dev/null +++ b/src/main/java/frc/robot/commands/StowCommands.java @@ -0,0 +1,22 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.subsystems.*; + +public class StowCommands { + public static CommandBase tele(Claw claw, Controlboard controlboard, Elevator elevator) { + return new ElevatorRaiseToCommand(elevator, controlboard, ElevatorPositions.ELEVATOR_STOW_TICKS) + .finallyDo((boolean interrupted) -> claw.closeClawSolenoid()); + } + + public static CommandBase auto(Claw claw, Controlboard controlboard, Elevator elevator) { + return Commands.sequence( + new ElevatorRaiseToCommand(elevator, controlboard, ElevatorPositions.ELEVATOR_STOW_TICKS), + claw.closeClawCommand() + ); + } +} diff --git a/src/main/java/frc/robot/commands/TeleopLedCommand.java b/src/main/java/frc/robot/commands/TeleopLedCommand.java new file mode 100644 index 0000000..0eb4f52 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopLedCommand.java @@ -0,0 +1,47 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.LedController; + +public class TeleopLedCommand extends CommandBase { + private final LedController _ledController; + private final Controlboard _controlboard; + private boolean _isIntaking; + private boolean _wasIntaking; + private GamePieceType _desiredGamePiece = GamePieceType.UNKNOWN_GAME_PIECE; + private HPIntakeStations _desiredHPIntakeStations = HPIntakeStations.UNKNOWN_INTAKE_STATION; + private ScoringLocations _desiredScoringLocation = ScoringLocations.UNKNOWN_SCORING_LOCATION; + private ScoringLevels _desiredScoringLevel = ScoringLevels.UNKNOWN_SCORING_LEVEL; + + public TeleopLedCommand(LedController ledController, Controlboard controlboard) { + _controlboard = controlboard; + _ledController = ledController; + + addRequirements(ledController); + } + + @Override + public void execute() { + GamePieceType temp = _controlboard.getHeldGamePiece(); + if (temp == GamePieceType.CONE_GAME_PIECE || temp.isCube()) { + _isIntaking = false; + } else if (temp.isUnknown()) { + _isIntaking = true; + } else { + _isIntaking = !_isIntaking; + } + + if (!(_isIntaking == _wasIntaking) + || _desiredGamePiece != _controlboard.getDesiredGamePiece()) { + _desiredGamePiece = _controlboard.getDesiredGamePiece(); + _ledController.setSolidColor(_desiredGamePiece.intakeColor, LedSections.ALL); + } + _wasIntaking = _isIntaking; + } +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerveCommand.java b/src/main/java/frc/robot/commands/TeleopSwerveCommand.java new file mode 100644 index 0000000..7fb23e7 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopSwerveCommand.java @@ -0,0 +1,194 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.Constants; +import frc.robot.constants.Constants.Control; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.DriveModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; + +public class TeleopSwerveCommand extends CommandBase { + private final Drivetrain _drivetrain; + private final Elevator _elevator; + private final Controlboard _controlboard; + private DriveModes _driveMode; + private double _maxSpeedMPS; + private Rotation2d _maxAngularSpeedPS; + + private static final DriveModes DEFAULT_DRIVE_MODE = DriveModes.FIELD_RELATIVE; + + public TeleopSwerveCommand( + Drivetrain drivetrain, + Elevator elevator, + Controlboard controlboard, + DriveModes initialMode, + double maxSpeedMPS, + Rotation2d maxAngularSpeedPS + ) { + _drivetrain = drivetrain; + _elevator = elevator; + _controlboard = controlboard; + _driveMode = initialMode; + _maxSpeedMPS = maxSpeedMPS; + _maxAngularSpeedPS = maxAngularSpeedPS; + + addRequirements(drivetrain); + } + + public TeleopSwerveCommand( + Drivetrain drivetrain, Elevator elevator, Controlboard controlboard, DriveModes initialMode + ) { + this( + drivetrain, + elevator, + controlboard, + initialMode, + Constants.CRobot.drive.control.defaultLimits.maxTranslationalVelocityMPS, + Constants.CRobot.drive.control.defaultLimits.maxAngularVelocityPS + ); + } + + public void teleopInit() { + _driveMode = DEFAULT_DRIVE_MODE; + } + + public boolean condActivateSnapToAngle(int anglePOV) { + if (anglePOV != -1) { + _driveMode = DriveModes.SNAP_TO_ANGLE; + // note that the pov axis does not align, we'll need some sort of flat + // offset here Our functional 0 will be right facing to align with + // odometry (3 on a standard clock face) POV 0 is "forward" (12 on a + // standard clock face) + // _drivetrain.setSnapAngle(((double) -anglePOV) + _drivetrain.POVZeroOffsetDeg()); // odom + // zero offset + + var deg = ((double) -anglePOV) + _drivetrain.POVZeroOffsetDeg(); + if (Math.abs(anglePOV) == 180.0 || Math.abs(anglePOV) == 0.0 || Math.abs(anglePOV) == 90.0 + || Math.abs(anglePOV) == 270.0) { + _drivetrain.setSnapAngleDeg(deg); + } + return true; + } + return false; + } + + public void condActivateSnapToAngleCritical(boolean trigger) { + if (trigger) { + _driveMode = DriveModes.SNAP_TO_ANGLE; + + _drivetrain.setSnapAngleDeg( + _drivetrain.isRedAlliance() + ^ (_drivetrain.getPose().getX() > Constants.CField.dims.halfX_M) + ? 180.0 + : 0.0 + ); + } + } + + public boolean condActivateChaseStaticTarget(Pose2d pose) { + if (_controlboard._xboxDrive.getHID().getXButtonPressed()) { + _driveMode = DriveModes.CHASE_STATIC_TARGET; + _drivetrain.setStaticTarget(pose); + return true; + } + return false; + } + + public boolean condActivateSnake() { + if (_controlboard._xboxDrive.getHID().getLeftBumperPressed()) { + _driveMode = DriveModes.SNAKE; + return true; + } + return false; + } + + public void setAntiTipMode() { + _driveMode = DriveModes.FIELD_RELATIVE_ANTI_TIP; + } + + public void setDefaultDriveMode() { + _driveMode = DEFAULT_DRIVE_MODE; + } + + @Override + public void execute() { + // this code should probably move, but here before the state machine + // management is also ok while dpad is pressed, set drive mode to + // SNAP_TO_ANGLE and give the drivetrain a new target angle + int anglePOV = 0; // needs to be initialized, but it doesn't matter what it's initalized to + // right? bc anglePOV isn't used unless isPIDTuningMode + + double xAxis = _controlboard.getDriveX(); + double yAxis = _controlboard.getDriveY(); + double rAxis = _controlboard.getRotX(); + double xyNet = Math.hypot(xAxis, yAxis); + + if (RobotAltModes.isPIDTuningMode) { + anglePOV = _controlboard._xboxDrive.getHID().getPOV(); + } + switch (_driveMode) { + case ROBOT_CENTRIC: + case FIELD_RELATIVE: + case FIELD_RELATIVE_ROTATION_COMPENSATION_SCALING: + case SLEWING_FIELD_RELATIVE: + case FIELD_RELATIVE_SKEW_COMPENSATION: + case FIELD_RELATIVE_254: + condActivateSnapToAngle(_controlboard._xboxDrive.getHID().getPOV()); + // condActivateSnapToAngleCritical(_controlboard._xboxDrive.getHID().getAButtonPressed()); + + if (RobotAltModes.isPIDTuningMode && anglePOV != -1) { + _drivetrain.testSteer((double) anglePOV); + } + break; + case FIELD_RELATIVE_ANTI_TIP: + _drivetrain.drive( + xAxis, + yAxis, + xyNet, + rAxis, + _elevator.getCurrentHeight() > ElevatorPositions.ELEVATOR_TIP_PREVENTION_THRESHOLD.ticks + ? DriveModes.FIELD_RELATIVE_ANTI_TIP + : DEFAULT_DRIVE_MODE, + _maxSpeedMPS + ); + return; + case SNAP_TO_ANGLE: + if (rAxis > Control.STICK_DEADBAND || rAxis < -Control.STICK_DEADBAND) { + _driveMode = DEFAULT_DRIVE_MODE; + } else { + condActivateSnapToAngle(_controlboard._xboxDrive.getHID().getPOV()); + } + break; + case SNAKE: + if (_controlboard._xboxDrive.getHID().getLeftBumperPressed() + || rAxis > Control.STICK_DEADBAND || rAxis < -Control.STICK_DEADBAND) { + _driveMode = DEFAULT_DRIVE_MODE; + } + break; + case TARGET_RELATIVE: + case CHASE_STATIC_TARGET: + if (rAxis > Control.STICK_DEADBAND || rAxis < -Control.STICK_DEADBAND + || xyNet > Control.STICK_NET_DEADBAND + || _controlboard._xboxDrive.getHID().getXButtonPressed()) { + _driveMode = DEFAULT_DRIVE_MODE; + } + break; + case CHASE_DYNAMIC_TARGET: + default: + System.err.println("Unkown drive mode"); + _driveMode = DEFAULT_DRIVE_MODE; + break; + } + + // We need logic here or in the drive commands that allows for desaturation to occur + // contextually, capping maximal turning speed when stationary but not allowing the drive + // command to override it due to maximum rotational speed being capped lower relative to the + // theoretical value + _drivetrain.drive(xAxis, yAxis, xyNet, rAxis, _driveMode, _maxSpeedMPS, _maxAngularSpeedPS); + } +} diff --git a/src/main/java/frc/robot/commands/base/ChargingStationBalanceCommand.java b/src/main/java/frc/robot/commands/base/ChargingStationBalanceCommand.java new file mode 100644 index 0000000..6f69ef7 --- /dev/null +++ b/src/main/java/frc/robot/commands/base/ChargingStationBalanceCommand.java @@ -0,0 +1,119 @@ +package frc.robot.commands.base; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import java.lang.Math; + +public class ChargingStationBalanceCommand extends CommandBase { + private Drivetrain _drivetrain; + private Controlboard _controlboard; + private boolean _allianceInvert; + private double _tiltDeg; + private double _targetRangeDeg; + private double _trimRangeDeg; + private double _trimSpeedMPS; + private double _maxSpeedMPS; + private double _desiredSpeedMPS; + private boolean _usingPitch; + private boolean _negateTilt; + private Timer _timer = new Timer(); + + public ChargingStationBalanceCommand( + Drivetrain drivetrain, + Controlboard controlboard, + boolean allianceInvert, + double targetRangeDeg, + double trimRangeDeg, + double trimSpeedMPS, + double maxSpeedMPS + ) { + _drivetrain = drivetrain; + _controlboard = controlboard; + _allianceInvert = allianceInvert; + _targetRangeDeg = Math.abs(targetRangeDeg); + _trimRangeDeg = Math.abs(trimRangeDeg); + _trimSpeedMPS = trimSpeedMPS; + _maxSpeedMPS = maxSpeedMPS; + + addRequirements(drivetrain); + } + + public ChargingStationBalanceCommand(Drivetrain drivetrain, Controlboard controlboard) { + this(drivetrain, controlboard, false, 7, 16, 0.13, 0.35); + } + + @Override + public void initialize() { + configureAndSnapAngle(); + + _timer.restart(); + } + + @Override + public void execute() { + configureAndSnapAngle(); + + double _tilt = + _usingPitch ? _drivetrain.getPitch().getDegrees() : _drivetrain.getRoll().getDegrees(); + _tilt = _tilt * (_negateTilt ? 1.0 : -1.0); + double absTilt = Math.abs(_tilt); + + if (absTilt > _targetRangeDeg) { + _timer.reset(); + _desiredSpeedMPS = absTilt > _trimRangeDeg ? _maxSpeedMPS : _trimSpeedMPS; + _desiredSpeedMPS = _tilt <= 0 ? -_desiredSpeedMPS : _desiredSpeedMPS; + } else { + _timer.start(); + _desiredSpeedMPS = 0; + } + + if (_allianceInvert) { + _drivetrain.snapToAngleDrive( + DriverStation.getAlliance() == DriverStation.Alliance.Red ? _desiredSpeedMPS + : -_desiredSpeedMPS, + 0 + ); + } else { + _drivetrain.snapToAngleDrive(_desiredSpeedMPS, 0); + } + } + + @Override + public void end(boolean interrupted) { + _drivetrain.forceChargingWheelDirection(); + _drivetrain.fieldRelativeDrive(0, 0, 0); + } + + @Override + public boolean isFinished() { + return _timer.hasElapsed(1.0); + // TODO: fill in with actual value; + } + + public void configureAndSnapAngle() { + double yaw = _drivetrain.toAbsoluteAngleDeg(_drivetrain.getYaw().getDegrees()); + + if (yaw > 315 || yaw < 45) { + _drivetrain.setSnapAngleDeg(0); + _usingPitch = true; + _negateTilt = false; + } else if (yaw >= 45 && yaw < 135) { + _drivetrain.setSnapAngleDeg(90); + _usingPitch = false; + _negateTilt = false; + + } else if (yaw >= 135 && yaw < 225) { + _drivetrain.setSnapAngleDeg(180); + _usingPitch = true; + _negateTilt = true; + } else { + // } else if (yaw >= 225_deg || yaw < 315_deg) { + _drivetrain.setSnapAngleDeg(270); + _usingPitch = false; + _negateTilt = true; + } + } +} diff --git a/src/main/java/frc/robot/commands/base/ClawOpenToCommand.java b/src/main/java/frc/robot/commands/base/ClawOpenToCommand.java new file mode 100644 index 0000000..e3661df --- /dev/null +++ b/src/main/java/frc/robot/commands/base/ClawOpenToCommand.java @@ -0,0 +1,54 @@ +package frc.robot.commands.base; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.subsystems.Claw; + +public class ClawOpenToCommand extends CommandBase { + private final Claw _claw; + private ClawPositions _clawPosition; + private final int _lowerBound, _upperBound; + + public ClawOpenToCommand(Claw claw, ClawPositions clawPosition, int upperBound, int lowerBound) { + _claw = claw; + _clawPosition = clawPosition; + _upperBound = upperBound; + _lowerBound = -Math.abs(lowerBound); + + addRequirements(claw); + } + + public ClawOpenToCommand(Claw claw, ClawPositions clawPosition, int tolerance) { + this(claw, clawPosition, tolerance, tolerance); + } + + public ClawOpenToCommand(Claw claw, ClawPositions clawPosition) { + this( + claw, clawPosition, ClawPositions.CLAW_TOLERANCE_TICKS, ClawPositions.CLAW_TOLERANCE_TICKS + ); + } + + @Override + public void initialize() { + _claw.setTargetPosition(_clawPosition); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + if (!Constants.isPneumaticClaw) { + int pos = _claw.getClawPosition(); + if (interrupted || pos < ClawPositions.CLAW_MIN_TICKS.ticks + || pos > ClawPositions.CLAW_OPEN_POS_TICKS.ticks) { + _claw.setClawPower(0.0); + } + } + } + + @Override + public boolean isFinished() { + return _claw.inTolerance(_lowerBound, _upperBound); + } +} diff --git a/src/main/java/frc/robot/commands/base/ClawOuttakeCommand.java b/src/main/java/frc/robot/commands/base/ClawOuttakeCommand.java new file mode 100644 index 0000000..4d9f599 --- /dev/null +++ b/src/main/java/frc/robot/commands/base/ClawOuttakeCommand.java @@ -0,0 +1,56 @@ +package frc.robot.commands.base; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.ClawScoreLevels; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; + +public class ClawOuttakeCommand extends CommandBase { + private final Claw _claw; + private final Controlboard _controlboard; + private ClawScoreLevels _clawScoreLevel; + private int _counter; + private boolean _isAuto; + + public ClawOuttakeCommand( + Claw claw, Controlboard controlboard, boolean isAuto, ClawScoreLevels clawScoreLevel + ) { + _claw = claw; + _controlboard = controlboard; + _clawScoreLevel = clawScoreLevel; + _isAuto = isAuto; + + addRequirements(claw); + } + + public ClawOuttakeCommand(Claw claw, Controlboard controlboard) { + this(claw, controlboard, false, ClawScoreLevels.CLAW_SCORE_UNKNOWN); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + _claw.setClawRollersPower( + _clawScoreLevel == ClawScoreLevels.CLAW_SCORE_UNKNOWN + ? _controlboard.getDesiredScoringLevel().getPower(_controlboard.getHeldGamePiece()) + : _clawScoreLevel.power + ); + } + + @Override + public void end(boolean interrupted) { + _controlboard.unsetHeldGamePiece(); + _claw.setClawRollersPower(0.0); + if (_isAuto) + _claw.closeClawSolenoid(); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/base/ElevatorRaiseToCommand.java b/src/main/java/frc/robot/commands/base/ElevatorRaiseToCommand.java new file mode 100644 index 0000000..a0df18b --- /dev/null +++ b/src/main/java/frc/robot/commands/base/ElevatorRaiseToCommand.java @@ -0,0 +1,109 @@ +package frc.robot.commands.base; + +import edu.wpi.first.networktables.GenericPublisher; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.ElevatorConfs; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Elevator; + +public class ElevatorRaiseToCommand extends CommandBase { + private final Elevator _elevator; + private final Controlboard _controlboard; + private final int _lowerTolerance, _upperTolerance; + private ElevatorPositions _elevatorPosition; + private ElevatorPositions _configuredPosition; + private boolean _startedMovement; + private static GenericPublisher _finishedTimeEntry; + private Timer _timer = new Timer(); + + public ElevatorRaiseToCommand( + Elevator elevator, + Controlboard controlboard, + ElevatorPositions elevatorPosition, + int lowerTolerance, + int upperTolerance + ) { + _elevator = elevator; + _controlboard = controlboard; + _elevatorPosition = elevatorPosition; + _lowerTolerance = -Math.abs(lowerTolerance); + _upperTolerance = upperTolerance; + + addRequirements(elevator); + if (_finishedTimeEntry == null && RobotAltModes.isElevatorTuningMode) { + ShuffleboardTab tuningTab = Shuffleboard.getTab("Tuning tab"); + _finishedTimeEntry = + tuningTab.add("Time finished elevator raise", 0.0).withPosition(1, 1).getEntry(); + } + } + + public ElevatorRaiseToCommand( + Elevator elevator, Controlboard controlboard, ElevatorPositions elevatorPosition + ) { + this( + elevator, + controlboard, + elevatorPosition, + ElevatorConfs.ELEVATOR_TOLERANCE_TICKS, + ElevatorConfs.ELEVATOR_TOLERANCE_TICKS + ); + } + + public ElevatorRaiseToCommand(Elevator elevator, Controlboard controlboard) { + this( + elevator, + controlboard, + ElevatorPositions.ELEVATOR_UNKNOWN_POSITION, + ElevatorConfs.ELEVATOR_TOLERANCE_TICKS, + ElevatorConfs.ELEVATOR_TOLERANCE_TICKS + ); + } + + @Override + public void initialize() { + _startedMovement = false; + _configuredPosition = (_elevatorPosition.isUnknown()) + ? _elevator.getElevatorScoringHeight( + _controlboard.getHeldGamePiece(), _controlboard.getDesiredScoringLevel() + ) + : _elevatorPosition; + } + + @Override + public void execute() { + // If elevator has not started moving, check for legal states + if (!_startedMovement) { + _startedMovement = true; + _elevator.setElevatorProfile(_configuredPosition); + if (RobotAltModes.isElevatorTuningMode) { + _timer.start(); + } + } + } + + @Override + public void end(boolean interrupted) { + int height = _elevator.getCurrentHeight(); + + // Emergency shutdown + if (height < ElevatorPositions.ELEVATOR_MIN_TICKS.ticks + || height > ElevatorPositions.ELEVATOR_MAX_TICKS.ticks) { + _elevator.setPower(0.0); + } + if (RobotAltModes.isElevatorTuningMode) { + _finishedTimeEntry.setDouble(_timer.get()); + } + } + + @Override + public boolean isFinished() { + // if going up shutdown if we go over max ticks + // if going down shutdown if we go under min ticks + return _elevator.inTolerance(_lowerTolerance, _upperTolerance); + } +} diff --git a/src/main/java/frc/robot/commands/base/ElevatorZeroCommand.java b/src/main/java/frc/robot/commands/base/ElevatorZeroCommand.java new file mode 100644 index 0000000..4c25229 --- /dev/null +++ b/src/main/java/frc/robot/commands/base/ElevatorZeroCommand.java @@ -0,0 +1,50 @@ +package frc.robot.commands.base; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.ElevatorConfs; +import frc.robot.subsystems.Elevator; + +public class ElevatorZeroCommand extends CommandBase { + private final Elevator _elevator; + private int _prev; + private int _count; + + public ElevatorZeroCommand(Elevator elevator) { + _elevator = elevator; + + addRequirements(elevator); + } + + @Override + public void initialize() { + _elevator.setPower(ElevatorConfs.ELEVATOR_ZEROING_SPEED_MPS); + _prev = _elevator.getCurrentHeight(); + _count = 0; + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + _elevator.setPower(0.0); + if (!interrupted) + _elevator.setEncoder(0); + } + + @Override + public boolean isFinished() { + int currHeight = _elevator.getCurrentHeight(); + + if (_prev - currHeight > ElevatorConfs.ELEVATOR_ZEROING_STALL_TOLERANCE) { + _prev = currHeight; + _count = 0; + return false; + } else if (_count++ < ElevatorConfs.ELEVATOR_ZEROING_STALL_LOOPS) { + _prev = currHeight; + return false; + } else { + return true; + } + } +} diff --git a/src/main/java/frc/robot/commands/base/GoToPoseCommand.java b/src/main/java/frc/robot/commands/base/GoToPoseCommand.java new file mode 100644 index 0000000..e79040d --- /dev/null +++ b/src/main/java/frc/robot/commands/base/GoToPoseCommand.java @@ -0,0 +1,95 @@ +package frc.robot.commands.base; + +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.wpilibj2.command.CommandBase; +import frc.robot.constants.Constants; +import frc.robot.subsystems.Drivetrain; + +public class GoToPoseCommand extends CommandBase { + private final Drivetrain _drivetrain; + private final Transform2d _relative; + private final boolean _isRelative; + private final double _xToleranceM; + private final double _yToleranceM; + private final Rotation2d _thetaTolerance; + private Pose2d _target; + + public GoToPoseCommand( + Drivetrain drivetrain, + Pose2d pose, + double xToleranceM, + double yToleranceM, + Rotation2d thetaTolerance + ) { + _isRelative = false; + _drivetrain = drivetrain; + _xToleranceM = xToleranceM; + _yToleranceM = yToleranceM; + _thetaTolerance = thetaTolerance; + + if (_isRelative) { + _relative = new Transform2d(new Pose2d(), pose); + } else { + _target = pose; + _relative = null; + } + + addRequirements(drivetrain); + } + + public GoToPoseCommand( + Drivetrain drivetrain, + Transform2d relative, + double xToleranceM, + double yToleranceM, + Rotation2d thetaTolerance + ) { + _isRelative = true; + _drivetrain = drivetrain; + _relative = relative; + _xToleranceM = xToleranceM; + _yToleranceM = yToleranceM; + _thetaTolerance = thetaTolerance; + + addRequirements(drivetrain); + } + + public GoToPoseCommand(Drivetrain drivetrain, Pose2d pose) { + this( + drivetrain, + pose, + Constants.CRobot.drive.control.xy.toleranceM, + Constants.CRobot.drive.control.xy.toleranceM, + Constants.CRobot.drive.control.theta.tolerance + ); + } + + @Override + public void initialize() { + _drivetrain.setTolerance(_xToleranceM, _yToleranceM, _thetaTolerance); + + if (_isRelative) { + _target = _drivetrain.getPose().transformBy(_relative); + } + + _drivetrain.setStaticTarget(_target); + } + + @Override + public void execute() { + _drivetrain.chaseStaticTargetDrive(); + } + + @Override + public void end(boolean interrupted) { + _drivetrain.resetDefaultTolerance(); + _drivetrain.fieldRelativeDrive(0.0, 0.0, 0.0); + } + + @Override + public boolean isFinished() { + return _drivetrain.inRange(); + } +} diff --git a/src/main/java/frc/robot/commands/base/PrepForPlatformIntakeCommand.java b/src/main/java/frc/robot/commands/base/PrepForPlatformIntakeCommand.java new file mode 100644 index 0000000..e165021 --- /dev/null +++ b/src/main/java/frc/robot/commands/base/PrepForPlatformIntakeCommand.java @@ -0,0 +1,23 @@ +package frc.robot.commands.base; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import frc.robot.commands.CommandUtils.ElevatorUtils; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Elevator; + +public class PrepForPlatformIntakeCommand extends ParallelDeadlineGroup { + public PrepForPlatformIntakeCommand(Claw claw, Controlboard controlboard, Elevator elevator) { + super( + ElevatorUtils.pieceSpecificElevatorRaiseToCommand( + claw, + controlboard, + elevator, + ElevatorPositions.ELEVATOR_PLATFORM_CUBE_INTAKE, + ElevatorPositions.ELEVATOR_PLATFORM_CONE_INTAKE + ), + new ClawOpenToCommand(claw, ClawPositions.CLAW_OPEN_POS_TICKS) + ); + } +} diff --git a/src/main/java/frc/robot/commands/base/SetClawGivenElevatorCommand.java b/src/main/java/frc/robot/commands/base/SetClawGivenElevatorCommand.java new file mode 100644 index 0000000..04de9de --- /dev/null +++ b/src/main/java/frc/robot/commands/base/SetClawGivenElevatorCommand.java @@ -0,0 +1,55 @@ +package frc.robot.commands.base; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Elevator; + +public class SetClawGivenElevatorCommand extends CommandBase { + private final Claw _claw; + private final Controlboard _controlboard; + private final Elevator _elevator; + private ClawPositions _clawPosition; + private int _elevatorPosition; + private boolean _higher; + + public SetClawGivenElevatorCommand( + Claw claw, + Controlboard controlboard, + Elevator elevator, + ClawPositions desiredClawPosition, + int elevatorPosition, + boolean higher + ) { + _claw = claw; + _controlboard = controlboard; + _elevator = elevator; + _clawPosition = desiredClawPosition; + _elevatorPosition = elevatorPosition; + _higher = higher; + } + + @Override + public void initialize() {} + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + if (!interrupted) { + if (_clawPosition == ClawPositions.CLAW_MAX_TICKS) { + _claw.openClawSolenoid(); + } else { + _claw.closeClawSolenoid(); + } + } + } + + @Override + public boolean isFinished() { + return _higher ? _elevator.getCurrentHeight() >= _elevatorPosition + : _elevator.getCurrentHeight() <= _elevatorPosition; + } +} diff --git a/src/main/java/frc/robot/commands/score/TeleopScoreCommands.java b/src/main/java/frc/robot/commands/score/TeleopScoreCommands.java new file mode 100644 index 0000000..52a8d11 --- /dev/null +++ b/src/main/java/frc/robot/commands/score/TeleopScoreCommands.java @@ -0,0 +1,538 @@ +package frc.robot.commands.score; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.DynamicGoToScoringLocationCommand; +import frc.robot.commands.LedCommands; +import frc.robot.commands.PrepToScoreCommands; +import frc.robot.commands.StowCommands; +import frc.robot.commands.TeleopSwerveCommand; +import frc.robot.commands.base.ClawOuttakeCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.Constants; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.enums.DriveModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.LedColors; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.constants.enums.Zones; +import frc.robot.subsystems.*; +import java.util.HashMap; +import java.util.Map; + +public class TeleopScoreCommands { + public static Map scoringCommands; + public static Map automaticScoringCommands; + + public static CommandBase manual( + ScoringLevels level, + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + if (scoringCommands == null) { + scoringCommands = new HashMap(); + scoringCommands.put(0, level0(claw, drivetrain, elevator, ledController, controlboard)); + scoringCommands.put(1, level1(claw, drivetrain, elevator, ledController, controlboard)); + scoringCommands.put(2, level2(claw, drivetrain, elevator, ledController, controlboard)); + } + return Commands.select( + scoringCommands, + () + -> level == ScoringLevels.UNKNOWN_SCORING_LEVEL + ? Math.min(controlboard.getDesiredScoringLevel().id, 2) + : level.id + ); + } + + public static CommandBase level0( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands.sequence( + Commands.deadline( + Commands.sequence( + prep(ledController, controlboard), + Commands.either( + PrepToScoreCommands.teleop( + claw, + controlboard, + elevator, + ledController, + ElevatorPositions.ELEVATOR_LEVEL_0_CUBE, + GamePieceType.CUBE_GAME_PIECE + ), + PrepToScoreCommands.teleop( + claw, + controlboard, + elevator, + ledController, + ElevatorPositions.ELEVATOR_LEVEL_0_CONE, + GamePieceType.CONE_GAME_PIECE + ), + () -> controlboard.getHeldGamePiece().isCube() + ), + confirmAndEject(claw, ledController, controlboard) + ), + driveAndSnap(drivetrain, elevator, controlboard) + ), + Commands + .deadline( + StowCommands.tele(claw, controlboard, elevator), + new TeleopSwerveCommand( + drivetrain, elevator, controlboard, DriveModes.FIELD_RELATIVE + ) + ) + .finallyDo((boolean interrupted) -> { + if (!interrupted) { + controlboard.setHeldGamePiece(GamePieceType.UNKNOWN_GAME_PIECE); + } + }) + ); + } + + public static CommandBase level1( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands.sequence( + Commands.deadline( + Commands.sequence( + prep(ledController, controlboard), + Commands.either( + PrepToScoreCommands.teleop( + claw, + controlboard, + elevator, + ledController, + ElevatorPositions.ELEVATOR_LEVEL_1_CUBE, + GamePieceType.CUBE_GAME_PIECE + ), + Commands.sequence( + claw.openClawCommand(), + PrepToScoreCommands.teleop( + claw, + controlboard, + elevator, + ledController, + ElevatorPositions.ELEVATOR_LEVEL_1_CONE, + GamePieceType.CONE_GAME_PIECE + ) + ), + () -> controlboard.getHeldGamePiece().isCube() + ), + confirmAndEject(claw, ledController, controlboard) + ), + driveAndSnap(drivetrain, elevator, controlboard) + ), + backoutAndStow(claw, drivetrain, elevator, controlboard) + ); + } + + public static CommandBase level2( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands.sequence( + Commands.deadline( + Commands.sequence( + prep(ledController, controlboard), + Commands.either( + Commands.sequence( + PrepToScoreCommands.teleop( + claw, + controlboard, + elevator, + ledController, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE, + GamePieceType.CUBE_GAME_PIECE + ), + claw.openClawCommand(), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_OUT_TIME_S) + ), + PrepToScoreCommands.teleop( + claw, + controlboard, + elevator, + ledController, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + GamePieceType.CONE_GAME_PIECE + ), + () -> controlboard.getHeldGamePiece().isCube() + ), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_OUT_TIME_S) + .beforeStarting(claw.openClawCommand()), + confirmAndEject(claw, ledController, controlboard) + ), + driveAndSnap(drivetrain, elevator, controlboard) + ), + backoutAndStow(claw, drivetrain, elevator, controlboard) + ); + } + + public static CommandBase automatic( + ScoringLevels level, + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + if (automaticScoringCommands == null) { + automaticScoringCommands = new HashMap(); + automaticScoringCommands.put( + 0, automaticLevel0(claw, drivetrain, elevator, ledController, controlboard) + ); + automaticScoringCommands.put( + 1, automaticLevel1(claw, drivetrain, elevator, ledController, controlboard) + ); + automaticScoringCommands.put( + 2, automaticLevel2(claw, drivetrain, elevator, ledController, controlboard) + ); + } + return Commands.select( + automaticScoringCommands, + () + -> level == ScoringLevels.UNKNOWN_SCORING_LEVEL + ? Math.min(controlboard.getDesiredScoringLevel().id, 2) + : level.id + ); + } + + public static CommandBase automaticLevel0( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands + .sequence( + prep(ledController, controlboard, LedColors.BLUE), + new DynamicGoToScoringLocationCommand( + drivetrain, + controlboard, + false, + Constants.SCORING_PREALIGN_RED_OFFSET, + ScoringLocations.UNKNOWN_SCORING_LOCATION, + Units.inchesToMeters(2), + Units.inchesToMeters(5), + Rotation2d.fromDegrees(10.0) + ), + PrepToScoreCommands.teleop(claw, controlboard, elevator, ledController), + Commands.deadline( + Commands.waitUntil(controlboard._xboxDrive.b()), + new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.SNAP_TO_ANGLE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + .beforeStarting(() -> { + controlboard.driverResetRumble(); + drivetrain.setSnapScoringAngle(); + }) + .finallyDo((boolean interrupted) -> controlboard.driverRumble()) + ), + LedCommands.dynamicBlinkLedCommand(ledController, controlboard), + Commands.either( + new ClawOuttakeCommand(claw, controlboard) + .withTimeout(ScoringLevels.Consts.TELEOP_CUBE_SHOT_TIMEOUT_S) + .andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S)), + new ClawOuttakeCommand(claw, controlboard) + .withTimeout(ScoringLevels.Consts.TELEOP_YEET_TIMEOUT_S), + () -> controlboard.getHeldGamePiece().isCube() + ), + + Commands.deadline( + Commands.sequence( + claw.closeClawCommand(), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S * 2), + StowCommands.tele(claw, controlboard, elevator) + ), + Commands.sequence( + drivetrain.forceAllianceBasedFieldRelativeMovementCommand(0.5, 0.0, 0.5), + new TeleopSwerveCommand( + drivetrain, elevator, controlboard, DriveModes.FIELD_RELATIVE + ) + ) + ) + + ) + .beforeStarting(() -> { + controlboard.driverRumble(); + ledController.setOverride(true); + }) + .finallyDo((boolean interrupted) -> { + controlboard.driverResetRumble(); + ledController.setOverride(false); + ledController.turnOffLeds(LedSections.ALL); + }) + .unless(() -> { + return drivetrain.isRedAlliance() + ? (drivetrain.getPose().getX() <= Zones.LocationMath.COMMUNITY_ZONE_RED_MIN_X_M) + : (drivetrain.getPose().getX() >= Zones.LocationMath.COMMUNITY_ZONE_BLUE_MAX_X_M); + }); + } + + public static CommandBase automaticLevel1( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands + .sequence( + prep(ledController, controlboard, LedColors.BLUE), + new DynamicGoToScoringLocationCommand( + drivetrain, + controlboard, + false, + Constants.SCORING_PREALIGN_RED_OFFSET, + ScoringLocations.UNKNOWN_SCORING_LOCATION, + Units.inchesToMeters(2), + Units.inchesToMeters(5), + Rotation2d.fromDegrees(10.0) + ), + claw.openClawCommand().unless( + () -> controlboard.getDesiredScoringLevel() != ScoringLevels.SCORING_LEVEL_1 + ), + PrepToScoreCommands.teleop(claw, controlboard, elevator, ledController), + Commands.deadline( + Commands.waitUntil(controlboard._xboxDrive.b()), + new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.SNAP_TO_ANGLE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + .beforeStarting(() -> { + controlboard.driverResetRumble(); + drivetrain.setSnapScoringAngle(); + }) + .finallyDo((boolean interrupted) -> controlboard.driverRumble()) + ), + LedCommands.dynamicBlinkLedCommand(ledController, controlboard), + Commands.either( + new ClawOuttakeCommand(claw, controlboard) + .withTimeout(ScoringLevels.Consts.TELEOP_CUBE_SHOT_TIMEOUT_S) + .andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S)), + new ClawOuttakeCommand(claw, controlboard) + .withTimeout(ScoringLevels.Consts.TELEOP_CONE_SHOT_TIMEOUT_S) + .andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S)), + () -> controlboard.getHeldGamePiece().isCube() + ), + + Commands.deadline( + Commands.sequence( + claw.closeClawCommand(), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S * 2), + StowCommands.tele(claw, controlboard, elevator) + ), + Commands.sequence( + drivetrain.forceAllianceBasedFieldRelativeMovementCommand(0.5, 0.0, 0.5), + new TeleopSwerveCommand( + drivetrain, elevator, controlboard, DriveModes.FIELD_RELATIVE + ) + ) + ) + + ) + .beforeStarting(() -> { + controlboard.driverRumble(); + ledController.setOverride(true); + }) + .finallyDo((boolean interrupted) -> { + controlboard.driverResetRumble(); + ledController.setOverride(false); + ledController.turnOffLeds(LedSections.ALL); + }) + .unless(() -> { + return drivetrain.isRedAlliance() + ? (drivetrain.getPose().getX() <= Zones.LocationMath.COMMUNITY_ZONE_RED_MIN_X_M) + : (drivetrain.getPose().getX() >= Zones.LocationMath.COMMUNITY_ZONE_BLUE_MAX_X_M); + }); + } + + public static CommandBase automaticLevel2( + Claw claw, + Drivetrain drivetrain, + Elevator elevator, + LedController ledController, + Controlboard controlboard + ) { + return Commands + .sequence( + prep(ledController, controlboard, LedColors.BLUE), + new DynamicGoToScoringLocationCommand( + drivetrain, + controlboard, + false, + Constants.SCORING_PREALIGN_RED_OFFSET, + ScoringLocations.UNKNOWN_SCORING_LOCATION, + Units.inchesToMeters(2), + Units.inchesToMeters(5), + Rotation2d.fromDegrees(10.0) + ), + PrepToScoreCommands.teleop(claw, controlboard, elevator, ledController), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_OUT_TIME_S) + .beforeStarting(claw.openClawCommand()) + .unless( + () -> controlboard.getDesiredScoringLevel() != ScoringLevels.SCORING_LEVEL_2 + ), + Commands.deadline( + Commands.waitUntil(controlboard._xboxDrive.b()), + new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.SNAP_TO_ANGLE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + .beforeStarting(() -> { + controlboard.driverResetRumble(); + drivetrain.setSnapScoringAngle(); + }) + .finallyDo((boolean interrupted) -> controlboard.driverRumble()) + ), + LedCommands.dynamicBlinkLedCommand(ledController, controlboard), + Commands.either( + new ClawOuttakeCommand(claw, controlboard) + .withTimeout(ScoringLevels.Consts.TELEOP_CUBE_SHOT_TIMEOUT_S) + .andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S)), + new ClawOuttakeCommand(claw, controlboard) + .withTimeout(ScoringLevels.Consts.TELEOP_CONE_SHOT_TIMEOUT_S) + .andThen(Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S)), + () -> controlboard.getHeldGamePiece().isCube() + ), + + Commands.deadline( + Commands.sequence( + claw.closeClawCommand(), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S * 2), + StowCommands.tele(claw, controlboard, elevator) + ), + Commands.sequence( + drivetrain.forceAllianceBasedFieldRelativeMovementCommand(0.5, 0.0, 0.5), + new TeleopSwerveCommand( + drivetrain, elevator, controlboard, DriveModes.FIELD_RELATIVE + ) + ) + ) + + ) + .beforeStarting(() -> { + controlboard.driverRumble(); + ledController.setOverride(true); + }) + .finallyDo((boolean interrupted) -> { + controlboard.driverResetRumble(); + ledController.setOverride(false); + ledController.turnOffLeds(LedSections.ALL); + }) + .unless(() -> { + return drivetrain.isRedAlliance() + ? (drivetrain.getPose().getX() <= Zones.LocationMath.COMMUNITY_ZONE_RED_MIN_X_M) + : (drivetrain.getPose().getX() >= Zones.LocationMath.COMMUNITY_ZONE_BLUE_MAX_X_M); + }); + } + + private static CommandBase prep(LedController ledController, Controlboard controlboard) { + return prep(ledController, controlboard, LedColors.YELLOW); + } + + private static CommandBase prep( + LedController ledController, Controlboard controlboard, LedColors color + ) { + return Commands.sequence(ledController.setSolidColorCommand(color), new InstantCommand(() -> { + if (controlboard.getHeldGamePiece().isUnknown()) + controlboard.setHeldGamePiece(GamePieceType.CONE_GAME_PIECE); + })); + } + + private static CommandBase confirmAndEject( + Claw claw, LedController ledController, Controlboard controlboard + ) { + return Commands.sequence( + Commands.waitUntil(() -> controlboard._xboxDrive.getLeftTriggerAxis() < 0.3), + Commands.parallel( + LedCommands.dynamicBlinkLedCommand(ledController, controlboard), + Commands.race( + new ClawOuttakeCommand(claw, controlboard), + Commands.either( + Commands.waitSeconds(ScoringLevels.Consts.TELEOP_CUBE_SHOT_TIMEOUT_S), + Commands.waitSeconds(ScoringLevels.Consts.TELEOP_CUBE_SHOT_TIMEOUT_S), + () -> controlboard.getHeldGamePiece().isCube() + ) + ) + ) + ); + } + + private static CommandBase driveAndSnap( + Drivetrain drivetrain, Elevator elevator, Controlboard controlboard + ) { + return new TeleopSwerveCommand( + drivetrain, + elevator, + controlboard, + DriveModes.SNAP_TO_ANGLE, + Constants.TRANSLATION_MAX_TRIM_SPEED_MPS, + Constants.ANGLE_MAX_TRIM_SPEED_DPS + ) + .beforeStarting(() -> drivetrain.setSnapScoringAngle()); + } + + private static CommandBase backoutAndStow( + Claw claw, Drivetrain drivetrain, Elevator elevator, Controlboard controlboard + ) { + return Commands + .deadline( + Commands.sequence( + Commands + .sequence( + claw.closeClawCommand(), + Commands.waitSeconds(ClawConfs.CLAW_DEPLOY_IN_TIME_S) + ) + .unless( + () -> controlboard.getDesiredScoringLevel() == ScoringLevels.SCORING_LEVEL_0 + ), + StowCommands.tele(claw, controlboard, elevator) + ), + Commands.sequence( + drivetrain.forceAllianceBasedFieldRelativeMovementCommand(1.0, 0.0, 0.3), + new TeleopSwerveCommand( + drivetrain, elevator, controlboard, DriveModes.FIELD_RELATIVE + ) + ) + ) + .finallyDo((boolean interrupted) -> { + if (!interrupted) { + controlboard.setHeldGamePiece(GamePieceType.UNKNOWN_GAME_PIECE); + } + }); + } +} diff --git a/src/main/java/frc/robot/commands/score/WireCoverScoreCommand.java b/src/main/java/frc/robot/commands/score/WireCoverScoreCommand.java new file mode 100644 index 0000000..d875800 --- /dev/null +++ b/src/main/java/frc/robot/commands/score/WireCoverScoreCommand.java @@ -0,0 +1,65 @@ +package frc.robot.commands.score; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.ClawRollersCommands; +import frc.robot.commands.base.ElevatorRaiseToCommand; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.enums.ClawDirections; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.ScoringLevels.Consts; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Elevator; + +public class WireCoverScoreCommand { + CommandBase score( + Claw claw, + Controlboard controlboard, + Elevator elevator, + double clawPower, + double elevatorThreshold + ) { + return Commands + .deadline( + Commands.sequence( + Commands.waitUntil(() -> elevator.getCurrentHeight() > elevatorThreshold), + claw.setClawPowerCommand(clawPower), + Commands.waitSeconds(Consts.AUTO_YEET_TIMEOUT_S) + ), + new ElevatorRaiseToCommand( + elevator, controlboard, ElevatorPositions.ELEVATOR_LEVEL_2_CONE + ) + ) + .finallyDo( + (boolean interrupted + ) -> elevator.setElevatorProfile(ElevatorPositions.ELEVATOR_STOW_TICKS) + ); + } + + CommandBase prepScore( + Claw claw, Controlboard controlboard, Elevator elevator, GamePieceType piece + ) { + return Commands.parallel( + ClawRollersCommands.conditionalCommand( + claw, + controlboard, + ClawConfs.CLAW_CUBE_INTAKE_RESETTLE, + ClawDirections.CLAW_ROLLERS_CUBE_IN, + ClawConfs.CLAW_CONE_INTAKE_RESETTLE, + ClawDirections.CLAW_ROLLERS_CONE_IN, + (() -> piece.isCube()) + ), + new ElevatorRaiseToCommand(elevator, controlboard, ElevatorPositions.ELEVATOR_STOW_TICKS), + claw.closeClawCommand() + ); + } + + CommandBase postScore(Claw claw, Controlboard controlboard, Elevator elevator) { + return Commands.parallel( + new ElevatorRaiseToCommand(elevator, controlboard, ElevatorPositions.ELEVATOR_STOW_TICKS), + new InstantCommand(() -> claw.setClawRollersPower(0.0)) + ); + } +} diff --git a/src/main/java/frc/robot/constants/ClawConfs.java b/src/main/java/frc/robot/constants/ClawConfs.java new file mode 100644 index 0000000..9b21c56 --- /dev/null +++ b/src/main/java/frc/robot/constants/ClawConfs.java @@ -0,0 +1,165 @@ +package frc.robot.constants; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import frc.robot.Ports.ClawPorts; + +public enum ClawConfs { + NO_CLAW(null, null), + // CLAW, + COMP_KLAW(ClawDims.EXTENDED_KLAW, ClawPorts.COMP_PORTS), + PRACTICE_KLAW(ClawDims.KLAW, ClawPorts.PRACTICE_PORTS), + // PNEUMATIC_CLAW(), + MOTOR_KLAW(ClawDims.EXTENDED_KLAW, ClawPorts.WRISTED_KLAW_PORTS); + + public final ClawDims dims; + public final ClawPorts ports; + + ClawConfs(ClawDims dims, ClawPorts ports) { + this.dims = dims; + this.ports = ports; + } + + public enum ClawDims { + // CLAW(), // no data + KLAW(15.0, 15.0), + EXTENDED_KLAW(24.0, 24.0); + + public final double openWidth_M, halfOpenWidth_M; + public final double closedWidth_M, halfClosedWidth_M; + + ClawDims(double open_IN, double closed_IN) { + openWidth_M = Units.inchesToMeters(open_IN); + halfOpenWidth_M = openWidth_M / 2.0; + closedWidth_M = Units.inchesToMeters(closed_IN); + halfClosedWidth_M = closedWidth_M / 2.0; + } + } + + public static final DoubleSolenoid.Value CLAW_SOLENOID_CLOSED = DoubleSolenoid.Value.kReverse; + public static final DoubleSolenoid.Value CLAW_SOLENOID_OPEN = DoubleSolenoid.Value.kForward; + public static final DoubleSolenoid.Value CLAW_SOLENOID_OFF = DoubleSolenoid.Value.kOff; + + public static final double CLAW_ROLLER_SPINUP_TIME_S = Units.millisecondsToSeconds(80); + public static final double CLAW_CUBE_SETTLE_TIME_S = Units.millisecondsToSeconds(20); + public static final double CLAW_CONE_SETTLE_TIME_S = Units.millisecondsToSeconds(60); + public static final double CLAW_CUBE_STALL_POWER = 0.15; + public static final double CLAW_CONE_STALL_POWER = 0.29; + public static final double CLAW_CUBE_AUTO_STALL_POWER = 0.3; + public static final double CLAW_CUBE_INTAKE_RESETTLE = 0.2; + public static final double CLAW_CONE_INTAKE_RESETTLE = 0.6; + public static final int CLAW_SHOT_DELAY_TICKS = 10; + public static final boolean PRIMARY_ROLLERS_INVERTED = false; + public static final boolean LEFT_ROLLERS_INVERTED = false; + public static final NeutralMode ROLLER_NEUTRAL_MODE = NeutralMode.Brake; + + public static final double CLAW_RELEASE_DELAY_S = 0.05; + public static final double CLAW_DEPLOY_IN_TIME_MS = 500; + public static final double CLAW_DEPLOY_IN_TIME_S = 0.5; + public static final double CLAW_DEPLOY_OUT_TIME_MS = 500; + public static final double CLAW_DEPLOY_OUT_TIME_S = 0.5; + + public static final double CLAW_SETTLING_WAIT_MS = 150; + public static final double CLAW_PLATFORM_CLEARANCE_WAIT_S = 0.8; + public static final double CLAW_OPEN_PLATFORM_PREP_DELAY_S = 0.5; + + /* Claw PID Values */ + public static final double CLAW_PFAC = 0.0; + public static final double CLAW_IFAC = 0.0; + public static final double CLAW_DFAC = 0.0; + public static final double CLAW_FFAC = 0.0; + + /* Claw Supply Current Limiting */ + public static final int CLAW_SUPPLY_CONTINUOUS_CURRENT_LIMIT = 40; + public static final int CLAW_SUPPLY_PEAK_CURRENT_LIMIT = 40; + public static final double CLAW_SUPPLY_PEAK_CURRENT_DURATION_S = 0.1; + public static final int CLAW_SUPPLY_PEAK_CURRENT_DURATION_MS = + (int) (1000.0 * CLAW_SUPPLY_PEAK_CURRENT_DURATION_S); + public static final boolean CLAW_SUPPLY_ENABLE_CURRENT_LIMIT = true; + + /* Claw Stator Current Limiting */ + public static final int CLAW_STATOR_CONTINUOUS_CURRENT_LIMIT = 40; + public static final int CLAW_STATOR_PEAK_CURRENT_LIMIT = 40; + public static final double CLAW_STATOR_PEAK_CURRENT_DURATION_S = 0.1; + public static final int CLAW_STATOR_PEAK_CURRENT_DURATION_MS = + (int) (1000.0 * CLAW_STATOR_PEAK_CURRENT_DURATION_S); + + public static final boolean CLAW_STATOR_ENABLE_CURRENT_LIMIT = true; + + // /* Claw Rollers PID Values */ + // public static final double CLAW_ROLLERS_PFAC = 0.0; + // public static final double CLAW_ROLLERS_IFAC = 0.0; + // public static final double CLAW_ROLLERS_DFAC = 0.0; + // public static final double CLAW_ROLLERS_FFAC = 0.0; + + /* Claw Rollers Supply Current Limiting */ + public static final int CLAW_ROLLERS_SUPPLY_CONTINUOUS_CURRENT_LIMIT = 40; + public static final int CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_LIMIT = 40; + public static final double CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_DURATION_S = 0.1; + public static final int CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_DURATION_MS = + (int) (1000.0 * CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_DURATION_S); + public static final boolean CLAW_ROLLERS_SUPPLY_ENABLE_CURRENT_LIMIT = true; + + /* Claw Rollers Stator Current Limiting */ + public static final int CLAW_ROLLERS_STATOR_CONTINUOUS_CURRENT_LIMIT = 40; + public static final int CLAW_ROLLERS_STATOR_PEAK_CURRENT_LIMIT = 40; + public static final double CLAW_ROLLERS_STATOR_PEAK_CURRENT_DURATION_S = 0.1; + public static final int CLAW_ROLLERS_STATOR_PEAK_CURRENT_DURATION_MS = + (int) (1000.0 * CLAW_ROLLERS_STATOR_PEAK_CURRENT_DURATION_S); + public static final boolean CLAW_ROLLERS_STATOR_ENABLE_CURRENT_LIMIT = true; + + public static final SupplyCurrentLimitConfiguration clawWristSupplyLimit = + new SupplyCurrentLimitConfiguration( + CLAW_SUPPLY_ENABLE_CURRENT_LIMIT, + CLAW_SUPPLY_CONTINUOUS_CURRENT_LIMIT, + CLAW_SUPPLY_PEAK_CURRENT_LIMIT, + CLAW_SUPPLY_PEAK_CURRENT_DURATION_S + ); + public static final StatorCurrentLimitConfiguration clawWristStatorLimit = + new StatorCurrentLimitConfiguration( + CLAW_STATOR_ENABLE_CURRENT_LIMIT, + CLAW_STATOR_CONTINUOUS_CURRENT_LIMIT, + CLAW_STATOR_PEAK_CURRENT_LIMIT, + CLAW_STATOR_PEAK_CURRENT_DURATION_S + ); + + public static final SupplyCurrentLimitConfiguration clawRollerSupplyLimit = + new SupplyCurrentLimitConfiguration( + CLAW_ROLLERS_SUPPLY_ENABLE_CURRENT_LIMIT, + CLAW_ROLLERS_SUPPLY_CONTINUOUS_CURRENT_LIMIT, + CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_LIMIT, + CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_DURATION_S + ); + + public static final StatorCurrentLimitConfiguration clawRollerStatorLimit = + new StatorCurrentLimitConfiguration( + CLAW_ROLLERS_STATOR_ENABLE_CURRENT_LIMIT, + CLAW_ROLLERS_STATOR_CONTINUOUS_CURRENT_LIMIT, + CLAW_ROLLERS_STATOR_PEAK_CURRENT_LIMIT, + CLAW_ROLLERS_STATOR_PEAK_CURRENT_DURATION_S + ); + + public static final double CLAW_OPEN_LOOP_RAMP = 0.0; + public static final double CLAW_CLOSED_LOOP_RAMP = 0.0; + + public static final class IntakeSpeed { + public static double CUBE_POWER; + public static double CONE_POWER; + public static double CONE_DETECTION_CURRENT_THRESHOLD_A; + + static { + if (RobotAltModes.isTestMode) { + CONE_DETECTION_CURRENT_THRESHOLD_A = 23.0; + CUBE_POWER = 0.3 * RobotAltModes.TEST_MODE_COEFFICIENT; + CONE_POWER = 0.6 * RobotAltModes.TEST_MODE_COEFFICIENT; + } else { + CONE_DETECTION_CURRENT_THRESHOLD_A = 23.0; + CONE_POWER = 0.84; + CUBE_POWER = 0.575; + } + } + } +} diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java new file mode 100644 index 0000000..6d67091 --- /dev/null +++ b/src/main/java/frc/robot/constants/Constants.java @@ -0,0 +1,170 @@ +package frc.robot.constants; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +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.util.Units; +import frc.robot.constants.enums.FieldVersions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.constants.enums.ModuleModels; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; + +public class Constants { + public static final int NUM_SCORING_LOCATIONS = 9; + + public static final RobotVersions CRobot = RobotVersions.CompBot; + public static final FieldVersions CField = FieldVersions.THEORETICAL_FIELD; + + // if we do our configurations right, this shouldn't be necessary + public static final boolean isCompBot = CRobot == RobotVersions.CompBot; + public static final boolean isPracticeBot = CRobot == RobotVersions.PracticeBot; + public static final boolean isSwerveBase = CRobot == RobotVersions.SwerveBase; + + public static final boolean isNASAField = CField == FieldVersions.NASA_FIELD; + public static final boolean isCurie = CField == FieldVersions.CURIE_FIELD; + public static final boolean isSVR = CField == FieldVersions.SVR_FIELD; + public static final boolean isArizona = CField == FieldVersions.ARIZONA_FIELD; + public static final boolean isCanada = CField == FieldVersions.CANADA_FIELD; + public static final boolean isSDSMK4 = CRobot.drive.type == ModuleModels.ModuleTypes.SDS_MK4; + public static final boolean isSDSMK4I = CRobot.drive.type == ModuleModels.ModuleTypes.SDS_MK4I; + public static final boolean isCANEncoder = CRobot == RobotVersions.SwerveBase; + + // These are high level robot configurations that fundamentally change robot behavior and + // control schemes, this also allows for manual overrides. I don't like this, would love to see + // alternatives... These should really become part of the top level RobotVersions + public static final boolean hasClaw = CRobot.claw != null; + public static final boolean isKlaw = + CRobot.claw == ClawConfs.COMP_KLAW || CRobot.claw == ClawConfs.PRACTICE_KLAW; + public static final boolean isPneumaticClaw = + isKlaw; // || CRobot.claw == ClawConfs.PNEUMATIC_CLAW; + public static final boolean hasLed = CRobot.led != null; + public static final boolean hasElevator = CRobot.elevator != null; + public static final boolean hasVision = CRobot.vision != null; + + /// need to move + // consts + /* ========== ROBOT OFFSET DIMENSIONS ========== */ + // TODO tune and test this value (location of robot when claw is against back of + // loading station + // when extended) + + public static final double LOADING_POSE_OFFSET_M = Units.inchesToMeters(27); + // Value when preparing should start + public static final double PLATFORM_PREP_DIST_M = + Units.inchesToMeters(35); // TODO update distance + public static final double PLATFORM_INTAKE_CONE_OFFSET_M = + Units.inchesToMeters(0); // TODO update distance back + // from cube (basically an + // offset relative to the + // cube + // pose) + // distance back driven when stowing elevator + + // TODO update distance + public static final double PLATFORM_BACKOUT_DIST_M = Units.feetToMeters(6); + + public static final double LOOP_PERIOD_MS = 20.0; + public static final double LOOP_PERIOD_S = Units.millisecondsToSeconds(LOOP_PERIOD_MS); + + // defaults + public static final ScoringLevels DEFAULT_SCORING_LEVEL = ScoringLevels.SCORING_LEVEL_2; + public static final ScoringLocations DEFAULT_SCORING_LOCATION = + ScoringLocations.SCORING_LOCATION_9; + public static final HPIntakeStations DEFAULT_INTAKE_STATION = + HPIntakeStations.DOUBLE_STATION_INNER; + public static final GamePieceType DEFAULT_GAME_PIECE = GamePieceType.CONE_GAME_PIECE; + + public static final class CTREConstants { + /* ============= Falcon Constants2 ============= */ + // ticks per motor rotation + public static final double FALCON_ENCODER_TICKS = 2048.0; + // TODO: measure this free speed on blocks + // 6380 +/- 10% + public static final double MAX_FALCON_RPM = 5800.0; + + public static final double CANCODER_ENCODER_TICKS = 4096.0; + + // multiply to convert Constants2 + public static final double FALCON_TO_RPS = 10.0 / FALCON_ENCODER_TICKS; + public static final double FALCON_TO_RPM = 60.0 * FALCON_TO_RPS; + public static final double CANCODER_TO_RPS = 10.0 / CANCODER_ENCODER_TICKS; + public static final double CANCODER_TO_RPM = 60.0 * CANCODER_TO_RPS; + public static final double RPS_TO_FALCON = 1.0 / FALCON_TO_RPS; + public static final double RPM_TO_FALCON = 1.0 / FALCON_TO_RPM; + public static final double RPS_TO_CANCODER = 1.0 / CANCODER_TO_RPS; + public static final double RPM_TO_CANCODER = 1.0 / CANCODER_TO_RPM; + public static final double FALCON_TO_DEG = 360.0 / FALCON_ENCODER_TICKS; + public static final double DEG_TO_FALCON = FALCON_ENCODER_TICKS / 360.0; + public static final double DEG_TO_CANCODER = CANCODER_ENCODER_TICKS / 360.0; + public static final double FALCON_TO_RAD = FALCON_TO_DEG * Math.PI / 180.0; + public static final double RAD_TO_FALCON = 180.0 / (FALCON_TO_DEG * Math.PI); + public static final double FALCON_TICKS_TO_ROT = 1.0 / FALCON_ENCODER_TICKS; + public static final double ROT_TO_FALCON_TICKS = FALCON_ENCODER_TICKS; + } + + public static final class Sensors { + public static final boolean INVERT_GYRO = false; + + public static final Rotation2d GYRO_ZERO_BLUE = Rotation2d.fromDegrees(0); + public static final Rotation2d GYRO_ZERO_RED = GYRO_ZERO_BLUE.plus(Rotation2d.fromDegrees(180)); + + public static final double POV_ZERO_BLUE_DEG = 0; + public static final double POV_ZERO_RED_DEG = POV_ZERO_BLUE_DEG + 180; + } + + /* For Path Planner */ + // TODO tune for trapezoidal control + public static final double AUTO_MAX_SPEED_MPS = 3.4; + public static final double AUTO_XY_TRANSLATION_MAX_A_MPS_SQ = 1.8; + public static final double CHARGE_MAX_SPEED_MPS = 2.0; + public static final double CHARGE_XY_TRANSLATION_MAX_A_MPS_SQ = 2.0; + public static final double CHARGE_TRAVERSE_MAX_SPEED_MPS = 1.4; + public static final double CHARGE_TRAVERSE_XY_TRANSLATION_MAX_A_MPS_SQ = 1.6; + + /* For Drivetrain Auto */ + public static final double THETA_AUTO_SCORE_TOLERANCE_DEG = 45; + + public static final double FORCED_WHEEL_ALIGNMENT_SPEED_MPS = 1; + + public static final double TRANSLATION_MAX_TRIM_SPEED_MPS = 1; + public static final Rotation2d ANGLE_MAX_TRIM_SPEED_DPS = Rotation2d.fromDegrees(90.0); + + public static final double SWERVE_SKEW_COMPENSATION_COEFFICIENT = 0.0; + + /* ========== SCORING PREALIGN OFFSET VALUES ========== */ + public static final double SCORING_PREALIGN_RED_OFFSET_X_M = Units.inchesToMeters(-6); // -12 + public static final double SCORING_PREALIGN_RED_OFFSET_Y_M = Units.inchesToMeters(0); + public static final double SCORING_PREALIGN_RED_OFFSET_THETA_DEG = 0; // degree + + /* ========== SCORING PREALIGN OFFSET POSES ========== */ + public static final Translation2d SCORING_PREALIGN_RED_OFFSET_XY = + new Translation2d(SCORING_PREALIGN_RED_OFFSET_X_M, SCORING_PREALIGN_RED_OFFSET_Y_M); + public static final Transform2d SCORING_PREALIGN_RED_OFFSET = new Transform2d( + SCORING_PREALIGN_RED_OFFSET_XY, Rotation2d.fromDegrees(SCORING_PREALIGN_RED_OFFSET_THETA_DEG) + ); + + /* ========== POST SCORING OFFSET VALUES ========== */ + public static final double POST_SCORING_RED_OFFSET_X_M = Units.inchesToMeters(-12); + public static final double POST_SCORING_RED_OFFSET_Y_M = Units.inchesToMeters(0); + public static final double POST_SCORING_RED_OFFSET_THETA_DEG = 0; // degree + + /* ========== POST SCORING OFFSET POSES ========== */ + public static final Translation2d POST_SCORING_RED_OFFSET_XY = + new Translation2d(POST_SCORING_RED_OFFSET_X_M, POST_SCORING_RED_OFFSET_Y_M); + public static final Transform2d POST_SCORING_RED_OFFSET = new Transform2d( + POST_SCORING_RED_OFFSET_XY, Rotation2d.fromDegrees(POST_SCORING_RED_OFFSET_THETA_DEG) + ); + + // Timeout + public static final double ALIGN_SHOT_TIMEOUT_S = 2; // second + + public static final class Control { + public static final double STICK_DEADBAND = 0.1; // TODO tune, too low + public static final double STICK_NET_DEADBAND = 0.125; // TODO tune, too low + + public static final boolean IS_OPEN_LOOP = false; // swerve + } +} diff --git a/src/main/java/frc/robot/constants/DrivetrainConfs.java b/src/main/java/frc/robot/constants/DrivetrainConfs.java new file mode 100644 index 0000000..cc1a25d --- /dev/null +++ b/src/main/java/frc/robot/constants/DrivetrainConfs.java @@ -0,0 +1,102 @@ +package frc.robot.constants; + +import edu.wpi.first.math.util.Units; +import frc.robot.Ports.DrivetrainPorts; +import frc.robot.constants.enums.*; + +public enum DrivetrainConfs { + COMP_BOT_CONFS( + DrivetrainDimensions.COMP_BOT_DIMENSIONS, + DrivetrainPorts.COMP_PORTS, + DrivetrainControl.COMP_CONTROL, + 4, + ModuleModels.SDS_MK4I_L2, + ModuleControl.FALCON_MK4I_FALCON_L2, + new double[] {360.0 - 186.38, 360.0 - 91.41, 360.0 - 199.27, 360.0 - 52.54} + ), + PRACTICE_BOT_CONFS( + DrivetrainDimensions.PRACTICE_BOT_DIMENSIONS, + DrivetrainPorts.PRACTICE_PORTS, + DrivetrainControl.PRACTICE_CONTROL, + 4, + ModuleModels.SDS_MK4I_L2, + ModuleControl.FALCON_MK4I_FALCON_L2, + new double[] { + 359.64, + 208.98, + 291.9, + 77.42, + } + ), + SWERVE_BASE_CONFS( + DrivetrainDimensions.SWERVE_BASE_DIMENSIONS, + DrivetrainPorts.SWERVE_BASE_PORTS, + DrivetrainControl.SWERVE_BASE_CONTROL, + 4, + ModuleModels.SDS_MK4_L2, + ModuleControl.FALCON_MK4_FALCON_L2, + new double[] {360.0 - 52.9, 360.0 - 7.5, 360.0 - 21.8, 360.0 - 349.0} + // new double[] {11.4, 338.1, 212.4, 81.5} + ); + + // Drivetrain Configurations + public final DrivetrainDimensions dims; + public final DrivetrainPorts ports; + public final DrivetrainControl control; + + /* Module Configurations */ + public final ModuleModels model; + public final ModuleControl moduleControl; + public final int numModules; + public final double[] moduleOffsetsDeg; + public final ModuleModels.ModuleTypes type; + public final ModuleModels.ModuleDriveRatios ratio; + public final double alignmentToleranceDeg; + public final double theoreticalMaxTranslationSpeed; + public final double theoreticalMaxRotationalSpeed; + + DrivetrainConfs( + DrivetrainDimensions dims, + DrivetrainPorts ports, + DrivetrainControl control, + int numModules, + ModuleModels model, + ModuleControl moduleControl, + double[] offsets + ) { + this(dims, ports, control, numModules, model, moduleControl, offsets, 5.0); + } + + DrivetrainConfs( + DrivetrainDimensions dims, + DrivetrainPorts ports, + DrivetrainControl control, + int numModules, + ModuleModels model, + ModuleControl moduleControl, + double[] offsets, + double alignmentToleranceDeg + ) { + assert (numModules == ports.drive.length); + assert (numModules == ports.steer.length); + assert (numModules == ports.encoder.length); + + this.dims = dims; + this.ports = ports; + this.control = control; + this.numModules = numModules; + this.model = model; + this.moduleControl = moduleControl; + this.type = model.type; + this.ratio = model.ratio; + this.alignmentToleranceDeg = alignmentToleranceDeg; + + assert (offsets.length == numModules); + moduleOffsetsDeg = offsets; + + this.theoreticalMaxTranslationSpeed = model.theoreticalMaxWheelSpeed; + + double location_m = Math.hypot(dims.halfTrackLength_M, dims.halfTrackWidth_M); + this.theoreticalMaxRotationalSpeed = 2 * Math.PI * location_m / model.theoreticalMaxWheelSpeed; + }; +} diff --git a/src/main/java/frc/robot/constants/ElevatorConfs.java b/src/main/java/frc/robot/constants/ElevatorConfs.java new file mode 100644 index 0000000..f45dc57 --- /dev/null +++ b/src/main/java/frc/robot/constants/ElevatorConfs.java @@ -0,0 +1,98 @@ +package frc.robot.constants; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import edu.wpi.first.math.util.Units; +import frc.robot.Ports.ElevatorPorts; + +public enum ElevatorConfs { + COMP_BOT_CONF(ElevatorPorts.COMP_PORTS, ElevatorDims.COMP_ELEVATOR, ElevatorControl.COMP_BOT), + PRACTICE_BOT_CONF( + ElevatorPorts.PRACTICE_PORTS, ElevatorDims.PRACTICE_ELEVATOR, ElevatorControl.PRACTICE_BOT + ); + + public final ElevatorPorts ports; + public final ElevatorDims dims; + public final ElevatorControl control; + + ElevatorConfs(ElevatorPorts ports, ElevatorDims dims, ElevatorControl control) { + this.ports = ports; + this.dims = dims; + this.control = control; + } + + public enum ElevatorDims { + COMP_ELEVATOR(), + PRACTICE_ELEVATOR(); + + public static final int ELEVATOR_INCLINE_ANGLE_DEG = 50; + // 1.114" pitch diameter + public static final double ELEVATOR_DRUM_RADIUS_M = Units.inchesToMeters(0.6365); + public static final double ELEVATOR_CIRCUMFERENCE_M = ELEVATOR_DRUM_RADIUS_M * 2 * Math.PI; + // TODO ALL + // 12:60 gear ratio on main stage + public static final double ELEVATOR_GEAR_RATIO = 5.0; + // 7 lb claw mechanism, 15-20 lbs + public static final double ELEVATOR_WEIGHT_LB = 7.2; + public static final double ELEVATOR_MIN_HEIGHT_M = 0; + public static final double ELEVATOR_MAX_HEIGHT_M = Units.inchesToMeters(35.0); + } + + public enum ElevatorControl { // values after elevator tuning + COMP_BOT(new double[] {0.355, 6.67, 0.05, 0.625}, new double[] {1.0, 2.0}), + PRACTICE_BOT(new double[] {0.0, 6.5, 0.0, 0.4}, new double[] {2.0, 2.0}); + + /* Elevator Characterization Values */ + public final double ELEVATOR_KS_V; + public final double ELEVATOR_KV_V_PER_MPS; + public final double ELEVATOR_KA_V_PER_MPSQ; + public final double ELEVATOR_KG_V; + + /* Elevator Trapezoidal Profile Limits */ + public final double ELEVATOR_MAX_VELO_MPS; + public final double ELEVATOR_MAX_ACC_MPSQ; + + /* Elevator PID Configuration */ + public static final double ELEVATOR_PFAC = 0.15; + public static final double ELEVATOR_IFAC = 0.0; + public static final double ELEVATOR_DFAC = 0.4; + public static final double ELEVATOR_FFAC = 0.0; + + public static final double ELEVATOR_OPEN_LOOP_RAMP = 0.0; + public static final double ELEVATOR_CLOSED_LOOP_RAMP = 0.0; + public static final double ELEVATOR_VOLTAGE_COMP_SATURATION = 12.0; + + ElevatorControl(double[] characterization, double[] trapezoidalLimits) { + ELEVATOR_KS_V = characterization[0]; + ELEVATOR_KV_V_PER_MPS = characterization[1]; + ELEVATOR_KA_V_PER_MPSQ = characterization[2]; + ELEVATOR_KG_V = characterization[3]; + + ELEVATOR_MAX_VELO_MPS = trapezoidalLimits[0]; + ELEVATOR_MAX_ACC_MPSQ = trapezoidalLimits[1]; + } + } + + public static final int ELEVATOR_CLEAR_OF_INTAKE_TICKS = 4; + // TODO Tune.... + public static final int ELEVATOR_TOLERANCE_TICKS = 300; + + public static final boolean ELEVATOR_MOTOR_INVERTED = true; + public static final boolean ELEVATOR_FOLLOWER_MOTOR_INVERTED = !ELEVATOR_MOTOR_INVERTED; + public static final NeutralMode ELEVATOR_NEUTRAL_MODE = NeutralMode.Coast; + public static final NeutralMode ELEVATOR_DISABLED_MODE = NeutralMode.Brake; + + public static final boolean ELEVATOR_SUPPLY_ENABLE_CURRENT_LIMIT = true; + public static final int ELEVATOR_SUPPLY_CONTINUOUS_CURRENT_LIMIT = 60; + public static final int ELEVATOR_SUPPLY_PEAK_CURRENT_LIMIT = 60; + public static final double ELEVATOR_SUPPLY_PEAK_CURRENT_DURATION = 0.1; + + public static final boolean ELEVATOR_STATOR_ENABLE_CURRENT_LIMIT = true; + public static final int ELEVATOR_STATOR_CONTINUOUS_CURRENT_LIMIT = 60; + public static final int ELEVATOR_STATOR_PEAK_CURRENT_LIMIT = 60; + public static final double ELEVATOR_STATOR_PEAK_CURRENT_DURATION = 0.1; + public static final double ELEVATOR_MOTOR_DEADBAND = 0.001; + + public static final int ELEVATOR_ZEROING_STALL_TOLERANCE = 50; + public static final int ELEVATOR_ZEROING_STALL_LOOPS = 6; + public static final double ELEVATOR_ZEROING_SPEED_MPS = -0.06; +} diff --git a/src/main/java/frc/robot/constants/LedConfs.java b/src/main/java/frc/robot/constants/LedConfs.java new file mode 100644 index 0000000..3b09eb0 --- /dev/null +++ b/src/main/java/frc/robot/constants/LedConfs.java @@ -0,0 +1,119 @@ +package frc.robot.constants; + +import frc.robot.Ports.LedPorts; +import frc.robot.constants.enums.LedColors; + +public enum LedConfs { + COMP_BOT_CONF(LedPorts.COMP_PORTS), + PRACTICE_BOT_CONF(LedPorts.PRACTICE_PORTS), + SWERVE_BASE_CONF(LedPorts.SWERVE_BASE_PORTS), + TEST_CONF(null); + + public final LedPorts ports; + + LedConfs(LedPorts ports) { + this.ports = ports; + } + + public static final int CANDLE_LENGTH = 8; + public static final int LED_LENGTH = 46; // 38; // 68 + public static final int STRIP_LENGTH = 14; + public static final int INDICATOR_LENGTH = 10; + public static final boolean ANIMATION_DIRECTION = false; + + public static final int SUBLEVEL_INCREMENT = 4; + public static final int INDICATOR_INCREMENT = 3; + + public static final int STRIP_1_START = CANDLE_LENGTH; + public static final int STRIP_2_START = CANDLE_LENGTH + STRIP_LENGTH + INDICATOR_LENGTH; + public static final int INDICATOR_START = CANDLE_LENGTH + STRIP_LENGTH; + + public static final int STRIP_HALF_1_START = CANDLE_LENGTH; + public static final int STRIP_HALF_1_END = STRIP_HALF_1_START + STRIP_LENGTH / 2; + + public static final int STRIP_HALF_2_START = CANDLE_LENGTH + STRIP_LENGTH / 2; + public static final int STRIP_HALF_2_END = LED_LENGTH; + + public enum ScoringSections { + BOTTOM(LedColors.PURPLE), + MIDDLE(LedColors.BLUE), + TOP(LedColors.GREEN), + UNKNOWN_SCORING_SECTION(LedColors.OFF); + + public final LedColors sectionColor; + ScoringSections(LedColors sectionColor) { + this.sectionColor = sectionColor; + } + } + + public enum ScoringSubsections { + SUB_BOTTOM(LedColors.PINK), + SUB_MIDDLE(LedColors.YELLOW), + SUB_TOP(LedColors.RED_ORANGE), + UNKNOWN_SCORING_SUBSECTION(LedColors.OFF); + + public final LedColors subsectionColor; + ScoringSubsections(LedColors subsectionColor) { + this.subsectionColor = subsectionColor; + } + } + + public enum LedStates { UNKNOWN_STATE, SCORING_DESIRED, INTAKING_DESIRED } + + public enum LedSections { + ALL(0, LED_LENGTH), + CANDLE(0, CANDLE_LENGTH), + NON_CANDLE(CANDLE_LENGTH, LED_LENGTH), + INDICATOR(INDICATOR_START, INDICATOR_START + INDICATOR_LENGTH), + NON_INDICATOR(CANDLE_LENGTH + INDICATOR_LENGTH, LED_LENGTH), + STRIP(CANDLE_LENGTH, CANDLE_LENGTH + STRIP_LENGTH), + STRIP_2(STRIP_2_START, STRIP_2_START + STRIP_LENGTH), + STRIP_THIRD_1(STRIP_1_START, STRIP_1_START + SUBLEVEL_INCREMENT), + STRIP_THIRD_2(STRIP_1_START + SUBLEVEL_INCREMENT, STRIP_1_START + SUBLEVEL_INCREMENT * 2), + STRIP_THIRD_3(STRIP_1_START + SUBLEVEL_INCREMENT * 2, STRIP_1_START + STRIP_LENGTH), + STRIP_2_THIRD_1(STRIP_2_START, STRIP_2_START + SUBLEVEL_INCREMENT), + STRIP_2_THIRD_2(STRIP_2_START + SUBLEVEL_INCREMENT, STRIP_2_START + SUBLEVEL_INCREMENT * 2), + STRIP_2_THIRD_3(STRIP_2_START + SUBLEVEL_INCREMENT * 2, STRIP_2_START + STRIP_LENGTH), + STRIP_HALF_1(STRIP_HALF_1_START, STRIP_HALF_1_END), + STRIP_HALF_2(STRIP_HALF_2_START, STRIP_HALF_2_END), + STRIP_2_HALF_1(STRIP_2_START + STRIP_LENGTH / 2, STRIP_2_START + STRIP_LENGTH), + STRIP_2_HALF_2(STRIP_2_START, STRIP_2_START + STRIP_LENGTH / 2), + INDICATOR_HALF_1(INDICATOR_START, INDICATOR_START + INDICATOR_LENGTH / 2), + INDICATOR_HALF_2(INDICATOR_START + INDICATOR_LENGTH / 2, INDICATOR_START + INDICATOR_LENGTH), + INDICATOR_THIRD_1(INDICATOR_START, INDICATOR_START + INDICATOR_INCREMENT), + INDICATOR_THIRD_2( + INDICATOR_START + INDICATOR_INCREMENT, INDICATOR_START + 2 * INDICATOR_INCREMENT + ), + INDICATOR_THIRD_3( + INDICATOR_START + INDICATOR_INCREMENT * 2, INDICATOR_START + INDICATOR_LENGTH + ); + + public final int start; + public final int end; + public final int length; + + LedSections(int start, int end) { + this.start = start; + this.end = end; + this.length = end - start; + } + } + + public static final int LED_WHITE_LEVEL = 50; + + public static final double ROLL_1_LOWER_DEG = 45; + public static final double ROLL_1_UPPER_DEG = 135; + + public static final double ROLL_2_LOWER_DEG = 225; + public static final double ROLL_2_UPPER_DEG = 315; + + public static final double ORIENTATION_BLUE_LOWER_DEG = 135; + public static final double ORIENTATION_BLUE_UPPER_DEG = 315; + public static final double ORIENTATION_RED_LOWER_DEG = ORIENTATION_BLUE_UPPER_DEG; + public static final double ORIENTATION_RED_UPPER_DEG = ORIENTATION_BLUE_LOWER_DEG; + + public static final double LEVEL_THRESHOLD = 2.5; + + // public static final double LEVEL_TIME_S = 5; + // public static final double RESET_TIME_MS = 0.0001; +} diff --git a/src/main/java/frc/robot/constants/RobotAltModes.java b/src/main/java/frc/robot/constants/RobotAltModes.java new file mode 100644 index 0000000..b863ba5 --- /dev/null +++ b/src/main/java/frc/robot/constants/RobotAltModes.java @@ -0,0 +1,25 @@ +package frc.robot.constants; + +import frc.robot.Robot; + +public final class RobotAltModes { + /* --- Test Mode Configurations --- */ + public static final boolean isTestMode = false; + public static final double TEST_MODE_COEFFICIENT = 0.5; + + /* --- Debug Modes */ + public static final boolean isVerboseMode = false; + public static final boolean isLoopTiming = false; + public static final boolean isAutoTuning = false; + public static final boolean isPoseTuning = false; + public static final boolean isPIDTuningMode = false; + public static final boolean isUnprofiledPIDMode = false; + public static final boolean isElevatorTuningMode = false; + + /* --- Sim Actionability --- */ + public static final boolean isSim = Robot.isSimulation(); + public static final boolean isSimElevator = false; + + // TODO: Move to current robot configurations + public static final boolean isVisionMode = false; +} diff --git a/src/main/java/frc/robot/constants/RobotVersions.java b/src/main/java/frc/robot/constants/RobotVersions.java new file mode 100644 index 0000000..1401ce6 --- /dev/null +++ b/src/main/java/frc/robot/constants/RobotVersions.java @@ -0,0 +1,52 @@ +package frc.robot.constants; + +import frc.robot.Ports.PneumaticPorts; + +public enum RobotVersions { + CompBot( + DrivetrainConfs.COMP_BOT_CONFS, + ClawConfs.COMP_KLAW, + ElevatorConfs.COMP_BOT_CONF, + VisionConfs.COMP_BOT_CONF, + LedConfs.COMP_BOT_CONF, + PneumaticPorts.COMP_PORTS + ), + PracticeBot( + DrivetrainConfs.PRACTICE_BOT_CONFS, + null, + null, + // ClawConfs.PRACTICE_KLAW, + // ElevatorConfs.PRACTICE_BOT_CONF, + VisionConfs.PRACTICE_BOT_CONF, + // LedConfs.PRACTICE_BOT_CONF, + null, + PneumaticPorts.PRACTICE_PORTS + ), + SwerveBase( + DrivetrainConfs.SWERVE_BASE_CONFS, null, null, VisionConfs.SWERVE_BASE_CONF, null, null + ), + TestBoard(null, null, null, null, null, null); + + public final DrivetrainConfs drive; + public final ClawConfs claw; + public final ElevatorConfs elevator; + public final VisionConfs vision; + public final LedConfs led; + public final PneumaticPorts air; + + RobotVersions( + DrivetrainConfs drive, + ClawConfs claw, + ElevatorConfs elevator, + VisionConfs vision, + LedConfs led, + PneumaticPorts air + ) { + this.drive = drive; + this.claw = claw; + this.elevator = elevator; + this.vision = vision; + this.led = led; + this.air = air; + } +} diff --git a/src/main/java/frc/robot/constants/VisionConfs.java b/src/main/java/frc/robot/constants/VisionConfs.java new file mode 100644 index 0000000..0339296 --- /dev/null +++ b/src/main/java/frc/robot/constants/VisionConfs.java @@ -0,0 +1,81 @@ +package frc.robot.constants; + +import edu.wpi.first.math.Vector; +import frc.robot.constants.enums.CameraConfigs; +import frc.robot.constants.enums.CameraSets; +import java.util.Arrays; + +public enum VisionConfs { + // Concept: camera configurations stay the same but sometimes we want to change the set of cameras + // that are actually initialized and interacted with. During a season we will change teh CameraSet + // as needed on they fly + // COMP_BOT_CONF(CameraSets.CAMERA_0, new CameraConfigs[] {CameraConfigs.COMP_LEFT_CAMERA}), + COMP_BOT_CONF( + CameraSets.BOTH_CAMERAS, + new CameraConfigs[] {CameraConfigs.COMP_LEFT_CAMERA, CameraConfigs.COMP_RIGHT_CAMERA} + ), + PRACTICE_BOT_CONF(CameraSets.CAMERA_0, new CameraConfigs[] {CameraConfigs.PRACTICE_LEFT_CAMERA}), + SWERVE_BASE_CONF(CameraSets.CAMERA_0, new CameraConfigs[] {CameraConfigs.DRIVE_BASE_CAMERA}), + TEST_CONF(CameraSets.NO_CAMERAS, new CameraConfigs[0]); + + public final CameraSets setup; + public final CameraConfigs[] cameras = new CameraConfigs[MAX_NUM_CAMERAS]; + + VisionConfs(CameraSets setup, CameraConfigs[] cameras) { + this.setup = setup; + + // Ensures the id set of cameras are valid + assert (setup.ids.length <= cameras.length); + assert (setup.ids.length <= MAX_NUM_CAMERAS); + + // Read in ids and cameras to their proper id index + int[] ids = new int[cameras.length]; + for (int i = 0; i < cameras.length; i++) { + assert (cameras[i].id < MAX_NUM_CAMERAS); + ids[i] = cameras[i].id; + this.cameras[ids[i]] = cameras[i]; + + // Ensure camera ids don't overlap with previous ids + for (int j = 0; j < i; j++) { + assert (ids[j] != ids[i]); + } + } + + // Ensure each camera id in the Camera set is represented + // (allows us to switch camera sets but not change camera config arrays on the fly) + Arrays.sort(ids); + for (int i = 0; i < setup.ids.length; i++) { + assert (Arrays.binarySearch(ids, setup.ids[i]) != 0); + } + } + + public CameraConfigs getCameraConfig(int id) { + assert (cameras[id] != null); + return cameras[id]; + } + + public static final int MAX_NUM_CAMERAS = 2; + + public static final int DRIVE_PIPELINE_INDEX = 5; + + public static final int PIPELINE_INDEX_3D = 0; + public static final int PIPELINE_INDEX_2D = 1; + + public static final double AMBIGUITY_SCALE = 160.0; + public static final double AMBIGUITY_CONSTANT = 0.2; + + public static final double X_DISTANCE_SCALE = 0.0153; // 0.0000447 * 3; + public static final double X_DISTANCE_POW = 0.257; // 1.81; + + public static final double Y_DISTANCE_SCALE = 0.0372; // 0.000271 * 3; + public static final double Y_DISTANCE_POW = 0.371; // 1.99; + + public static final double THETA_SCALE = 95.2; + public static final double THETA_CONSTANT = 56.4; + + public static final double DEFAULT_X_DEV = 0.06; + public static final double DEFAULT_Y_DEV = 0.24; + public static final double DEFAULT_THETA_DEV = 180; + + public static final double SINGLE_TAG_MAX_DIST_M = 2; +} diff --git a/src/main/java/frc/robot/constants/enums/AutoCommand.java b/src/main/java/frc/robot/constants/enums/AutoCommand.java new file mode 100644 index 0000000..389f67f --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/AutoCommand.java @@ -0,0 +1,28 @@ +package frc.robot.constants.enums; + +public enum AutoCommand { + AUTO_PREP_GROUND_INTAKE, + AUTO_PREP_GROUND_INTAKE_CUBE, + AUTO_PREP_GROUND_INTAKE_CONE, + AUTO_GROUND_INTAKE, + AUTO_GROUND_INTAKE_CUBE, + AUTO_GROUND_INTAKE_CONE, + AUTO_POST_GROUND_INTAKE, + AUTO_PREP_SCORE, + AUTO_PREP_SCORE_CONE_2, + AUTO_PREP_SCORE_CONE_0, + AUTO_PREP_SCORE_CUBE_2, + AUTO_PREP_SCORE_CUBE_1, + AUTO_PREP_SCORE_CUBE_0, + AUTO_SCORE_CUBE_0, + AUTO_SCORE_CUBE_1, + AUTO_SCORE_CUBE_2, + AUTO_SCORE_CONE_2, + AUTO_POST_SCORE, + FORCE_CUBE_SHOT, + FORCE_CONE_SHOT, + AUTO_POST_GROUND_INTAKE_CONE, + AUTO_WIRE_SCORE, + AUTO_WIRE_PREP_SCORE, + AUTO_WIRE_POST_SCORE +} diff --git a/src/main/java/frc/robot/constants/enums/AutonomousAction.java b/src/main/java/frc/robot/constants/enums/AutonomousAction.java new file mode 100644 index 0000000..97b9b75 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/AutonomousAction.java @@ -0,0 +1,66 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.subsystems.LedController; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +public class AutonomousAction { + private final String name; + private final CommandBase underlying; + + private CommandBase command; + + private static final List actionList = new ArrayList(); + private static final Map eventMap = new HashMap(); + + AutonomousAction(String name, CommandBase underlying) { + this.name = name; + this.underlying = underlying; + } + + private static void register(AutonomousAction action) { + actionList.add(action); + eventMap.put(action.name, action.command); + } + + public static void registerAutonomousAction(String name, double waitSeconds) { + AutonomousAction action = new AutonomousAction(name, new WaitCommand(waitSeconds)); + action.command = action.underlying; + register(action); + } + + public static void registerAutonomousAction( + String name, LedController controller, LedColors color + ) { + AutonomousAction action = new AutonomousAction( + name, new InstantCommand(() -> controller.setSolidColor(color, LedSections.ALL)) + ); + action.command = action.underlying; + register(action); + } + + public static void registerAutonomousAction(String name, String message) { + AutonomousAction action = new AutonomousAction(name, new PrintCommand(message)); + // unclear if we can just skip this step or not + action.command = new InstantCommand(() -> action.underlying.schedule()); + register(action); + } + + public static void registerAutonomousAction(String name, CommandBase command) { + AutonomousAction action = new AutonomousAction(name, command); + action.command = new InstantCommand(() -> action.underlying.schedule()); + register(action); + } + + public static Map getEventMap() { + return eventMap; + } +} diff --git a/src/main/java/frc/robot/constants/enums/AutonomousRoutines.java b/src/main/java/frc/robot/constants/enums/AutonomousRoutines.java new file mode 100644 index 0000000..36abb77 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/AutonomousRoutines.java @@ -0,0 +1,802 @@ +package frc.robot.constants.enums; + +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; +import com.pathplanner.lib.auto.SwerveAutoBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +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.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.AutonomousScoreCommands; +import frc.robot.commands.LedCommands; +import frc.robot.commands.base.ChargingStationBalanceCommand; +import frc.robot.constants.Constants; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Controlboard; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.LedController; +import frc.robot.utils.PIDConstants; +import java.util.Map; +import java.util.Vector; + +public enum AutonomousRoutines { + DEFAULT_AUTO(false, "DEFAULT", LedColors.BLOOD_RED, Commands.print("DEFAULT AUTO SAYS HI")), + + // Wire cover shot autos + S1_I1_W_AUTO( + false, + "S1 to I1 to W (CC)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_1_TO_INTAKE_CONE_1, PathLegs.INTAKE_1_TO_WIRESHOT} + ), + S1_I1_W_I2_AUTO( + false, + "S1 to I1 to W to I2 (CCC)", + LedColors.OFF, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_CONE_1, + PathLegs.INTAKE_1_TO_WIRESHOT, + PathLegs.WIRESHOT_TO_INTAKE_2} + ), + S1_I1_W_I2_C_AUTO( + false, + "S1 to I1 to W to I2 to C (CCC)", + LedColors.OFF, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_CONE_1, + PathLegs.INTAKE_1_TO_WIRESHOT, + PathLegs.WIRESHOT_TO_INTAKE_2, + PathLegs.INTAKE_2_TO_CHARGE_2}, + Builders.AUTO_BUILDER, + true, + false + ), + S1_I1_W_I2_S2_AUTO( + false, + "S1 to I1 to W to I2 to S2 (CCC)", + LedColors.OFF, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_CONE_1, + PathLegs.INTAKE_1_TO_WIRESHOT, + PathLegs.WIRESHOT_TO_INTAKE_2, + PathLegs.INTAKE_2_TO_SCORE_2} + ), + + // 0 piece + charge station balance + C_AUTO( + false, + "C ONLY", + LedColors.SANDY_BROWN, + new PathLegs[] {PathLegs.SCORE_TO_CHARGE}, + Builders.DEFAULT_BUILDER, + true, + false + ), + + // 1 piece + leave + S9_L_AUTO(false, "S9 to L", LedColors.DARK_ORANGE, new PathLegs[] {PathLegs.SCORE_9_TO_LOADING}), + + // 1 piece + charge + S4_M_C_AUTO( + true, + "S4 to M to C (C)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_4_TO_MOBILITY, PathLegs.MOBILITY_TO_CHARGE_4}, + Builders.DEFAULT_BUILDER, + true, + false + ), + S6_M_C_AUTO( + true, + "S6 to M to C (C)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_6_TO_MOBILITY, PathLegs.MOBILITY_TO_CHARGE_6}, + Builders.DEFAULT_BUILDER, + true, + false + ), + + // 1 piece + charge station balance + S1_C_AUTO( + false, + "S1 to C", + LedColors.CHARTREUSE, + new PathLegs[] {PathLegs.SCORE_1_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S2_C_AUTO( + false, + "S2 (CUBE) to C", + LedColors.PINK_LAVENDAR, + new PathLegs[] {PathLegs.SCORE_2_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S3_C_AUTO( + false, + "S3 to C", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_3_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S4_C_AUTO( + false, + "S4 to C", + LedColors.CADET_GREY, + new PathLegs[] {PathLegs.SCORE_4_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S6_C_AUTO( + false, + "S6 to C", + LedColors.BLOOD_RED, + new PathLegs[] {PathLegs.SCORE_6_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S7_C_AUTO( + false, + "S7 to C", + LedColors.ELECTRIC_VIOLET, + new PathLegs[] {PathLegs.SCORE_7_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S8_C_AUTO( + false, + "S8 (CUBE) to C", + LedColors.INCHWORM, + new PathLegs[] {PathLegs.SCORE_8_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + S9_C_AUTO( + false, + "S9 to C", + LedColors.YELLOW, + new PathLegs[] {PathLegs.SCORE_9_TO_CHARGE_2, PathLegs.SCORE_TO_CHARGE} + ), + + // 1 piece + leave + charge station balance + S1_L_C_AUTO( + false, + "S1 LEAVE to C", + LedColors.RAZZLE_DAZZLE_ROSE, + new PathLegs[] {PathLegs.SCORE_1_TO_CHARGE_2, PathLegs.INTAKE_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + S9_L_C_AUTO( + false, + "S9 LEAVE to C", + LedColors.SAFFRON, + new PathLegs[] {PathLegs.SCORE_9_TO_CHARGE_1, PathLegs.INTAKE_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + + // 1.5 piece + S1_I1_AUTO( + false, "S1 to I1 NO C", LedColors.OFF, new PathLegs[] {PathLegs.SCORE_1_TO_INTAKE_CONE_1} + ), + S4_I2_AUTO( + false, + "S4 to I2 NO C (CC)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_4_TO_INTAKE_2}, + Builders.CHARGING_BUILDER + ), + S6_I3_AUTO( + false, + "S6 to I3 NO C (CC)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_6_TO_INTAKE_3, PathLegs.MOBILITY_INTAKE_3_TO_CHARGE}, + Builders.CHARGING_BUILDER + ), + + // 1.5 piece + charge station balance + S1_I1_C_AUTO( + false, + "S1 to I1 to C", + LedColors.AZURE, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_1, PathLegs.INTAKE_1_TO_CHARGE_2, PathLegs.INTAKE_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + S4_I2_C_AUTO( + false, + "S4 to I2 to C", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_4_TO_INTAKE_2, PathLegs.MOBILITY_INTAKE_2_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + S6_I3_C_AUTO( + false, + "S6 to I3 to C", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_6_TO_INTAKE_3, PathLegs.MOBILITY_INTAKE_3_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + S9_I4_C_AUTO( + false, + "S9 to I4 to C", + LedColors.AQUA, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, PathLegs.INTAKE_4_TO_CHARGE_2, PathLegs.INTAKE_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + + // 1.5 piece + yeet + charge station balance + S4_I2_C_Y_AUTO( + true, + "S4 to I2 to C Y (CC)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_4_TO_INTAKE_2, PathLegs.MOBILITY_INTAKE_2_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + S6_I3_C_Y_AUTO( + true, + "S6 to I3 to C Y (CC)", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_6_TO_INTAKE_3, PathLegs.MOBILITY_INTAKE_3_TO_CHARGE}, + Builders.CHARGING_BUILDER, + true, + true + ), + + // 2 piece + S1_I1_S2_AUTO( + false, + "S1 to I1 to S2", + LedColors.BLUE, + new PathLegs[] {PathLegs.SCORE_1_TO_INTAKE_1, PathLegs.INTAKE_1_TO_SCORE_2} + ), + S3_I1_S2_AUTO( + false, + "S3 to I1 to S2", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_3_TO_INTAKE_1, PathLegs.INTAKE_1_TO_SCORE_2} + ), + S4_I1_S2_AUTO( + false, + "S4 to I1 to S2", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_4_TO_INTAKE_1, PathLegs.INTAKE_1_TO_SCORE_2} + ), + S4_I2_S2_AUTO( + false, + "S4 to I2 to S2", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_4_TO_INTAKE_2, PathLegs.INTAKE_2_TO_SCORE_2} + ), + S6_I3_S8_AUTO( + false, + "S6 to I3 to S8", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_6_TO_INTAKE_3, PathLegs.INTAKE_3_TO_SCORE_8} + ), + S6_I4_S8_AUTO( + false, + "S6 to I4 to S8", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_6_TO_INTAKE_4, PathLegs.INTAKE_4_TO_SCORE_8} + ), + S7_I4_S8_AUTO( + false, + "S7 to I4 to S8", + LedColors.OFF, + new PathLegs[] {PathLegs.SCORE_7_TO_INTAKE_4, PathLegs.INTAKE_4_TO_SCORE_8} + ), + S9_I4_S8_AUTO( + false, + "S9 to I4 to S8 (CQ)", + LedColors.DARK_GREEN, + new PathLegs[] {PathLegs.SCORE_9_TO_INTAKE_4, PathLegs.INTAKE_4_TO_SCORE_8} + ), + + // 2 piece + charge station balance + S1_I1_S2_C_AUTO( + true, + "S1 to I1 to S2 to C", + LedColors.MEDIUM_SPRING_GREEN, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_1, PathLegs.INTAKE_1_TO_SCORE_2, PathLegs.SCORE_2_TO_CHARGE_2}, + Builders.DEFAULT_BUILDER, + true, + false + ), + S9_I4_S8_C_AUTO( + false, + "S9 to I4 to S8 to C (CQ)", + LedColors.FUSCHIA, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, PathLegs.INTAKE_4_TO_SCORE_8, PathLegs.SCORE_8_TO_CHARGE_2}, + Builders.DEFAULT_BUILDER, + true, + false + ), + + // 2.5 piece + S1_I1_S2_I2_AUTO( + false, + "S1 to I1 to S2 to I2", + LedColors.LESS_LIME, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_1, PathLegs.INTAKE_1_TO_SCORE_2, PathLegs.SCORE_2_TO_INTAKE_2} + ), + + // 2.5 piece + charge station balance + // CHEZY DEV + S1_I1_S2_I2_C_AUTO( + true, + "S1 to I1 to S2 to I2 to C", + LedColors.MAGENTA, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_1, + PathLegs.INTAKE_1_TO_SCORE_2, + PathLegs.SCORE_2_TO_INTAKE_2, + PathLegs.INTAKE_2_TO_CHARGE_2}, + Builders.DEFAULT_BUILDER, + true, + false + ), + S9_I4_S8_I3_C_AUTO( + false, + "S9 to I4 to S8 to I3 to C (CQQ)", + LedColors.WHITE, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, + PathLegs.INTAKE_4_TO_SCORE_8, + PathLegs.SCORE_8_TO_INTAKE_3, + PathLegs.INTAKE_3_TO_CHARGE_2}, + Builders.AUTO_BUILDER, + true, + false + ), + S9_I4_S8_I3_CONE_C_AUTO( + true, + "S9 to I4 to S8 to I3 CONE to C (CQC)", + LedColors.OFF, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, + PathLegs.INTAKE_4_TO_SCORE_8, + PathLegs.SCORE_8_TO_INTAKE_3_CONE, + PathLegs.INTAKE_3_CONE_TO_CHARGE_2}, + Builders.AUTO_BUILDER, + true, + false + ), + + // 3 piece + // CHEZY DEV + S1_I1_S2_I2_S1_AUTO( + true, + "S1 to I1 to S2 to I2 to S1", + LedColors.BLACK, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_1, + PathLegs.INTAKE_1_TO_SCORE_2, + PathLegs.SCORE_2_TO_INTAKE_2, + PathLegs.INTAKE_2_TO_SCORE_1} + ), + S1_I1_S2_I2_S2_AUTO( + false, + "S1 to I1 to S2 to I2 to S2", + LedColors.BLACK, + new PathLegs[] { + PathLegs.SCORE_1_TO_INTAKE_1, + PathLegs.INTAKE_1_TO_SCORE_2, + PathLegs.SCORE_2_TO_INTAKE_2, + PathLegs.INTAKE_2_TO_SCORE_2} + ), + S9_I4_S8_I3_S8_AUTO( + false, + "S9 to I4 to S8 to I3 to S8 (CQQ)", + LedColors.LIME, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, + PathLegs.INTAKE_4_TO_SCORE_8, + PathLegs.SCORE_8_TO_INTAKE_3_CONE} + ), + S9_I4_S8_I3_S9_CQC_AUTO( + "S9 to I4 to S8 to I3 CONE to S9 (CQC)", + LedColors.OFF, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, + PathLegs.INTAKE_4_TO_SCORE_8, + PathLegs.SCORE_8_TO_INTAKE_3_CONE, + PathLegs.INTAKE_3_TO_SCORE_9_CONE} + ), + S9_SCORE_3_LINK_AUTO( + false, + "S9 Score 3 Link", + LedColors.SEA_GREEN, + new PathLegs[] { + PathLegs.SCORE_9_TO_INTAKE_4, + PathLegs.INTAKE_4_TO_SCORE_8, + PathLegs.SCORE_8_TO_INTAKE_3_CONE, + PathLegs.INTAKE_3_TO_SCORE_7} + ), + /* hybrid auto for testing */ + S1_I1_VIS_S2_AUTO( + true, + "S1 to I1 to S2 Hybrid Auto", + LedColors.BLOOD_RED, + new PathLegs[] {PathLegs.SCORE_1_TO_INTAKE_1}, + new PathLegs[] {PathLegs.INTAKE_1_TO_SCORE_2}, + Builders.DEFAULT_BUILDER + ); + + public final boolean showInDashboard; + public final String name; + public final LedColors color; + public final boolean buildable; + public final Builders builder; + public final Builders builder2; + public final boolean balanceAtEnd; + public final boolean yeetCone; + + private PathLegs[] _paths; + private PathLegs[] _paths2; + private PathPlannerState initialState; + private Pose2d initialRedPose, initialBluePose; + public CommandBase command; + public CommandBase builtCommand; + public CommandBase builtCommandVision; + + AutonomousRoutines(String shuffleboardName, LedColors color, CommandBase command) { + this(true, shuffleboardName, color, command); + } + AutonomousRoutines(boolean show, String shuffleboardName, LedColors color, CommandBase command) { + this.showInDashboard = show; + this.name = shuffleboardName; + this.color = color; + this.command = command; + + this.buildable = false; + this.builder = null; + this.builder2 = null; + this.balanceAtEnd = false; + this.yeetCone = false; + } + + AutonomousRoutines(String shuffleboardName, PathLegs[] paths) { + this(true, shuffleboardName, paths); + } + + AutonomousRoutines(boolean show, String shuffleboardName, PathLegs[] paths) { + this(show, shuffleboardName, LedColors.OFF, paths); + } + + AutonomousRoutines(String shuffleboardName, LedColors color, PathLegs[] paths) { + this(true, shuffleboardName, color, paths); + } + + AutonomousRoutines(boolean show, String shuffleboardName, LedColors color, PathLegs[] paths) { + this(show, shuffleboardName, color, paths, Builders.DEFAULT_BUILDER); + } + + AutonomousRoutines(String shuffleboardName, LedColors color, PathLegs[] paths, Builders builder) { + this(true, shuffleboardName, color, paths, builder); + } + + AutonomousRoutines( + boolean show, String shuffleboardName, LedColors color, PathLegs[] paths, Builders builder + ) { + this(show, shuffleboardName, color, paths, builder, false, false); + } + + AutonomousRoutines( + boolean show, + String shuffleboardName, + LedColors color, + PathLegs[] paths, + Builders builder, + boolean balance, + boolean yeet + ) { + this.showInDashboard = show; + this.name = shuffleboardName; + this.color = color; + + this.buildable = builder != null && paths.length > 0; + this.builder = builder; + this.builder2 = null; + this._paths = paths; + this._paths2 = null; + + this.balanceAtEnd = balance; + this.yeetCone = yeet; + } + + AutonomousRoutines( + boolean show, + String shuffleboardName, + LedColors color, + PathLegs[] preVisPaths, + PathLegs[] postVisPaths, + Builders builder + ) { + this.showInDashboard = show; + this.name = shuffleboardName; + this.color = color; + + this.buildable = builder != null && (preVisPaths.length + postVisPaths.length) > 0; + this.builder = null; + this.builder2 = builder; + this._paths = preVisPaths; + this._paths2 = postVisPaths; + + this.balanceAtEnd = false; + this.yeetCone = false; + } + + public void build( + Drivetrain drivetrain, + Controlboard controlboard, + Elevator elevator, + Claw claw, + LedController ledController + ) { + if (_paths2 != null) { + build2(drivetrain, controlboard, elevator, claw, ledController); + } else if (buildable) { + Vector pathSet = new Vector(); + for (PathLegs p : _paths) { + if (p.currentlyExists) { + pathSet.add(PathPlanner.loadPath(p.name, p.constraint.pathPlanner)); + } else { + System.out.println("Auto routine with currently undefined path " + p.name); + System.exit(1); + } + } + initialState = pathSet.get(0).getInitialState(); + Pose2d initialPose = initialState.poseMeters; + initialBluePose = + new Pose2d(initialPose.getX(), initialPose.getY(), Rotation2d.fromDegrees(180.0)); + initialRedPose = new Pose2d( + Constants.CField.dims.x_M - initialPose.getX(), + initialPose.getY(), + Rotation2d.fromDegrees(0.0) + ); + + builtCommand = builder.construct(pathSet) + .finallyDo((boolean interrupted) -> drivetrain.autoZeroGyro()) + .beforeStarting(() -> { + controlboard.setHeldGamePiece(GamePieceType.CONE_GAME_PIECE); + drivetrain.setAutoPrepScore(false); + }); + + // The order of these matters, a lot + + if (balanceAtEnd && yeetCone) { + command = Commands.sequence( + builtCommand, + new ChargingStationBalanceCommand(drivetrain, controlboard), + AutonomousScoreCommands.yeet(elevator, controlboard, claw, ClawScoreLevels.CONE_LEVEL_2) + .asProxy() + ); + } else if (balanceAtEnd) { + command = Commands.sequence( + builtCommand, new ChargingStationBalanceCommand(drivetrain, controlboard) + ); + } else if (yeetCone) { + command = Commands.sequence( + builtCommand, + AutonomousScoreCommands.yeet(elevator, controlboard, claw, ClawScoreLevels.CONE_LEVEL_2) + .asProxy() + ); + } else { + command = builtCommand; + } + } + } + + public void build2( + Drivetrain drivetrain, + Controlboard controlboard, + Elevator elevator, + Claw claw, + LedController ledController + ) { + if (buildable) { + Vector pathSet = new Vector(); + for (PathLegs p : _paths) { + if (p.currentlyExists) { + pathSet.add(PathPlanner.loadPath(p.name, p.constraint.pathPlanner)); + } else { + System.out.println("Auto routine with currently undefined path " + p.name); + System.exit(1); + } + } + + initialState = pathSet.get(0).getInitialState(); + Pose2d initialPose = initialState.poseMeters; + initialBluePose = + new Pose2d(initialPose.getX(), initialPose.getY(), Rotation2d.fromDegrees(180.0)); + initialRedPose = new Pose2d( + Constants.CField.dims.x_M - initialPose.getX(), + initialPose.getY(), + Rotation2d.fromDegrees(0.0) + ); + + Vector pathSet2 = new Vector(); + for (PathLegs p : _paths2) { + if (p.currentlyExists) { + pathSet2.add(PathPlanner.loadPath(p.name, p.constraint.pathPlanner)); + } else { + System.out.println("Auto routine with currently undefined path " + p.name); + System.exit(1); + } + } + + builtCommand = builder.construct(pathSet).beforeStarting(() -> { + controlboard.setHeldGamePiece(GamePieceType.CONE_GAME_PIECE); + drivetrain.setAutoPrepScore(false); + }); + + builtCommandVision = + builder2.construct2(pathSet2) + .finallyDo((boolean interrupted) -> drivetrain.autoZeroGyro()) + .beforeStarting( + () -> drivetrain.setPose(pathSet2.get(0).getInitialState().poseMeters) + ); + + command = Commands.sequence( + builtCommand, + ledController.setColorCommand(LedColors.AQUA, LedSections.ALL), + builtCommandVision + ); + } + } + + public void configureCommand(CommandBase command) { + this.command = command; + } + + public Pose2d getInitialPose(Alliance alliance) { + return alliance == Alliance.Blue ? initialBluePose : initialRedPose; + } + + public enum Builders { + DEFAULT_BUILDER(Constants.CRobot.drive.control.xy, Constants.CRobot.drive.control.theta), + AUTO_BUILDER(Constants.CRobot.drive.control.xy, Constants.CRobot.drive.control.autonTheta), + CHARGING_BUILDER( + Constants.CRobot.drive.control.xy, Constants.CRobot.drive.control.chargerAutonTheta + ); + + private SwerveAutoBuilder builder; + private SwerveAutoBuilder builder2; + private final PIDConstants xy; + private final PIDConstants theta; + + Builders(PIDConstants xy, PIDConstants theta) { + this.xy = xy; + this.theta = theta; + } + + public void initialize(Drivetrain drivetrain, Map eventMap) { + builder = new SwerveAutoBuilder( + () + -> { return drivetrain.getPose(); }, + (Pose2d initPose) + -> { drivetrain.setPose(initPose, initPose.getRotation()); }, + drivetrain.getSwerveKinematics(), + xy.getPathPlannerTranslation(), + theta.getPathPlannerTheta(), + (SwerveModuleState[] states) + -> { drivetrain.setModuleStates(states); }, + eventMap, + true, + drivetrain + ); + builder2 = new SwerveAutoBuilder( + () + -> { return drivetrain.getPose(); }, + (Pose2d initPose) + -> { drivetrain.setPose(initPose); }, + drivetrain.getSwerveKinematics(), + xy.getPathPlannerTranslation(), + xy.getPathPlannerTheta(), + (SwerveModuleState[] states) + -> { drivetrain.setModuleStates(states); }, + eventMap, + true, + drivetrain + ); + } + + public CommandBase construct(Vector trajectories) { + return builder.fullAuto(trajectories); + } + + public CommandBase construct2(Vector trajectories) { + return builder2.fullAuto(trajectories); + } + } + + public enum PPPConstraints { + AUTO_DEFAULT(3.4, 1.8), + CHARGE(2.0, 2.0), + CHARGE_TRAVERSAL(1.4, 1.6); + + public final com.pathplanner.lib.PathConstraints pathPlanner; + + PPPConstraints(double maxVelo_mps, double maxAccel_mps2) { + pathPlanner = new com.pathplanner.lib.PathConstraints(maxVelo_mps, maxAccel_mps2); + } + } + + public enum PathLegs { + INTAKE_1_TO_WIRESHOT("INTAKE_2_TO_CHARGE_2", PPPConstraints.AUTO_DEFAULT), + INTAKE_1_TO_SCORE_2("INTAKE_1_TO_SCORE_2", PPPConstraints.AUTO_DEFAULT), + INTAKE_2_TO_CHARGE_2("INTAKE_2_TO_CHARGE_2", PPPConstraints.CHARGE), + INTAKE_2_TO_SCORE_1("INTAKE_2_TO_SCORE_1", PPPConstraints.AUTO_DEFAULT), + INTAKE_2_TO_SCORE_2("INTAKE_2_TO_SCORE_2", PPPConstraints.AUTO_DEFAULT), + INTAKE_3_CONE_TO_CHARGE_2("INTAKE_3_CONE_TO_CHARGE_2", PPPConstraints.AUTO_DEFAULT), + INTAKE_3_TO_SCORE_8("INTAKE_3_TO_SCORE_8", PPPConstraints.AUTO_DEFAULT), + INTAKE_3_TO_SCORE_9_CONE("INTAKE_3_TO_SCORE_9_CONE", PPPConstraints.AUTO_DEFAULT), + INTAKE_4_TO_SCORE_8("INTAKE_4_TO_SCORE_8", PPPConstraints.AUTO_DEFAULT), + MOBILITY_INTAKE_2_TO_CHARGE("MOBILITY_INTAKE_2_TO_CHARGE", PPPConstraints.CHARGE), + MOBILITY_INTAKE_3_TO_CHARGE("MOBILITY_INTAKE_3_TO_CHARGE", PPPConstraints.CHARGE), + MOBILITY_TO_CHARGE_4("MOBILITY_TO_CHARGE_4", PPPConstraints.AUTO_DEFAULT), + MOBILITY_TO_CHARGE_6("MOBILITY_TO_CHARGE_6", PPPConstraints.CHARGE), + SCORE_1_TO_INTAKE_1("SCORE_1_TO_INTAKE_1", PPPConstraints.AUTO_DEFAULT), + SCORE_1_TO_INTAKE_CONE_1("SCORE_1_TO_INTAKE_CONE_1", PPPConstraints.AUTO_DEFAULT), + SCORE_4_TO_INTAKE_2("SCORE_4_TO_INTAKE_2", PPPConstraints.CHARGE_TRAVERSAL), + SCORE_4_TO_MOBILITY("SCORE_4_TO_MOBILITY", PPPConstraints.CHARGE_TRAVERSAL), + SCORE_6_TO_INTAKE_3("SCORE_6_TO_INTAKE_3", PPPConstraints.CHARGE_TRAVERSAL), + SCORE_6_TO_MOBILITY("SCORE_6_TO_MOBILITY", PPPConstraints.CHARGE_TRAVERSAL), + SCORE_8_TO_INTAKE_3_CONE("SCORE_8_TO_INTAKE_3_CONE", PPPConstraints.AUTO_DEFAULT), + SCORE_8_TO_INTAKE_3("SCORE_8_TO_INTAKE_3", PPPConstraints.AUTO_DEFAULT), + SCORE_9_TO_INTAKE_4("SCORE_9_TO_INTAKE_4", PPPConstraints.AUTO_DEFAULT), + WIRESHOT_TO_INTAKE_2("WIRESHOT_TO_INTAKE_2", PPPConstraints.AUTO_DEFAULT), + SCORE_TO_CHARGE(false, "SCORE_TO_CHARGE", PPPConstraints.CHARGE), + INTAKE_TO_CHARGE(false, "INTAKE_TO_CHARGE", PPPConstraints.CHARGE), + INTAKE_TOP_TO_CHARGE(false, "INTAKE_TOP_TO_CHARGE", PPPConstraints.CHARGE), + INTAKE_BOTTOM_TO_CHARGE(false, "INTAKE_BOTTOM_TO_CHARGE", PPPConstraints.CHARGE), + INTAKE_3_TO_CHARGE_2(false, "INTAKE_3_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_2_TO_CHARGE_2("SCORE_2_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_8_TO_CHARGE_2(false, "SCORE_8_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_TO_CHARGE_ALIGN_I3(false, "SCORE_TO_CHARGE_ALIGN_I3", PPPConstraints.CHARGE), + SCORE_6_MOBILITY_TO_CHARGE(false, "SCORE_6_MOBILITY_TO_CHARGE", PPPConstraints.CHARGE), + SCORE_9_TO_LOADING(false, "SCORE_9_TO_LOADING", PPPConstraints.CHARGE), + SCORE_1_TO_CHARGE_2(false, "SCORE_1_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_3_TO_CHARGE_2(false, "SCORE_3_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_4_TO_CHARGE_2(false, "SCORE_4_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_6_TO_CHARGE_2(false, "SCORE_6_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_7_TO_CHARGE_2(false, "SCORE_7_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_9_TO_CHARGE_2(false, "SCORE_9_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_9_TO_CHARGE_1(false, "SCORE_9_TO_CHARGE_1", PPPConstraints.CHARGE), + INTAKE_1_TO_CHARGE_2(false, "INTAKE_1_TO_CHARGE_2", PPPConstraints.CHARGE), + INTAKE_4_TO_CHARGE_2(false, "INTAKE_4_TO_CHARGE_2", PPPConstraints.CHARGE), + SCORE_3_TO_INTAKE_1(false, "SCORE_3_TO_INTAKE_1", PPPConstraints.AUTO_DEFAULT), + SCORE_4_TO_INTAKE_1(false, "SCORE_4_TO_INTAKE_1", PPPConstraints.AUTO_DEFAULT), + SCORE_6_TO_INTAKE_4(false, "SCORE_6_TO_INTAKE_4", PPPConstraints.AUTO_DEFAULT), + SCORE_7_TO_INTAKE_4(false, "SCORE_7_TO_INTAKE_4", PPPConstraints.AUTO_DEFAULT), + SCORE_2_TO_INTAKE_2("SCORE_2_TO_INTAKE_2", PPPConstraints.AUTO_DEFAULT), + INTAKE_3_TO_SCORE_7(false, "INTAKE_3_TO_SCORE_7", PPPConstraints.AUTO_DEFAULT); + + public final boolean currentlyExists; + public final String name; + public final PPPConstraints constraint; + + PathLegs(String name, PPPConstraints constraint) { + this(true, name, constraint); + } + + PathLegs(boolean exists, String name, PPPConstraints constraint) { + this.currentlyExists = exists; + this.name = name; + this.constraint = constraint; + } + } +} diff --git a/src/main/java/frc/robot/constants/enums/CameraConfigs.java b/src/main/java/frc/robot/constants/enums/CameraConfigs.java new file mode 100644 index 0000000..19ec58b --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/CameraConfigs.java @@ -0,0 +1,50 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + +public enum CameraConfigs { + // TODO: make these unique to device ids + // TODO: measure drive base cam dimensions + COMP_LEFT_CAMERA("Arducam_Forward", 0, 10.3, 7.5, 29.6, 16.0, 15.0, 0.0, "LEFT"), + COMP_RIGHT_CAMERA("Arducam_Backward", 1, 10.3, -7.5, 29.6, -16.0, 15.0, 0.0, "RIGHT"), + PRACTICE_LEFT_CAMERA("Arducam_Forward", 0, 10.3, 5.2, 30.55, 0.0, 15.0, 0, "LEFT"), + PRACTICE_RIGHT_CAMERA("Arducam_Backward", 1, 10.3, -5.2, 30.55, 0.0, 15.0, 0, "RIGHT"), + DRIVE_BASE_CAMERA("Arducam_Forward", 0, 1, -1, 29.75, 0.0, 21.0, 0.0, "LEFT"); + + public final String cameraName; + public final int id; + public final String fieldObjectName; + public final double r2cXaxisM, r2cYaxisM, r2cZaxisM; + public final double r2cYaw_RAD, r2cPitch_RAD, r2cRoll_RAD; + public final Translation3d r2cTranslation; + public final Rotation3d r2cRotation; + public final Transform3d r2cTransform; + + CameraConfigs( + String cameraName, + int id, + double x, + double y, + double z, + double yaw, + double pitch, + double roll, + String fieldObjectName + ) { + this.cameraName = cameraName; + this.id = id; + this.fieldObjectName = fieldObjectName; + r2cXaxisM = Units.inchesToMeters(x); + r2cYaxisM = Units.inchesToMeters(y); + r2cZaxisM = Units.inchesToMeters(z); + r2cYaw_RAD = Units.degreesToRadians(yaw); + r2cPitch_RAD = Units.degreesToRadians(pitch); + r2cRoll_RAD = Units.degreesToRadians(roll); + r2cTranslation = new Translation3d(r2cXaxisM, r2cYaxisM, r2cZaxisM); + r2cRotation = new Rotation3d(r2cRoll_RAD, r2cPitch_RAD, r2cYaw_RAD); + r2cTransform = new Transform3d(r2cTranslation, r2cRotation); + } +} diff --git a/src/main/java/frc/robot/constants/enums/CameraSets.java b/src/main/java/frc/robot/constants/enums/CameraSets.java new file mode 100644 index 0000000..d0f3cd9 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/CameraSets.java @@ -0,0 +1,60 @@ +package frc.robot.constants.enums; + +import java.util.Arrays; + +public enum CameraSets { + NO_CAMERAS(new int[0], -1), + CAMERA_0(new int[] {0}, 0), + CAMERA_1(new int[] {1}, 1), + BOTH_CAMERAS(new int[] {0, 1}, 0); + + public final int[] ids; + public final int primary; + private boolean[] compatibleCameraSet; + + CameraSets(int[] cameraIDs, int primary) { + ids = cameraIDs; + Arrays.sort(ids); + this.primary = primary; + } + + public boolean isCompatible(CameraSets set) { + if (compatibleCameraSet == null) { + CameraSets[] sets = CameraSets.values(); + compatibleCameraSet = new boolean[sets.length]; + + // Caching this list is much better than doing it each time + for (int i = 0; i < compatibleCameraSet.length; i++) { + compatibleCameraSet[i] = idSubset(sets[i]); + } + } + + return compatibleCameraSet[set.ordinal()]; + } + + // Find if passed sets ids are a subset of current camera set's ids + private boolean idSubset(CameraSets subset) { + // for each id, if the id is not found in the ids array return false + for (int i = 0; i < subset.ids.length; i++) { + if (Arrays.binarySearch(this.ids, subset.ids[i]) == 0) + return false; + } + + // set is a subset of this.ids + return true; + } + + public boolean containsID(int id) { + if (this.ids.length == 0) { + return false; + } + return Arrays.binarySearch(this.ids, id) != 0; + } + + public boolean hasCamera0() { + return this == CAMERA_0 || this == BOTH_CAMERAS; + } + public boolean hasCamera1() { + return this == CAMERA_1 || this == BOTH_CAMERAS; + } +} diff --git a/src/main/java/frc/robot/constants/enums/ClawDirections.java b/src/main/java/frc/robot/constants/enums/ClawDirections.java new file mode 100644 index 0000000..2b5b729 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ClawDirections.java @@ -0,0 +1,8 @@ +package frc.robot.constants.enums; + +public enum ClawDirections { + CLAW_ROLLERS_CONE_IN, + CLAW_ROLLERS_CONE_OUT, + CLAW_ROLLERS_CUBE_IN, + CLAW_ROLLERS_CUBE_OUT +} diff --git a/src/main/java/frc/robot/constants/enums/ClawPositions.java b/src/main/java/frc/robot/constants/enums/ClawPositions.java new file mode 100644 index 0000000..2cd1fef --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ClawPositions.java @@ -0,0 +1,23 @@ +package frc.robot.constants.enums; + +public enum ClawPositions { + CLAW_POSITION_UNKNOWN(0), + CLAW_OPEN_POS_TICKS(2048), + CLAW_MAX_TICKS(2048), + CLAW_CLOSED_POS_TICKS(0), + CLAW_MIN_TICKS(0), + CLAW_OPEN_POS_THRESHOLD(2028), + CLAW_CLOSED_POS_THRESHOLD(21); + + public final int ticks; + + ClawPositions(int ticks) { + this.ticks = ticks; + } + + private static final int CLAW_MIN_FALCON_TICKS = 0; + private static final int CLAW_MAX_FALCON_TICKS = 2048; + private static final int CLAW_OPEN_TICKS = 1024; + + public static final int CLAW_TOLERANCE_TICKS = 1000; +} diff --git a/src/main/java/frc/robot/constants/enums/ClawScoreLevels.java b/src/main/java/frc/robot/constants/enums/ClawScoreLevels.java new file mode 100644 index 0000000..8a7bfc2 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ClawScoreLevels.java @@ -0,0 +1,28 @@ +package frc.robot.constants.enums; + +public enum ClawScoreLevels { + CLAW_SCORE_UNKNOWN(GamePieceType.CONE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_2), + CONE_LEVEL_0(GamePieceType.CONE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_0), + CONE_LEVEL_1(GamePieceType.CONE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_1), + CONE_LEVEL_2(GamePieceType.CONE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_2), + CUBE_LEVEL_0(GamePieceType.CUBE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_0), + CUBE_LEVEL_1(GamePieceType.CUBE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_1), + CUBE_LEVEL_2(GamePieceType.CUBE_GAME_PIECE, ScoringLevels.SCORING_LEVEL_2); + + public final double power; + public final double autoTimeout; + public final double teleTimeout; + + ClawScoreLevels(double power, double autoTO, double teleTO) { + this.power = power; + this.autoTimeout = autoTO; + this.teleTimeout = teleTO; + } + + // explicitly tie it to the SoringLevels enum + ClawScoreLevels(GamePieceType piece, ScoringLevels level) { + this.power = level.getPower(piece); + this.autoTimeout = level.getAutoTO(piece); + this.teleTimeout = level.getTeleTO(piece); + } +} diff --git a/src/main/java/frc/robot/constants/enums/DriveModes.java b/src/main/java/frc/robot/constants/enums/DriveModes.java new file mode 100644 index 0000000..ba0f2b4 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/DriveModes.java @@ -0,0 +1,21 @@ +package frc.robot.constants.enums; + +/** + * The Drive function acts like a dispatch to different swerve functionalities based off + * of mode. Default mode is configurable, but should usually be field relative. + **/ +public enum DriveModes { + UNKNOWN, + ROBOT_CENTRIC, + FIELD_RELATIVE, + SNAP_TO_ANGLE, + SNAKE, + TARGET_RELATIVE, + CHASE_STATIC_TARGET, + CHASE_DYNAMIC_TARGET, + SLEWING_FIELD_RELATIVE, + FIELD_RELATIVE_SKEW_COMPENSATION, + FIELD_RELATIVE_ANTI_TIP, + FIELD_RELATIVE_ROTATION_COMPENSATION_SCALING, + FIELD_RELATIVE_254 +} diff --git a/src/main/java/frc/robot/constants/enums/DrivetrainControl.java b/src/main/java/frc/robot/constants/enums/DrivetrainControl.java new file mode 100644 index 0000000..b77f536 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/DrivetrainControl.java @@ -0,0 +1,144 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.RobotAltModes; +import frc.robot.utils.*; + +public enum DrivetrainControl { + COMP_CONTROL( + KinematicLimits.fromDegrees(4.96824, 11.0, 450.0, 900.0), + KinematicLimits.fromDegrees(4.96824, 3.0, 360.0, 360.0), + 15.0, + Rotation2d.fromDegrees(900.0), + new PIDFConstantsM(3.0, 0.0, 0.0, 0.0, Units.inchesToMeters(0.5)), + PIDFConstantsRad.fromDegrees(2.0, 0.0, 0.0, 0.0, 1.0), + PIDConstants.fromDegrees(1.85, 0.0, 0.0), + PIDConstants.fromDegrees(1.75, 0.0, 0.0) + ), + PRACTICE_CONTROL( + KinematicLimits.fromDegrees(4.96824, 15.0, 450.0, 900.0), + KinematicLimits.fromDegrees(4.96824, 3.0, 360.0, 360.0), + 15.0, + Rotation2d.fromDegrees(900.0), + new PIDFConstantsM(3.0, 0.0, 0.0, 0.0, Units.inchesToMeters(0.5)), + PIDFConstantsRad.fromDegrees(2.0, 0.0, 0.0, 0.0, 1.0), + PIDConstants.fromDegrees(1.85, 0.0, 0.0), + PIDConstants.fromDegrees(1.75, 0.0, 0.0) + ), + SWERVE_BASE_CONTROL( + KinematicLimits.fromDegrees(4.96824, 15.0, 450.0, 900.0), + KinematicLimits.fromDegrees(4.96824, 3.0, 360.0, 360.0), + 15.0, + Rotation2d.fromDegrees(900.0), + new PIDFConstantsM(10.0, 0.0, 0.2, 0.0, Units.inchesToMeters(2.0)), + PIDFConstantsRad.fromDegrees(10.0, 0.0, 0.36, 0.0, 0.5), + PIDConstants.fromDegrees(0.0, 0.0, 0.0), + PIDConstants.fromDegrees(0.0, 0.0, 0.0) + ); + + public final KinematicLimits defaultLimits; + public final KinematicLimits trapezoidalLimits; + public final KinematicLimits slewingLimits; + public final PIDFConstantsM xy; + public final PIDFConstantsRad theta; + public final PIDConstants autonTheta; + public final PIDConstants chargerAutonTheta; + + DrivetrainControl( + KinematicLimits defaultLimits, + KinematicLimits trapezoidalLimits, + KinematicLimits slewingLimits, + PIDFConstantsM xy, + PIDFConstantsRad theta, + PIDConstants autonTheta, + PIDConstants chargerAutonTheta + ) { + this.defaultLimits = defaultLimits; + this.trapezoidalLimits = trapezoidalLimits; + this.slewingLimits = slewingLimits; + this.xy = xy; + this.theta = theta; + this.autonTheta = autonTheta; + this.chargerAutonTheta = chargerAutonTheta; + + if (RobotAltModes.isTestMode) { + this.defaultLimits.multiply(RobotAltModes.TEST_MODE_COEFFICIENT); + } + } + + DrivetrainControl( + KinematicLimits defaultLimits, + KinematicLimits trapezoidalLimits, + double translationSlewingRate, + Rotation2d angularSlewingRate, + PIDFConstantsM xy, + PIDFConstantsRad theta, + PIDConstants autonTheta, + PIDConstants chargerAutonTheta + ) { + this( + defaultLimits, + trapezoidalLimits, + new KinematicLimits( + defaultLimits.maxTranslationalVelocityMPS, + translationSlewingRate, + defaultLimits.maxAngularVelocityPS, + angularSlewingRate + ), + xy, + theta, + autonTheta, + chargerAutonTheta + ); + } + + DrivetrainControl( + KinematicLimits defaultLimits, + KinematicLimits trapezoidalLimits, + KinematicLimits slewingLimits, + PIDFConstantsM xy, + PIDFConstantsRad theta + ) { + this( + defaultLimits, + trapezoidalLimits, + slewingLimits, + xy, + theta, + PIDConstants.fromDegrees(1.85, 0.0, 0.0), + PIDConstants.fromDegrees(1.75, 0.0, 0.0) + ); + } + + DrivetrainControl( + KinematicLimits defaultLimits, + KinematicLimits trapezoidalLimits, + double translationSlewingRate, + Rotation2d angularSlewingRate, + PIDFConstantsM xy, + PIDFConstantsRad theta + ) { + this( + defaultLimits, + trapezoidalLimits, + new KinematicLimits( + defaultLimits.maxTranslationalVelocityMPS, + translationSlewingRate, + defaultLimits.maxAngularVelocityPS, + angularSlewingRate + ), + xy, + theta + ); + } + + public ProfiledPIDController getAngularProfiledPIDController() { + return theta.getProfiledController(trapezoidalLimits.getAngularTrapezoidalContraints()); + } + + public ProfiledPIDController getTranslationaProfiledPIDController() { + return xy.getProfiledController(trapezoidalLimits.getTranslationalTrapezoidalContraints()); + } +} diff --git a/src/main/java/frc/robot/constants/enums/DrivetrainDimensions.java b/src/main/java/frc/robot/constants/enums/DrivetrainDimensions.java new file mode 100644 index 0000000..a9c1090 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/DrivetrainDimensions.java @@ -0,0 +1,41 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.util.Units; + +public enum DrivetrainDimensions { + COMP_BOT_DIMENSIONS(20.75, 26), + PRACTICE_BOT_DIMENSIONS(20.75, 26), + SWERVE_BASE_DIMENSIONS(21.5, 28); + + public final double bumperSegmentDepth_M; + public double trackLength_M, trackWidth_M, halfTrackLength_M, halfTrackWidth_M; + public double frameLength_M, frameWidth_M, halfFrameLength_M, halfFrameWidth_M; + public double bumperLength_M, bumperWidth_M, halfBumperLength_M, halfBumperWidth_M; + + DrivetrainDimensions(double sqrTrackDimensionIn, double sqrFrameDimensionIn) { + this(sqrTrackDimensionIn, sqrTrackDimensionIn, sqrFrameDimensionIn, sqrFrameDimensionIn, 3.25); + } + + DrivetrainDimensions( + double trackLength_IN, + double trackWidth_IN, + double frameLength_IN, + double frameWidth_IN, + double bumperSegmentDepth_IN + ) { + bumperSegmentDepth_M = Units.inchesToMeters(bumperSegmentDepth_IN); + trackLength_M = Units.inchesToMeters(trackLength_IN); + trackWidth_M = Units.inchesToMeters(trackWidth_IN); + halfTrackLength_M = trackLength_M / 2.0; + halfTrackWidth_M = trackWidth_M / 2.0; + + frameLength_M = Units.inchesToMeters(frameLength_IN); + frameWidth_M = Units.inchesToMeters(frameWidth_IN); + halfFrameLength_M = frameLength_M / 2.0; + halfFrameWidth_M = frameWidth_M / 2.0; + bumperLength_M = frameLength_M + 2.0 * bumperSegmentDepth_M; + bumperWidth_M = frameWidth_M + 2.0 * bumperSegmentDepth_M; + halfBumperLength_M = bumperLength_M / 2.0; + halfBumperWidth_M = bumperLength_M / 2.0; + } +} diff --git a/src/main/java/frc/robot/constants/enums/ElevatorPositions.java b/src/main/java/frc/robot/constants/enums/ElevatorPositions.java new file mode 100644 index 0000000..c08b612 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ElevatorPositions.java @@ -0,0 +1,48 @@ +package frc.robot.constants.enums; + +import frc.robot.constants.Constants; + +// TODO: redo this somehow... must be derived from both current robot and current field somehow... +public enum ElevatorPositions { + // in falcon ticks + ELEVATOR_UNKNOWN_POSITION(0), + ELEVATOR_MIN_TICKS(1, 1), + ELEVATOR_STOW_TICKS(1000, 2000), // stow location, above 0 by ~ 1x or 2x tolerance + ELEVATOR_GROUND_CUBE_INTAKE(1250, 2000), // Height for cubes from ground intake + ELEVATOR_GROUND_CONE_INTAKE(4300, 5700), + ELEVATOR_LEVEL_0_CONE(1000, 14000), // scoring height for cone on level 0 + ELEVATOR_LEVEL_0_CUBE(1000, 16500), // scoring height for cube on level 0 + ELEVATOR_LEVEL_2_CONE_OPTIMIZATION(12000, ELEVATOR_UNKNOWN_POSITION.ticks), + ELEVATOR_LEVEL_2_CONE_MANUAL_OPTIMIZATION(15000, ELEVATOR_UNKNOWN_POSITION.ticks), + ELEVATOR_TIP_PREVENTION_THRESHOLD(30000, 12000), // Height that will engage anti tip + ELEVATOR_DROP_CONE_INTAKE(7000), + ELEVATOR_LEVEL_1_CONE(46400, 52000), // scoring height for cone on level 1 + ELEVATOR_LEVEL_1_CUBE(43000, 47000), // scoring height for cube on level 1 + AUTO_ELEVATOR_LEVEL_1_CUBE(26400, 23400), // 45001, // 23400, // 24400, // 26400, + AUTO_ELEVATOR_LEVEL_2_CUBE(43000, 44500), // 44501, // 43001, + ELEVATOR_PLATFORM_CONE_INTAKE(66300, 57000), // Height for intaking from the platform + ELEVATOR_PLATFORM_CUBE_INTAKE(62500, 63000), // Height for intaking from the platform + ELEVATOR_LEVEL_2_CUBE(64500, 69500), // scoring height for cube on level 2 + ELEVATOR_LEVEL_2_CONE(67500, 69500), // scoring height for cone on level 2 + ELEVATOR_MAX_TICKS(68460, 71000); + + public final int ticks; + + // probably add a 3'rd option that can be used for field specific things + ElevatorPositions(int compTicks, int pracTick) { + ticks = Constants.isCompBot ? compTicks + : Constants.isPracticeBot ? pracTick + : ELEVATOR_MIN_FALCON_TICKS; + } + + ElevatorPositions(int everyRobot) { + ticks = everyRobot; + } + + public static final int ELEVATOR_MIN_FALCON_TICKS = 1; + public static final int ELEVATOR_MAX_FALCON_TICKS = Constants.isCompBot ? 67800 : 71000; + + public boolean isUnknown() { + return this == ELEVATOR_UNKNOWN_POSITION; + } +} diff --git a/src/main/java/frc/robot/constants/enums/FieldDims.java b/src/main/java/frc/robot/constants/enums/FieldDims.java new file mode 100644 index 0000000..a203990 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/FieldDims.java @@ -0,0 +1,70 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.utils.UnitUtils; + +public enum FieldDims { + THEORETICAL_FIELD_DIMS(), + NASA_FIELD_DIMS(FieldDims.THEORETICAL_X_IN - 72.25, FieldDims.THEORETICAL_Y_IN - 30.5), + CANADA_FIELD_DIMS(), + ARIZONA_FIELD_DIMS(), + SVR_FIELD_DIMS(), + CURIE_FIELD_DIMS(), + CHEZY_FIELD_DIMS(), + CALGAMES_FIELD_DIMS(); + + // Base Values + public final double x_IN, x_M; + public final double y_IN, y_M; + public final double halfX_IN, halfX_M; + public final double halfY_IN, halfY_M; + public final double xDelta_IN, xDelta_M; + public final double yDelta_IN, yDelta_M; + + FieldDims(double x, double y) { + x_IN = x; + x_M = Units.inchesToMeters(x); + y_IN = y; + y_M = Units.inchesToMeters(y); + + halfX_IN = x / 2.0; + halfX_M = Units.inchesToMeters(halfX_IN); + halfY_IN = y / 2.0; + halfY_M = Units.inchesToMeters(halfY_IN); + + xDelta_IN = x_IN - THEORETICAL_X_IN; + xDelta_M = Units.inchesToMeters(xDelta_IN); + yDelta_IN = y_IN - THEORETICAL_Y_IN; + yDelta_M = Units.inchesToMeters(yDelta_IN); + } + FieldDims() { + this(THEORETICAL_X_IN, THEORETICAL_Y_IN); + } + + // CONSTANT VALUES THAT SHOULD NEVER CHANGE + public static final double THEORETICAL_X_IN = 651.25, + THEORETICAL_X_M = Units.inchesToMeters(THEORETICAL_X_IN); + public static final double THEORETICAL_Y_IN = 315.5, + THEORETICAL_Y_M = Units.inchesToMeters(THEORETICAL_Y_IN); + public static final double TAPE_WIDTH_IN = 2.0, + TAPE_WIDTH_M = Units.inchesToMeters(TAPE_WIDTH_IN); + + public static final double GRID_HARD_STOPS_TO_STAGING_MARK_M = Units.inchesToMeters(224.0); + // Wall to scoring hard stop dimension (the construction is 54.05 and the manual includes tape) + public static final double GRID_HARD_STOPS_LENGTH_M = Units.inchesToMeters(56.25) - TAPE_WIDTH_IN; + public static final double GRID_HARD_STOPS_TO_CHARGE_STATION_M = Units.inchesToMeters(60.69); + + /* ========== THEORETICAL SCORING Y ALIGNMENT ========== */ + public static final double[] THEORETICAL_SCORING_POSITION_Y_M = + UnitUtils.inchesToMeters(new double[] { + 20.0, 42.0, 64.0, 86.0, 108.0, 130.0, 152.0, 174.0, 196.0}); + + public static final double STAGING_MARK_TO_COMMUNITY_ZONE_M = Units.inchesToMeters(85.13); + + public static final double GRID_HARD_STOPS_TO_COMMUNITY_ZONE = + GRID_HARD_STOPS_TO_STAGING_MARK_M - STAGING_MARK_TO_COMMUNITY_ZONE_M; + + public static final double BLUE_SCORING_X_M = + GRID_HARD_STOPS_LENGTH_M + Constants.CRobot.drive.dims.halfBumperLength_M; +} diff --git a/src/main/java/frc/robot/constants/enums/FieldVersions.java b/src/main/java/frc/robot/constants/enums/FieldVersions.java new file mode 100644 index 0000000..fdaa272 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/FieldVersions.java @@ -0,0 +1,130 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.apriltag.AprilTag; +import java.util.List; +import java.util.Vector; + +public enum FieldVersions { + THEORETICAL_FIELD( + FieldDims.THEORETICAL_FIELD_DIMS, + new TagPositions[] { + TagPositions.OFFICIAL_16H5_TAG_1, + TagPositions.OFFICIAL_16H5_TAG_2, + TagPositions.OFFICIAL_16H5_TAG_3, + TagPositions.OFFICIAL_16H5_TAG_4, + TagPositions.OFFICIAL_16H5_TAG_5, + TagPositions.OFFICIAL_16H5_TAG_6, + TagPositions.OFFICIAL_16H5_TAG_7, + TagPositions.OFFICIAL_16H5_TAG_8}, + LocationOffsets.THEORETICAL_OFFSETS + ), + NASA_FIELD( + FieldDims.NASA_FIELD_DIMS, + new TagPositions[] { + TagPositions.NASA_16H5_TAG_1, + TagPositions.NASA_16H5_TAG_2, + TagPositions.NASA_16H5_TAG_3, + TagPositions.NASA_16H5_TAG_4, + TagPositions.NASA_16H5_TAG_5, + TagPositions.NASA_16H5_TAG_6, + TagPositions.NASA_16H5_TAG_7, + TagPositions.NASA_16H5_TAG_8}, + LocationOffsets.NASA_OFFSETS + ), + CANADA_FIELD( + FieldDims.CANADA_FIELD_DIMS, + new TagPositions[] { + TagPositions.CANADA_16H5_TAG_1, + TagPositions.CANADA_16H5_TAG_2, + TagPositions.CANADA_16H5_TAG_3, + TagPositions.CANADA_16H5_TAG_4, + TagPositions.CANADA_16H5_TAG_5, + TagPositions.CANADA_16H5_TAG_6, + TagPositions.CANADA_16H5_TAG_7, + TagPositions.CANADA_16H5_TAG_8}, + LocationOffsets.CANADA_OFFSETS + ), + ARIZONA_FIELD( + FieldDims.ARIZONA_FIELD_DIMS, + new TagPositions[] { + TagPositions.ARIZONA_16H5_TAG_1, + TagPositions.ARIZONA_16H5_TAG_2, + TagPositions.ARIZONA_16H5_TAG_3, + TagPositions.ARIZONA_16H5_TAG_4, + TagPositions.ARIZONA_16H5_TAG_5, + TagPositions.ARIZONA_16H5_TAG_6, + TagPositions.ARIZONA_16H5_TAG_7, + TagPositions.ARIZONA_16H5_TAG_8}, + LocationOffsets.ARIZONA_OFFSETS + ), + SVR_FIELD( + FieldDims.SVR_FIELD_DIMS, + new TagPositions[] { + TagPositions.SVR_16H5_TAG_1, + TagPositions.SVR_16H5_TAG_2, + TagPositions.SVR_16H5_TAG_3, + TagPositions.SVR_16H5_TAG_4, + TagPositions.SVR_16H5_TAG_5, + TagPositions.SVR_16H5_TAG_6, + TagPositions.SVR_16H5_TAG_7, + TagPositions.SVR_16H5_TAG_8}, + LocationOffsets.SVR_OFFSETS + ), + CURIE_FIELD( + FieldDims.CURIE_FIELD_DIMS, + new TagPositions[] { + TagPositions.CURIE_16H5_TAG_1, + TagPositions.CURIE_16H5_TAG_2, + TagPositions.CURIE_16H5_TAG_3, + TagPositions.CURIE_16H5_TAG_4, + TagPositions.CURIE_16H5_TAG_5, + TagPositions.CURIE_16H5_TAG_6, + TagPositions.CURIE_16H5_TAG_7, + TagPositions.CURIE_16H5_TAG_8}, + LocationOffsets.CURIE_OFFSETS + ), + CHEZY_FIELD( + FieldDims.CHEZY_FIELD_DIMS, + new TagPositions[] { + TagPositions.CHEZY_16H5_TAG_1, + TagPositions.CHEZY_16H5_TAG_2, + TagPositions.CHEZY_16H5_TAG_3, + TagPositions.CHEZY_16H5_TAG_4, + TagPositions.CHEZY_16H5_TAG_5, + TagPositions.CHEZY_16H5_TAG_6, + TagPositions.CHEZY_16H5_TAG_7, + TagPositions.CHEZY_16H5_TAG_8}, + LocationOffsets.CHEZY_OFFSETS + ), + CALGAMES_FIELD( + FieldDims.CALGAMES_FIELD_DIMS, + new TagPositions[] { + TagPositions.CALGAMES_16H5_TAG_1, + TagPositions.CALGAMES_16H5_TAG_2, + TagPositions.CALGAMES_16H5_TAG_3, + TagPositions.CALGAMES_16H5_TAG_4, + TagPositions.CALGAMES_16H5_TAG_5, + TagPositions.CALGAMES_16H5_TAG_6, + TagPositions.CALGAMES_16H5_TAG_7, + TagPositions.CALGAMES_16H5_TAG_8}, + LocationOffsets.CALGAMES_OFFSETS + ); + + public final FieldDims dims; + public final LocationOffsets offsets; + + public final int numTags; + public final List aprilTags; + + FieldVersions(FieldDims fieldDims, TagPositions[] tagArr, LocationOffsets offsets) { + this.dims = fieldDims == null ? FieldDims.THEORETICAL_FIELD_DIMS : fieldDims; + this.offsets = offsets == null ? LocationOffsets.THEORETICAL_OFFSETS : offsets; + + tagArr = tagArr == null ? new TagPositions[0] : tagArr; + numTags = tagArr.length; + aprilTags = new Vector(); + for (int i = 0; i < numTags; i++) { + aprilTags.add(tagArr[i].aprilTag); + } + } +} diff --git a/src/main/java/frc/robot/constants/enums/GamePieceType.java b/src/main/java/frc/robot/constants/enums/GamePieceType.java new file mode 100644 index 0000000..e93ea15 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/GamePieceType.java @@ -0,0 +1,28 @@ +package frc.robot.constants.enums; + +public enum GamePieceType { + UNKNOWN_GAME_PIECE("UNKNOWN/NONE", LedColors.OFF), + CUBE_GAME_PIECE(LedColors.CUBE), + CONE_GAME_PIECE(LedColors.CONE); + + public final String label; + public final LedColors intakeColor; + + GamePieceType(LedColors intakeColor) { + label = this.name().replace("_GAME_PIECE", ""); + this.intakeColor = intakeColor; + } + + GamePieceType(String label, LedColors intakeColor) { + this.label = label; + this.intakeColor = intakeColor; + } + + public boolean isCube() { + return this == CUBE_GAME_PIECE; + } + + public boolean isUnknown() { + return this == UNKNOWN_GAME_PIECE; + } +} diff --git a/src/main/java/frc/robot/constants/enums/HPIntakeStations.java b/src/main/java/frc/robot/constants/enums/HPIntakeStations.java new file mode 100644 index 0000000..dc32cb0 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/HPIntakeStations.java @@ -0,0 +1,143 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; +import frc.robot.utils.AlliancePose2d; + +public enum HPIntakeStations { + UNKNOWN_INTAKE_STATION(LedColors.OFF), + DOUBLE_STATION_INNER( + LedColors.BLUE, + LocationMath.DOUBLE_STATION_BLUE_X_M - Constants.LOADING_POSE_OFFSET_M + - Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.DOUBLE_STATION_INNER_Y_M, + LocationMath.DOUBLE_STATION_BLUE_YAW, + LocationMath.DOUBLE_STATION_RED_X_M + Constants.LOADING_POSE_OFFSET_M + + Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.DOUBLE_STATION_INNER_Y_M, + LocationMath.DOUBLE_STATION_RED_YAW + ), + DOUBLE_STATION_OUTER( + LedColors.GREEN, + LocationMath.DOUBLE_STATION_BLUE_X_M - Constants.LOADING_POSE_OFFSET_M + - Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.DOUBLE_STATION_OUTER_Y_M, + LocationMath.DOUBLE_STATION_BLUE_YAW, + LocationMath.DOUBLE_STATION_RED_X_M + Constants.LOADING_POSE_OFFSET_M + + Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.DOUBLE_STATION_OUTER_Y_M, + LocationMath.DOUBLE_STATION_RED_YAW + ), + SINGLE_STATION( + LedColors.PINK, + LocationMath.SINGLE_STATION_BLUE_X_M, + LocationMath.SINGLE_STATION_Y_M - Constants.CRobot.drive.dims.halfBumperWidth_M, + LocationMath.SINGLE_STATION_ANGLE, + LocationMath.SINGLE_STATION_RED_X_M, + LocationMath.SINGLE_STATION_Y_M - Constants.CRobot.drive.dims.halfBumperWidth_M, + LocationMath.SINGLE_STATION_ANGLE + ); + + public final AlliancePose2d pose; + public final LedColors stationColor; + + HPIntakeStations( + LedColors stationColor, + double blueX, + double blueY, + Rotation2d blueYaw, + double redX, + double redY, + Rotation2d redYaw + ) { + this.stationColor = stationColor; + Pose2d bluePose = new Pose2d(blueX, blueY, blueYaw); + Pose2d redPose = new Pose2d(redX, redY, redYaw); + pose = new AlliancePose2d(bluePose, redPose); + } + + HPIntakeStations(LedColors stationColor) { + pose = null; + this.stationColor = stationColor; + } + + public HPIntakeStations getIncr() { + return HPIntakeStations.values()[((ordinal() + 1) % HPIntakeStations.values().length)]; + } + + public static final class LocationMath { + /* ========== DOUBLE STATION INNER / OUTER DIMENSIONS ========== */ + public static final double DOUBLE_STATION_COVER_MAX_TO_BORDER_M = Units.inchesToMeters(34.86); + public static final double DIVIDER_WALL_TO_BORDER_M = Units.inchesToMeters(99.07); + public static final double DOUBLE_STATION_COVER_MIN_TO_BORDER_M = + DIVIDER_WALL_TO_BORDER_M - Units.inchesToMeters(34.25); + + /* ========== DERIVED LOADING ZONE DIMENSIONS ========== */ + public static final double LOADING_ZONE_MIN_Y_M = + FieldDims.THEORETICAL_Y_M - DIVIDER_WALL_TO_BORDER_M; + public static final double LOADING_ZONE_MAX_Y_M = FieldDims.THEORETICAL_Y_M; + + public static final double LOADING_ZONE_RED_MIN_X_M = Units.inchesToMeters(0.0); + // (full field / 2) - 61.36Units.inchesToMeters() + public static final double LOADING_ZONE_RED_MAX_X_M = + (FieldDims.THEORETICAL_X_M / 2.0) - Units.inchesToMeters(61.36); + public static final double LOADING_ZONE_BLUE_MIN_X_M = + FieldDims.THEORETICAL_X_M - LOADING_ZONE_RED_MAX_X_M; + public static final double LOADING_ZONE_BLUE_MAX_X_M = FieldDims.THEORETICAL_X_M; + + /* ========== DERIVED DOUBLE STATION INNER ROBOT Y ALIGNMENT ========== */ + public static final double DOUBLE_STATION_INNER_MIN_Y_M = Constants.CField.dims.y_M + - DIVIDER_WALL_TO_BORDER_M + Constants.CRobot.drive.dims.halfBumperWidth_M; + public static final double DOUBLE_STATION_INNER_MAX_Y_M = Constants.CField.dims.y_M + - DOUBLE_STATION_COVER_MIN_TO_BORDER_M + - (Constants.hasClaw ? Constants.CRobot.claw.dims.halfOpenWidth_M : 0.0); + public static final double DOUBLE_STATION_INNER_Y_M = + (DOUBLE_STATION_INNER_MIN_Y_M + DOUBLE_STATION_INNER_MAX_Y_M) / 2.0 + + Constants.CField.offsets.shiftDoubleInnerIntakeY_M; + + /* ========== DERIVED DOUBLE STATION OUTER ROBOT Y ALIGNMENT ========== */ + public static final double DOUBLE_STATION_OUTER_MAX_Y_M = + Constants.CField.dims.y_M - Constants.CRobot.drive.dims.halfBumperWidth_M; + public static final double DOUBLE_STATION_OUTER_MIN_Y_M = Constants.CField.dims.y_M + - DOUBLE_STATION_COVER_MAX_TO_BORDER_M + + (Constants.hasClaw ? Constants.CRobot.claw.dims.halfOpenWidth_M : 0.0); + public static final double DOUBLE_STATION_OUTER_Y_M = + (DOUBLE_STATION_OUTER_MAX_Y_M + DOUBLE_STATION_OUTER_MIN_Y_M) / 2.0 + + Constants.CField.offsets.shiftDoubleOuterIntakeY_M; + + /* ========== DERIVED DOUBLE STATION X ALIGNMENT ========== */ + public static final double DOUBLE_STATION_RED_X_M = Units.inchesToMeters(14.0); + public static final double DOUBLE_STATION_BLUE_X_M = + Constants.CField.dims.x_M - Units.inchesToMeters(14.0); + + /* ========== DERIVED DOUBLE STATION YAW ALIGNMENT ========== */ + public static final Rotation2d DOUBLE_STATION_BLUE_YAW = Rotation2d.fromDegrees(0.0); + public static final Rotation2d DOUBLE_STATION_RED_YAW = Rotation2d.fromDegrees(180.0); + + /* ========== SINGLE STATION DIMENSIONS ========== */ + public static final Rotation2d SINGLE_STATION_ANGLE = Rotation2d.fromDegrees(90.0); + public static final Rotation2d RED_PLATFORM_ANGLE = Rotation2d.fromDegrees(180.0); + public static final Rotation2d BLUE_PLATFORM_ANGLE = Rotation2d.fromDegrees(0.0); + public static final double SINGLE_STATION_LENGTH_M = Units.inchesToMeters(22.75); + public static final double BORDER_TO_SINGLE_STATION_MAX_M = Units.inchesToMeters(103.625); + + /* ========== DERIVED SINGLE STATION X ALIGNMENT ========== */ + public static final double SINGLE_STATION_RED_MIN_X_M = + BORDER_TO_SINGLE_STATION_MAX_M - SINGLE_STATION_LENGTH_M; + public static final double SINGLE_STATION_RED_MAX_X_M = BORDER_TO_SINGLE_STATION_MAX_M; + public static final double SINGLE_STATION_RED_X_M = + (SINGLE_STATION_RED_MIN_X_M + SINGLE_STATION_RED_MAX_X_M) / 2.0; + public static final double SINGLE_STATION_BLUE_MIN_X_M = + Constants.CField.dims.x_M - BORDER_TO_SINGLE_STATION_MAX_M; + public static final double SINGLE_STATION_BLUE_MAX_X_M = + SINGLE_STATION_BLUE_MIN_X_M + SINGLE_STATION_LENGTH_M; + public static final double SINGLE_STATION_BLUE_X_M = + (SINGLE_STATION_BLUE_MIN_X_M + SINGLE_STATION_BLUE_MAX_X_M) / 2.0; + + /* ========== DERIVED SINGLE STATION Y ALIGNMENT ========== */ + // Final goal (drive into wall for guaranteed alignment) + public static final double SINGLE_STATION_Y_M = Constants.CField.dims.y_M; + } +} diff --git a/src/main/java/frc/robot/constants/enums/LedColors.java b/src/main/java/frc/robot/constants/enums/LedColors.java new file mode 100644 index 0000000..5d8923b --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/LedColors.java @@ -0,0 +1,47 @@ +package frc.robot.constants.enums; + +public enum LedColors { + SPACE_COOKIE(130, 197, 228), + BLUE(0, 0, 255), + OFF(0, 0, 0), + BLACK(0, 0, 0), + DARK_GREEN(0, 100, 0), + AZURE(0, 145, 255), + LESS_LIME(0, 200, 0), + GREEN(0, 255, 0), + LIME(0, 255, 0), + MEDIUM_SPRING_GREEN(0, 255, 145), + AQUA(0, 255, 255), + CYAN(0, 255, 255), + SEA_GREEN(3, 252, 219), + BLOOD_RED(100, 0, 0), + CUBE(133, 23, 192), + PURPLE(133, 23, 192), + ELECTRIC_VIOLET(145, 0, 255), + CHARTREUSE(145, 255, 0), + BLUE_VIOLET(146, 52, 235), + CADET_GREY(156, 164, 181), + INCHWORM(188, 237, 81), + PINK_LAVENDAR(224, 182, 212), + RED_ORANGE(230, 1, 1), + RAZZLE_DAZZLE_ROSE(235, 52, 216), + SAFFRON(235, 180, 52), + SANDY_BROWN(237, 156, 81), + PINK(255, 0, 159), + MAGENTA(255, 0, 145), + FUSCHIA(255, 0, 255), + DARK_ORANGE(255, 145, 0), + CONE(255, 196, 0), + YELLOW(255, 196, 0), + WHITE(255, 255, 255); + + public final int r; + public final int g; + public final int b; + + LedColors(int r, int g, int b) { + this.r = r; + this.g = g; + this.b = b; + } +} diff --git a/src/main/java/frc/robot/constants/enums/LedModes.java b/src/main/java/frc/robot/constants/enums/LedModes.java new file mode 100644 index 0000000..db608d0 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/LedModes.java @@ -0,0 +1,31 @@ +package frc.robot.constants.enums; + +import com.ctre.phoenix.led.*; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; +import com.ctre.phoenix.led.LarsonAnimation.BounceMode; +import frc.robot.constants.LedConfs; + +public enum LedModes { + COLOR_FLOW(new ColorFlowAnimation(255, 0, 0, 150, 1, -1, Direction.Forward, 0)), + FIRE(new FireAnimation(0.7, 0.1, -1, 1, 1, false, 0)), + LARSON(new LarsonAnimation(255, 0, 0, 150, 1, -1, BounceMode.Front, 2, 0)), + RAINBOW(new RainbowAnimation(0.7, 0.6, -1)), + RGB_FADE(new RgbFadeAnimation()), + BLINK_CONE(new StrobeAnimation( + LedColors.CONE.r, LedColors.CONE.g, LedColors.CONE.b, LedConfs.LED_WHITE_LEVEL, 0.6, -1, 0 + )), + BLINK_CUBE(new StrobeAnimation( + LedColors.CUBE.r, LedColors.CUBE.g, LedColors.CUBE.b, LedConfs.LED_WHITE_LEVEL, 0.6, -1, 0 + )); + + public final Animation animation; + + LedModes(Animation animation) { + this.animation = animation; + } + + public LedModes getDecr() { + int prev = (this == COLOR_FLOW ? LedModes.values().length : ordinal()) - 1; + return LedModes.values()[prev]; + } +} diff --git a/src/main/java/frc/robot/constants/enums/LocationOffsets.java b/src/main/java/frc/robot/constants/enums/LocationOffsets.java new file mode 100644 index 0000000..6200b99 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/LocationOffsets.java @@ -0,0 +1,109 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; + +/* + * This is a stash spot for various field specific shifts from field derived values + * It could probably get split into more than 1 file + */ +public enum LocationOffsets { + THEORETICAL_OFFSETS(28.0, 35.0, 0.0), + NASA_OFFSETS( + 27.0, + 35.0, + 0.0, + 0.0, + -6.0, + FieldDims.NASA_FIELD_DIMS.yDelta_IN, + 0.0, + 0.0, + new double[] { + -2.5 - FieldDims.NASA_FIELD_DIMS.yDelta_IN, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + 2.5, + new double[] {0.0, -0.5, -2.5, 0.0, 0.0, -2.5, 0.0, -2.5, -2.5} + ), + CANADA_OFFSETS(28.0, 35.0, 0.0), + ARIZONA_OFFSETS(28.0, 35.0, 0.0), + SVR_OFFSETS(28.0, 35.0, 0.0), + CURIE_OFFSETS(28.0, 35.0, 0.0), + CHEZY_OFFSETS(28.0, 35.0, 0.0), + CALGAMES_OFFSETS(28.0, 35.0, 0.0); + + public final double shiftDoubleInnerIntakeY_M, shiftDoubleOuterIntakeY_M; + public final double shiftAllBlueScoreX_M, shiftAllRedScoreX_M; + public final double[] shiftBlueScoreY_M, shiftRedScoreY_M; + + public final double loadingPoseOffset_M; + // Value when preparing should start + // TODO update distance + public final double platformPrepDistance_M; + // TODO update distance back from cube (basically an offset relative to the cube pose) + // distance back driven when stowing elevator + public final double platformIntakeConeOffset_M; + + LocationOffsets( + double loadingPoseOffset, + double platformPrepDistance, + double platformIntakeConeOffset, + double doubleInnerIntakeY, + double doubleOuterIntakeY, + double shiftAllBlueX, + double shiftAllRedX, + double shiftAllBlueY, + double[] shiftBlueY, + double shiftAllRedY, + double[] shiftRedY + ) { + this.loadingPoseOffset_M = loadingPoseOffset; + this.platformPrepDistance_M = platformPrepDistance; + this.platformIntakeConeOffset_M = platformIntakeConeOffset; + + assert (shiftBlueY.length == Constants.NUM_SCORING_LOCATIONS); + assert (shiftRedY.length == Constants.NUM_SCORING_LOCATIONS); + shiftBlueScoreY_M = new double[Constants.NUM_SCORING_LOCATIONS]; + shiftRedScoreY_M = new double[Constants.NUM_SCORING_LOCATIONS]; + + for (int i = 0; i < Constants.NUM_SCORING_LOCATIONS; i++) { + shiftBlueScoreY_M[i] = Units.inchesToMeters(shiftAllBlueY + shiftBlueY[i]); + shiftRedScoreY_M[i] = Units.inchesToMeters(shiftAllRedY + shiftRedY[i]); + } + shiftAllBlueScoreX_M = Units.inchesToMeters(shiftAllBlueX); + shiftAllRedScoreX_M = Units.inchesToMeters(shiftAllRedX); + shiftDoubleInnerIntakeY_M = Units.inchesToMeters(doubleInnerIntakeY); + shiftDoubleOuterIntakeY_M = Units.inchesToMeters(doubleOuterIntakeY); + } + + LocationOffsets(LocationOffsets copy) { + this.loadingPoseOffset_M = copy.loadingPoseOffset_M; + this.platformPrepDistance_M = copy.platformPrepDistance_M; + this.platformIntakeConeOffset_M = copy.platformIntakeConeOffset_M; + this.shiftDoubleInnerIntakeY_M = copy.shiftDoubleInnerIntakeY_M; + this.shiftDoubleOuterIntakeY_M = copy.shiftDoubleOuterIntakeY_M; + this.shiftAllBlueScoreX_M = copy.shiftAllBlueScoreX_M; + this.shiftAllRedScoreX_M = copy.shiftAllRedScoreX_M; + this.shiftBlueScoreY_M = copy.shiftBlueScoreY_M; + this.shiftRedScoreY_M = copy.shiftRedScoreY_M; + } + LocationOffsets( + double loadingPoseOffset, double platformPrepDistance, double platformIntakeConeOffset + ) { + this.loadingPoseOffset_M = loadingPoseOffset; + this.platformPrepDistance_M = platformPrepDistance; + this.platformIntakeConeOffset_M = platformIntakeConeOffset; + + shiftDoubleInnerIntakeY_M = 0.0; + shiftDoubleOuterIntakeY_M = 0.0; + shiftAllBlueScoreX_M = 0.0; + shiftAllRedScoreX_M = 0.0; + shiftBlueScoreY_M = new double[Constants.NUM_SCORING_LOCATIONS]; + shiftRedScoreY_M = new double[Constants.NUM_SCORING_LOCATIONS]; + } + + public final double getBlueScoringY(int id) { + return FieldDims.THEORETICAL_SCORING_POSITION_Y_M[id] + shiftBlueScoreY_M[id]; + } + public final double getRedScoringY(int id) { + return FieldDims.THEORETICAL_SCORING_POSITION_Y_M[id] + shiftRedScoreY_M[id]; + } +} diff --git a/src/main/java/frc/robot/constants/enums/ModuleControl.java b/src/main/java/frc/robot/constants/enums/ModuleControl.java new file mode 100644 index 0000000..ccd338d --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ModuleControl.java @@ -0,0 +1,177 @@ +package frc.robot.constants.enums; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import frc.robot.constants.RobotAltModes; +import frc.robot.utils.PIDFConstants; + +public enum ModuleControl { + FALCON_MK4_FALCON_L2( + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(0.6, 0.0, 12.0, 0.0), + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(0.2, 0.0, 0.0, 0.0) + ), + FALCON_MK4I_FALCON_L2( + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(1.2, 0.0, 24.0, 0.0), + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(0.4, 0.0, 0.0, 0.0) + ), + COLSON_FALCON_MK4_FALCON_L2( + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(0.6, 0.0, 12.0, 0.0), + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(0.2, 0.0, 0.0, 0.0) + ), + COLSON_FALCON_MK4I_FALCON_L2( + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(1.2, 0.0, 24.0, 0.0), + new PIDFConstants(0.10, 0.0, 0.0, 0.0), + new PIDFConstants(0.4, 0.0, 0.0, 0.0) + ); + // https://github.com/FRCTeam2910/2021CompetitionRobot/blob/5fabbff6814a8fa71ef614f691342847ad885bf5/src/main/java/org/frcteam2910/c2020/subsystems/DrivetrainSubsystem.java + + // TODO: Move and factor into an enum or class + /* Drive Motor Closed Loop PID Values */ + public final PIDFConstants drive; + /* Steer Motor Closed Loop PID Values */ + public final PIDFConstants steer; + public final PIDFConstants simDrive; + public final PIDFConstants simSteer; + public final TalonFXConfiguration driveConf; + public final TalonFXConfiguration steerConf; + public final boolean invertEncoder = false; + + ModuleControl(PIDFConstants drive, PIDFConstants steer) { + this(drive, steer, drive, steer); + } + + ModuleControl( + PIDFConstants drive, PIDFConstants steer, PIDFConstants simDrive, PIDFConstants simSteer + ) { + this.drive = drive; + this.steer = steer; + this.simDrive = simDrive; + this.simSteer = simSteer; + driveConf = initDriveFalcon(); + steerConf = initSteerFalcon(); + } + + private TalonFXConfiguration initDriveFalcon() { + TalonFXConfiguration driveConfiguration = new TalonFXConfiguration(); + + // TODO: Move and factor into an enum or class + /* Swerve Drive Motor Configuration */ + SupplyCurrentLimitConfiguration driveSupplyLimit = new SupplyCurrentLimitConfiguration( + ElectricalConf.DRIVE_ENABLE_SUPPLY_CURRENT_LIMIT, + ElectricalConf.DRIVE_CONTINUOUS_SUPPLY_CURRENT_LIMIT, + ElectricalConf.DRIVE_PEAK_SUPPLY_CURRENT_LIMIT, + ElectricalConf.DRIVE_PEAK_CURRENT_DURATION + ); + + driveConfiguration.slot0 = + RobotAltModes.isSim ? simDrive.toCTRESlotConfiguration() : drive.toCTRESlotConfiguration(); + driveConfiguration.supplyCurrLimit = driveSupplyLimit; + driveConfiguration.statorCurrLimit = ElectricalConf.DRIVE_STATOR_LIMIT; + driveConfiguration.initializationStrategy = SensorInitializationStrategy.BootToZero; + driveConfiguration.openloopRamp = ElectricalConf.DRIVE_OPEN_LOOP_RAMP; + driveConfiguration.closedloopRamp = ElectricalConf.DRIVE_CLOSED_LOOP_RAMP; + + // TODO: Should this always be on? + driveConfiguration.voltageCompSaturation = + ElectricalConf.AUTONOMOUS_MOTOR_VOLTAGE_COMPENSATION_SCALE; + // TODO: Avoid circular reference + return driveConfiguration; + } + + private TalonFXConfiguration initSteerFalcon() { + TalonFXConfiguration steerConfiguration = new TalonFXConfiguration(); + + /* Swerve Angle Motor Configurations */ + SupplyCurrentLimitConfiguration angleSupplyLimit = new SupplyCurrentLimitConfiguration( + ElectricalConf.ANGLE_ENABLE_CURRENT_LIMIT, + ElectricalConf.ANGLE_CONTINUOUS_CURRENT_LIMIT, + ElectricalConf.ANGLE_PEAK_CURRENT_LIMIT, + ElectricalConf.ANGLE_PEAK_CURRENT_DURATION + ); + StatorCurrentLimitConfiguration angleStatorLimit = new StatorCurrentLimitConfiguration( + ElectricalConf.ANGLE_ENABLE_CURRENT_LIMIT, + ElectricalConf.ANGLE_CONTINUOUS_CURRENT_LIMIT, + ElectricalConf.ANGLE_PEAK_CURRENT_LIMIT, + ElectricalConf.ANGLE_PEAK_CURRENT_DURATION + ); + steerConfiguration.slot0 = + RobotAltModes.isSim ? simSteer.toCTRESlotConfiguration() : steer.toCTRESlotConfiguration(); + + steerConfiguration.supplyCurrLimit = angleSupplyLimit; + steerConfiguration.statorCurrLimit = angleStatorLimit; + steerConfiguration.initializationStrategy = SensorInitializationStrategy.BootToZero; + + steerConfiguration.voltageCompSaturation = + ElectricalConf.AUTONOMOUS_MOTOR_VOLTAGE_COMPENSATION_SCALE; + + return steerConfiguration; + } + + /* Neutral Modes */ + public static final NeutralMode STEER_NEUTRAL_MODE = NeutralMode.Coast; + public static final NeutralMode DRIVE_NEUTRAL_MODE = NeutralMode.Brake; + + // TODO: Move and factor into an enum or class + public static final class ElectricalConf { + /* Drive Motor Characterization Values */ + // divide by 12 to convert from volts to percent output for CTRE (not sure if + // this is needed) + public static final double DRIVE_KS_VOLT = 0.667 / 12; + public static final double DRIVE_KV_VOLTPMPS = 2.44 / 12; + public static final double DRIVE_KA_VOLTPMPS_SQ = 0.27 / 12; + // /* Drive Motor Characterization Values */ + // public static final SimpleMotorFeedforward driveFeedforward = new + // SimpleMotorFeedforward(0.2, 2.2201, 0.16343); + // ??? + + /* Swerve Current Limiting */ + public static final double ANGLE_CONTINUOUS_CURRENT_LIMIT = 30; + public static final double ANGLE_PEAK_CURRENT_LIMIT = 40; + public static final double ANGLE_PEAK_CURRENT_DURATION = 0.1; + public static final boolean ANGLE_ENABLE_CURRENT_LIMIT = true; + + public static final double DRIVE_CONTINUOUS_SUPPLY_CURRENT_LIMIT = 30; + public static final double DRIVE_PEAK_SUPPLY_CURRENT_LIMIT = 40; + + public static final double DRIVE_CONTINUOUS_STATOR_CURRENT_LIMIT = 80; + public static final double DRIVE_PEAK_STATOR_CURRENT_LIMIT = 90; + + public static final double AUTON_DRIVE_CONTINUOUS_STATOR_CURRENT_LIMIT = 60; + public static final double AUTON_DRIVE_PEAK_STATOR_CURRENT_LIMIT = 80; + + public static final double DRIVE_PEAK_CURRENT_DURATION = 0.1; + public static final boolean DRIVE_ENABLE_STATOR_CURRENT_LIMIT = true; + public static final boolean DRIVE_ENABLE_SUPPLY_CURRENT_LIMIT = true; + + public static final double DRIVE_OPEN_LOOP_RAMP = 0.25; + public static final double DRIVE_CLOSED_LOOP_RAMP = 0.0; + + public static final double AUTONOMOUS_MOTOR_VOLTAGE_COMPENSATION_SCALE = 12.0; + + // TODO: Move and factor into an enum or class + public static final StatorCurrentLimitConfiguration DRIVE_STATOR_LIMIT = + new StatorCurrentLimitConfiguration( + ElectricalConf.DRIVE_ENABLE_STATOR_CURRENT_LIMIT, + ElectricalConf.DRIVE_CONTINUOUS_STATOR_CURRENT_LIMIT, + ElectricalConf.DRIVE_PEAK_STATOR_CURRENT_LIMIT, + ElectricalConf.DRIVE_PEAK_CURRENT_DURATION + ); + public static final StatorCurrentLimitConfiguration AUTO_DRIVE_STATOR_LIMIT = + new StatorCurrentLimitConfiguration( + ElectricalConf.DRIVE_ENABLE_STATOR_CURRENT_LIMIT, + ElectricalConf.DRIVE_CONTINUOUS_STATOR_CURRENT_LIMIT, + ElectricalConf.DRIVE_PEAK_STATOR_CURRENT_LIMIT, + ElectricalConf.DRIVE_PEAK_CURRENT_DURATION + ); + } +} diff --git a/src/main/java/frc/robot/constants/enums/ModuleModels.java b/src/main/java/frc/robot/constants/enums/ModuleModels.java new file mode 100644 index 0000000..e27b0a6 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ModuleModels.java @@ -0,0 +1,71 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.util.Units; + +public enum ModuleModels { + SDS_MK4_L1(ModuleTypes.SDS_MK4, ModuleDriveRatios.SDS_L1), + SDS_MK4_L2(ModuleTypes.SDS_MK4, ModuleDriveRatios.SDS_L2), + SDS_MK4_L3(ModuleTypes.SDS_MK4, ModuleDriveRatios.SDS_L3), + SDS_MK4_L4(ModuleTypes.SDS_MK4, ModuleDriveRatios.SDS_L4), + SDS_MK4I_L1(ModuleTypes.SDS_MK4I, ModuleDriveRatios.SDS_L1), + SDS_MK4I_L2(ModuleTypes.SDS_MK4I, ModuleDriveRatios.SDS_L2), + SDS_MK4I_L3(ModuleTypes.SDS_MK4I, ModuleDriveRatios.SDS_L3); + + public final ModuleTypes type; + public final ModuleDriveRatios ratio; + public final double driveRatio, steerRatio; + public final double theoreticalMaxWheelSpeed; + + ModuleModels(ModuleTypes type, ModuleDriveRatios ratio) { + this.type = type; + this.ratio = ratio; + this.driveRatio = ratio.driveRatio; + this.steerRatio = type.steerRatio; + // motor RPM --> motor RPS --> wheel RPS --> wheel MPS + // TODO check this number, probably replace theoretical max with on blocks max + this.theoreticalMaxWheelSpeed = (6380.0 / 60.0) * type.wheelCircumferenceM / ratio.driveRatio; + } + + public enum ModuleTypes { + SDS_MK4(false, false, new int[][] {{32, 15}, {60, 10}}), + SDS_MK4I(false, true, new int[][] {{50, 14}, {60, 10}}); + // SWERVEX + // SWERVENSTEER + + public final boolean invertDrive; + public final boolean invertSteer; + public final double steerRatio; + public final double wheelDiameterM = Units.inchesToMeters(3.94); + public final double wheelCircumferenceM = wheelDiameterM * Math.PI; + + ModuleTypes(boolean invertDrive, boolean invertSteer, int[][] gearPairs) { + this.invertDrive = invertDrive; + this.invertSteer = invertSteer; + + double ratio = 1.0; + for (int i = 0; i < gearPairs.length; i++) { + assert (gearPairs[i].length == 2); + ratio = ratio * gearPairs[i][0] / gearPairs[i][1]; + } + this.steerRatio = ratio; + } + } + + public enum ModuleDriveRatios { + SDS_L1(new int[][] {{50, 14}, {19, 25}, {45, 15}}), + SDS_L2(new int[][] {{50, 14}, {17, 27}, {45, 15}}), + SDS_L3(new int[][] {{50, 14}, {16, 28}, {45, 15}}), + SDS_L4(new int[][] {{48, 16}, {16, 28}, {45, 15}}); + + public final double driveRatio; + + ModuleDriveRatios(int[][] gearPairs) { + double ratio = 1.0; + for (int i = 0; i < gearPairs.length; i++) { + assert (gearPairs[i].length == 2); + ratio = ratio * gearPairs[i][0] / gearPairs[i][1]; + } + this.driveRatio = ratio; + } + } +} diff --git a/src/main/java/frc/robot/constants/enums/ScoringLevels.java b/src/main/java/frc/robot/constants/enums/ScoringLevels.java new file mode 100644 index 0000000..6f42ad9 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ScoringLevels.java @@ -0,0 +1,169 @@ +package frc.robot.constants.enums; + +public enum ScoringLevels { + UNKNOWN_SCORING_LEVEL( + "UNKNOWN SCORING LEVEL", + 2, + 1.0, + 0.5, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE + ), + // hybrid scoring locations + SCORING_LEVEL_0( + "(ZERO) LVL 0", + 0, + 0.5, + 0.6, + ElevatorPositions.ELEVATOR_LEVEL_0_CONE, + ElevatorPositions.ELEVATOR_LEVEL_0_CUBE, + Consts.AUTO_CONE_SHOT_TIMEOUT_S, + Consts.AUTO_CUBE_SHOT_TIMEOUT_S, + 0.3, + 0.3 + ), + // middle scoring locations + SCORING_LEVEL_1( + "(ONE) LVL 1", + 1, + 0.8, + 0.5, + ElevatorPositions.ELEVATOR_LEVEL_1_CONE, + ElevatorPositions.ELEVATOR_LEVEL_1_CUBE + ), + // top scoring locations + SCORING_LEVEL_2( + "(TWO) LVL 2", + 2, + 1.0, + 0.5, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE + ), + SCORING_THROW( + "THROWING", + 3, + 1.0, + 1.0, + ElevatorPositions.ELEVATOR_LEVEL_2_CONE, + ElevatorPositions.ELEVATOR_LEVEL_2_CUBE, + Consts.AUTO_YEET_TIMEOUT_S, + Consts.AUTO_YEET_TIMEOUT_S, + Consts.TELEOP_YEET_TIMEOUT_S, + Consts.TELEOP_YEET_TIMEOUT_S + ); + + public final String label; + public final int id; + public final double conePower; + public final double cubePower; + public final ElevatorPositions coneHeight; + public final ElevatorPositions cubeHeight; + + public final double coneAutoTO; + public final double coneTeleTO; + public final double cubeAutoTO; + public final double cubeTeleTO; + + ScoringLevels( + String label, + int id, + double conePower, + double cubePower, + ElevatorPositions coneHeight, + ElevatorPositions cubeHeight, + double coneAutoTO, + double cubeAutoTO, + double coneTeleTO, + double cubeTeleTO + ) { + this.label = label; + this.id = id; + this.conePower = -Math.abs(conePower); + this.cubePower = cubePower; + this.coneHeight = coneHeight; + this.cubeHeight = cubeHeight; + this.coneAutoTO = coneAutoTO; + this.coneTeleTO = coneTeleTO; + this.cubeAutoTO = cubeAutoTO; + this.cubeTeleTO = cubeTeleTO; + } + + ScoringLevels( + String label, + int id, + double conePower, + double cubePower, + ElevatorPositions coneHeight, + ElevatorPositions cubeHeight + ) { + this( + label, + id, + conePower, + cubePower, + coneHeight, + cubeHeight, + Consts.AUTO_CONE_SHOT_TIMEOUT_S, + Consts.AUTO_CUBE_SHOT_TIMEOUT_S, + Consts.TELEOP_CONE_SHOT_TIMEOUT_S, + Consts.TELEOP_CUBE_SHOT_TIMEOUT_S + ); + } + + public double getPower(GamePieceType heldPiece) { + return heldPiece.isCube() ? cubePower : conePower; + } + + public double getAutoTO(GamePieceType heldPiece) { + return heldPiece.isCube() ? cubeAutoTO : coneAutoTO; + } + + public double getTeleTO(GamePieceType heldPiece) { + return heldPiece.isCube() ? cubeTeleTO : coneTeleTO; + } + + public ElevatorPositions getPosition(GamePieceType heldPiece) { + return heldPiece.isCube() ? cubeHeight : coneHeight; + } + + // TODO: FIX Ryan made a mistake + public ScoringLevels getIncr() { + switch (id) { + case 0: + return SCORING_LEVEL_1; + case 1: + case 2: + default: + return SCORING_LEVEL_2; + } + } + + public ScoringLevels getDecr() { + switch (id) { + case 2: + return SCORING_LEVEL_1; + case 1: + case 0: + default: + return SCORING_LEVEL_0; + } + } + + public static final class Consts { + public static final double CONE_LEVEL_0_POWER = -0.5; + public static final double CONE_LEVEL_1_POWER = -0.8; + public static final double CONE_LEVEL_2_POWER = -1.0; + public static final double CUBE_LEVEL_0_POWER = 0.6; + public static final double CUBE_LEVEL_1_POWER = 0.5; + public static final double CUBE_LEVEL_2_POWER = 0.5; + public static final double CUBE_THROW_POWER = 1.00; + + public static final double TELEOP_CONE_SHOT_TIMEOUT_S = 0.16; + public static final double TELEOP_CUBE_SHOT_TIMEOUT_S = 0.25; + public static final double TELEOP_YEET_TIMEOUT_S = 0.3; + public static final double AUTO_CONE_SHOT_TIMEOUT_S = 0.8; + public static final double AUTO_CUBE_SHOT_TIMEOUT_S = 0.15; + public static final double AUTO_YEET_TIMEOUT_S = 0.5; + } +} diff --git a/src/main/java/frc/robot/constants/enums/ScoringLocations.java b/src/main/java/frc/robot/constants/enums/ScoringLocations.java new file mode 100644 index 0000000..9e094b5 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/ScoringLocations.java @@ -0,0 +1,229 @@ +package frc.robot.constants.enums; + +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.util.Units; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.constants.Constants; +import frc.robot.constants.LedConfs.ScoringSections; +import frc.robot.constants.LedConfs.ScoringSubsections; +import frc.robot.utils.AlliancePose2d; + +public enum ScoringLocations { + // Unknown location will default to locaation 9 behavior + UNKNOWN_SCORING_LOCATION( + 8, + ScoringSections.UNKNOWN_SCORING_SECTION, + ScoringSubsections.UNKNOWN_SCORING_SUBSECTION, + CameraSets.CAMERA_0, + CameraSets.CAMERA_1 + ), + // Cone, wire cover section, adjacent to the wall + SCORING_LOCATION_1( + 0, + ScoringSections.BOTTOM, + ScoringSubsections.SUB_BOTTOM, + CameraSets.CAMERA_1, + CameraSets.CAMERA_0 + ), + // Cube, wire cover section, cube platform + SCORING_LOCATION_2(1, ScoringSections.BOTTOM, ScoringSubsections.SUB_MIDDLE, CameraSets.CAMERA_0), + // Cone, wire cover section, middle most + SCORING_LOCATION_3( + 2, + ScoringSections.BOTTOM, + ScoringSubsections.SUB_TOP, + CameraSets.CAMERA_0, + CameraSets.CAMERA_1 + ), + // Cone, coopertition section, closest to wire cover + SCORING_LOCATION_4( + 3, + ScoringSections.MIDDLE, + ScoringSubsections.SUB_BOTTOM, + CameraSets.CAMERA_1, + CameraSets.CAMERA_0 + ), + // Cube, coopertition section, cube platform + SCORING_LOCATION_5(4, ScoringSections.MIDDLE, ScoringSubsections.SUB_MIDDLE, CameraSets.CAMERA_0), + // Cone, coopertition section, closest to loading zone + SCORING_LOCATION_6( + 5, + ScoringSections.MIDDLE, + ScoringSubsections.SUB_TOP, + CameraSets.CAMERA_0, + CameraSets.CAMERA_1 + ), + // Cone, loading zone section, middle most + SCORING_LOCATION_7( + 6, + ScoringSections.TOP, + ScoringSubsections.SUB_BOTTOM, + CameraSets.CAMERA_1, + CameraSets.CAMERA_0 + ), + // Cube, loading zone section, cube platform + SCORING_LOCATION_8(7, ScoringSections.TOP, ScoringSubsections.SUB_MIDDLE, CameraSets.CAMERA_0), + // Cone, loading zone section, adjacent to laoding zone + SCORING_LOCATION_9( + 8, ScoringSections.TOP, ScoringSubsections.SUB_TOP, CameraSets.CAMERA_0, CameraSets.CAMERA_1 + ); + + /* ========== SCORING ANGLE ========== */ + public static final Rotation2d RED_SCORING_ANGLE = Rotation2d.fromDegrees(0.0); + public static final Rotation2d BLUE_SCORING_ANGLE = Rotation2d.fromDegrees(180.0); + + public final int index; + public final AlliancePose2d pose; + public final ScoringSections section; + public final ScoringSubsections subsection; + public final CameraSets redCameras, blueCameras; + + ScoringLocations( + int index, ScoringSections section, ScoringSubsections subsection, CameraSets camera + ) { + this(index, section, subsection, camera, camera); + } + + ScoringLocations( + int index, + AlliancePose2d poses, + ScoringSections section, + ScoringSubsections subsection, + CameraSets camera + ) { + this(index, poses, section, subsection, camera, camera); + } + + ScoringLocations( + int index, + Pose2d bluePose, + Pose2d redPose, + ScoringSections section, + ScoringSubsections subsection, + CameraSets camera + ) { + this(index, bluePose, redPose, section, subsection, camera, camera); + } + + ScoringLocations( + int index, + Pose2d bluePose, + Pose2d redPose, + ScoringSections section, + ScoringSubsections subsection, + CameraSets blueCameras, + CameraSets redCameras + ) { + this( + index, new AlliancePose2d(bluePose, redPose), section, subsection, blueCameras, redCameras + ); + } + + ScoringLocations( + int index, + AlliancePose2d poses, + ScoringSections section, + ScoringSubsections subsection, + CameraSets blueCameras, + CameraSets redCameras + ) { + this.index = index; + this.section = section; + this.subsection = subsection; + pose = poses; + this.blueCameras = blueCameras; + this.redCameras = redCameras; + } + + ScoringLocations( + int index, + ScoringSections section, + ScoringSubsections subsection, + CameraSets blueCameras, + CameraSets redCameras + ) { + this.index = index; + this.section = section; + this.subsection = subsection; + pose = derivedPose(); + this.blueCameras = blueCameras; + this.redCameras = redCameras; + } + + private AlliancePose2d derivedPose() { + /* ========== DERIVED SCORING X COORDINATES ========== */ + double blueX = FieldDims.GRID_HARD_STOPS_LENGTH_M + + Constants.CRobot.drive.dims.halfBumperLength_M + + Constants.CField.offsets.shiftAllBlueScoreX_M; + double redX = (Constants.CField.dims.x_M - FieldDims.GRID_HARD_STOPS_LENGTH_M) + - Constants.CRobot.drive.dims.halfBumperLength_M + + Constants.CField.offsets.shiftAllRedScoreX_M; + + Pose2d bluePose = + new Pose2d(blueX, Constants.CField.offsets.getBlueScoringY(index), BLUE_SCORING_ANGLE); + Pose2d redPose = + new Pose2d(redX, Constants.CField.offsets.getRedScoringY(index), RED_SCORING_ANGLE); + return new AlliancePose2d(bluePose, redPose); + } + + // TODO FIX RYAN'S DUMB + public ScoringLocations getIncr() { + switch (index) { + case 0: + return SCORING_LOCATION_2; + case 1: + return SCORING_LOCATION_3; + case 2: + return SCORING_LOCATION_4; + case 3: + return SCORING_LOCATION_5; + case 4: + return SCORING_LOCATION_6; + case 5: + return SCORING_LOCATION_7; + case 6: + return SCORING_LOCATION_8; + case 7: + return SCORING_LOCATION_9; + case 8: + return SCORING_LOCATION_1; + default: + return SCORING_LOCATION_9; + } + } + + public ScoringLocations getDecr() { + switch (index) { + case 0: + return SCORING_LOCATION_9; + case 1: + return SCORING_LOCATION_1; + case 2: + return SCORING_LOCATION_2; + case 3: + return SCORING_LOCATION_3; + case 4: + return SCORING_LOCATION_4; + case 5: + return SCORING_LOCATION_5; + case 6: + return SCORING_LOCATION_6; + case 7: + return SCORING_LOCATION_7; + case 8: + return SCORING_LOCATION_8; + default: + return SCORING_LOCATION_8; + } + } + + public CameraSets getScoringCamera(boolean isRed) { + return isRed ? redCameras : blueCameras; + } + + public static Rotation2d getScoreAngle(Alliance alliance) { + return alliance == Alliance.Red ? RED_SCORING_ANGLE : BLUE_SCORING_ANGLE; + } +} diff --git a/src/main/java/frc/robot/constants/enums/StaticTargets.java b/src/main/java/frc/robot/constants/enums/StaticTargets.java new file mode 100644 index 0000000..da334f8 --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/StaticTargets.java @@ -0,0 +1,44 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.utils.AlliancePose2d; + +public enum StaticTargets { + CHARGING_TARGET( + Zones.ROBOT_COMMUNITY, + new AlliancePose2d( + new Pose2d(Zones.CHARGING_STATION_ROBOT_BALANCE.redMax, Rotation2d.fromDegrees(0.0)), + new Pose2d(Zones.CHARGING_STATION_ROBOT_BALANCE.blueMax, Rotation2d.fromDegrees(0.0)) + ) + ), + INTAKE_TARGET(Zones.LOADING, HPIntakeStations.DOUBLE_STATION_INNER.pose); + + Zones zone; + AlliancePose2d target; + + StaticTargets(Zones zone, Pose2d blue, Pose2d red) { + this.zone = zone; + this.target = new AlliancePose2d(blue, red); + } + + StaticTargets(Zones zone, AlliancePose2d target) { + this.zone = zone; + this.target = target; + } + + public AlliancePose2d getAllianceTarget(Pose2d cur) { + return new AlliancePose2d( + zone.inBlueZone(cur.getTranslation()) ? target.blue : cur, + zone.inRedZone(cur.getTranslation()) ? target.red : cur + ); + } + + public Pose2d getAllianceTarget(Pose2d cur, boolean isRed) { + if (isRed) { + return zone.inRedZone(cur.getTranslation()) ? target.red : cur; + } else { + return zone.inBlueZone(cur.getTranslation()) ? target.blue : cur; + } + } +} diff --git a/src/main/java/frc/robot/constants/enums/TagPositions.java b/src/main/java/frc/robot/constants/enums/TagPositions.java new file mode 100644 index 0000000..8f0183d --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/TagPositions.java @@ -0,0 +1,151 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; + +public enum TagPositions { + OFFICIAL_16H5_TAG_1(1, 610.77, 42.19, Consts.GRID_Z_IN, Consts.RED_YAW_DEG), + OFFICIAL_16H5_TAG_2(2, 610.77, 108.19, Consts.GRID_Z_IN, Consts.RED_YAW_DEG), + OFFICIAL_16H5_TAG_3(3, 610.77, 174.19, Consts.GRID_Z_IN, Consts.RED_YAW_DEG), + OFFICIAL_16H5_TAG_4(4, 636.96, 265.74, Consts.SUBSTATION_Z_IN, Consts.RED_YAW_DEG), + OFFICIAL_16H5_TAG_5(5, 14.25, 265.74, Consts.SUBSTATION_Z_IN, Consts.BLUE_YAW_DEG), + OFFICIAL_16H5_TAG_6(6, 40.45, 174.19, Consts.GRID_Z_IN, Consts.BLUE_YAW_DEG), + OFFICIAL_16H5_TAG_7(7, 40.45, 108.19, Consts.GRID_Z_IN, Consts.BLUE_YAW_DEG), + OFFICIAL_16H5_TAG_8(8, 40.45, 42.19, Consts.GRID_Z_IN, Consts.BLUE_YAW_DEG), + NASA_16H5_TAG_1( + 1, 610.77 + FieldDims.NASA_FIELD_DIMS.xDelta_IN, 42.19, Consts.GRID_Z_IN, Consts.RED_YAW_DEG + ), + NASA_16H5_TAG_2( + 2, 610.77 + FieldDims.NASA_FIELD_DIMS.xDelta_IN, 108.19, Consts.GRID_Z_IN, Consts.RED_YAW_DEG + ), + NASA_16H5_TAG_3( + 3, 610.77 + FieldDims.NASA_FIELD_DIMS.xDelta_IN, 174.19, Consts.GRID_Z_IN, Consts.RED_YAW_DEG + ), + NASA_16H5_TAG_4( + 4, + FieldDims.NASA_FIELD_DIMS.x_IN - Consts.TAG_WIDTH_IN, + 265.74, + Consts.SUBSTATION_Z_IN, + Consts.RED_YAW_DEG + ), + NASA_16H5_TAG_5( + 5, + Consts.TAG_WIDTH_IN, + 265.74 + FieldDims.NASA_FIELD_DIMS.yDelta_IN, + Consts.SUBSTATION_Z_IN, + Consts.BLUE_YAW_DEG + ), + NASA_16H5_TAG_6( + 6, + Consts.TAG_WIDTH_IN, + 174.19 + FieldDims.NASA_FIELD_DIMS.yDelta_IN, + Consts.GRID_Z_IN, + Consts.BLUE_YAW_DEG + ), + NASA_16H5_TAG_7( + 7, + Consts.TAG_WIDTH_IN, + 108.19 + FieldDims.NASA_FIELD_DIMS.yDelta_IN, + Consts.GRID_Z_IN, + Consts.BLUE_YAW_DEG + ), + NASA_16H5_TAG_8( + 8, + Consts.TAG_WIDTH_IN, + 42.19 + FieldDims.NASA_FIELD_DIMS.yDelta_IN, + Consts.GRID_Z_IN, + Consts.BLUE_YAW_DEG + ), + CURIE_16H5_TAG_1(OFFICIAL_16H5_TAG_1), + CURIE_16H5_TAG_2(OFFICIAL_16H5_TAG_2), + CURIE_16H5_TAG_3(OFFICIAL_16H5_TAG_3), + CURIE_16H5_TAG_4(OFFICIAL_16H5_TAG_4), + CURIE_16H5_TAG_5(OFFICIAL_16H5_TAG_5), + CURIE_16H5_TAG_6(OFFICIAL_16H5_TAG_6), + CURIE_16H5_TAG_7(OFFICIAL_16H5_TAG_7), + CURIE_16H5_TAG_8(OFFICIAL_16H5_TAG_8), + SVR_16H5_TAG_1(OFFICIAL_16H5_TAG_1), + SVR_16H5_TAG_2(OFFICIAL_16H5_TAG_2), + SVR_16H5_TAG_3(OFFICIAL_16H5_TAG_3), + SVR_16H5_TAG_4(OFFICIAL_16H5_TAG_4), + SVR_16H5_TAG_5(OFFICIAL_16H5_TAG_5), + SVR_16H5_TAG_6(OFFICIAL_16H5_TAG_6), + SVR_16H5_TAG_7(OFFICIAL_16H5_TAG_7), + SVR_16H5_TAG_8(OFFICIAL_16H5_TAG_8), + CANADA_16H5_TAG_1(OFFICIAL_16H5_TAG_1), + CANADA_16H5_TAG_2(OFFICIAL_16H5_TAG_2), + CANADA_16H5_TAG_3(OFFICIAL_16H5_TAG_3), + CANADA_16H5_TAG_4(OFFICIAL_16H5_TAG_4), + CANADA_16H5_TAG_5(OFFICIAL_16H5_TAG_5), + CANADA_16H5_TAG_6(OFFICIAL_16H5_TAG_6), + CANADA_16H5_TAG_7(OFFICIAL_16H5_TAG_7), + CANADA_16H5_TAG_8(OFFICIAL_16H5_TAG_8), + ARIZONA_16H5_TAG_1(OFFICIAL_16H5_TAG_1), + ARIZONA_16H5_TAG_2(OFFICIAL_16H5_TAG_2), + ARIZONA_16H5_TAG_3(OFFICIAL_16H5_TAG_3), + ARIZONA_16H5_TAG_4(OFFICIAL_16H5_TAG_4), + ARIZONA_16H5_TAG_5(OFFICIAL_16H5_TAG_5), + ARIZONA_16H5_TAG_6(OFFICIAL_16H5_TAG_6), + ARIZONA_16H5_TAG_7(OFFICIAL_16H5_TAG_7), + ARIZONA_16H5_TAG_8(OFFICIAL_16H5_TAG_8), + CHEZY_16H5_TAG_1(OFFICIAL_16H5_TAG_1), + CHEZY_16H5_TAG_2(OFFICIAL_16H5_TAG_2), + CHEZY_16H5_TAG_3(OFFICIAL_16H5_TAG_3), + CHEZY_16H5_TAG_4(OFFICIAL_16H5_TAG_4), + CHEZY_16H5_TAG_5(OFFICIAL_16H5_TAG_5), + CHEZY_16H5_TAG_6(OFFICIAL_16H5_TAG_6), + CHEZY_16H5_TAG_7(OFFICIAL_16H5_TAG_7), + CHEZY_16H5_TAG_8(OFFICIAL_16H5_TAG_8), + CALGAMES_16H5_TAG_1(OFFICIAL_16H5_TAG_1), + CALGAMES_16H5_TAG_2(OFFICIAL_16H5_TAG_2), + CALGAMES_16H5_TAG_3(OFFICIAL_16H5_TAG_3), + CALGAMES_16H5_TAG_4(OFFICIAL_16H5_TAG_4), + CALGAMES_16H5_TAG_5(OFFICIAL_16H5_TAG_5), + CALGAMES_16H5_TAG_6(OFFICIAL_16H5_TAG_6), + CALGAMES_16H5_TAG_7(OFFICIAL_16H5_TAG_7), + CALGAMES_16H5_TAG_8(OFFICIAL_16H5_TAG_8); + + public final int id; + public final AprilTag aprilTag; + + TagPositions(int id, double x, double y, double z, double yaw, double xShift, double yShift) { + this.id = id; + + double x_M = Units.inchesToMeters(x + xShift); + double y_M = Units.inchesToMeters(y + yShift); + double z_M = Units.inchesToMeters(z); + double yaw_RAD = Units.degreesToRadians(yaw); + + aprilTag = new AprilTag(id, new Pose3d(x_M, y_M, z_M, new Rotation3d(0.0, 0.0, yaw_RAD))); + } + TagPositions(int id, double x, double y, double z, double yaw) { + this(id, x, y, z, yaw, 0.0, 0.0); + } + TagPositions(TagPositions tag) { + this(tag, 0.0, 0.0); + } + TagPositions(TagPositions pos, double xShift, double yShift) { + this.id = pos.id; + + this.aprilTag = new AprilTag( + pos.id, + new Pose3d( + pos.aprilTag.pose.getX() + Units.inchesToMeters(xShift), + pos.aprilTag.pose.getY() + Units.inchesToMeters(yShift), + pos.aprilTag.pose.getZ(), + pos.aprilTag.pose.getRotation() + ) + ); + } + + public static final class Consts { + public static final double TAG_WIDTH_IN = 0.25, + TAG_WIDTH_M = Units.inchesToMeters(TAG_WIDTH_IN); + public static final double BLUE_YAW_DEG = 0.0; + public static final double RED_YAW_DEG = 180.0; + public static final double GRID_Z_IN = 18.22, GRID_Z_M = Units.inchesToMeters(GRID_Z_IN); + public static final double SUBSTATION_Z_IN = 27.375, + SUBSTATION_Z_M = Units.inchesToMeters(SUBSTATION_Z_IN); + } +} diff --git a/src/main/java/frc/robot/constants/enums/Zones.java b/src/main/java/frc/robot/constants/enums/Zones.java new file mode 100644 index 0000000..6e8dcfd --- /dev/null +++ b/src/main/java/frc/robot/constants/enums/Zones.java @@ -0,0 +1,158 @@ +package frc.robot.constants.enums; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; + +public enum Zones { + // TODO: give real values for SCORING zone, this is the full field + SCORING( + new Translation2d(0.0, 0.0), + new Translation2d(FieldDims.THEORETICAL_X_M, FieldDims.THEORETICAL_Y_M), + new Translation2d(0.0, 0.0), + new Translation2d(FieldDims.THEORETICAL_X_M, FieldDims.THEORETICAL_Y_M) + ), + ROBOT_COMMUNITY( + new Translation2d( + LocationMath.COMMUNITY_ZONE_BLUE_MIN_X_M + Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.COMMUNITY_ZONE_MIN_Y_M + Constants.CRobot.drive.dims.halfBumperWidth_M + ), + new Translation2d( + LocationMath.COMMUNITY_ZONE_BLUE_MAX_X_M - Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.COMMUNITY_ZONE_MAX_Y_M - Constants.CRobot.drive.dims.halfBumperWidth_M + ), + new Translation2d( + LocationMath.COMMUNITY_ZONE_RED_MIN_X_M + Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.COMMUNITY_ZONE_MIN_Y_M + Constants.CRobot.drive.dims.halfBumperWidth_M + ), + new Translation2d( + LocationMath.COMMUNITY_ZONE_RED_MAX_X_M - Constants.CRobot.drive.dims.halfBumperLength_M, + LocationMath.COMMUNITY_ZONE_MAX_Y_M - Constants.CRobot.drive.dims.halfBumperWidth_M + ) + ), + LOADING( + new Translation2d( + HPIntakeStations.LocationMath.LOADING_ZONE_BLUE_MIN_X_M + + Constants.CRobot.drive.dims.halfBumperLength_M, + HPIntakeStations.LocationMath.LOADING_ZONE_MIN_Y_M + + Constants.CRobot.drive.dims.halfBumperWidth_M + ), + new Translation2d( + HPIntakeStations.LocationMath.LOADING_ZONE_BLUE_MAX_X_M + - Constants.CRobot.drive.dims.halfBumperLength_M, + HPIntakeStations.LocationMath.LOADING_ZONE_MAX_Y_M + - Constants.CRobot.drive.dims.halfBumperWidth_M + ), + new Translation2d( + HPIntakeStations.LocationMath.LOADING_ZONE_RED_MIN_X_M + + Constants.CRobot.drive.dims.halfBumperLength_M, + HPIntakeStations.LocationMath.LOADING_ZONE_MIN_Y_M + + Constants.CRobot.drive.dims.halfBumperWidth_M + ), + new Translation2d( + HPIntakeStations.LocationMath.LOADING_ZONE_RED_MAX_X_M + - Constants.CRobot.drive.dims.halfBumperLength_M, + HPIntakeStations.LocationMath.LOADING_ZONE_MAX_Y_M + - Constants.CRobot.drive.dims.halfBumperWidth_M + ) + ), + CHARGING_STATION_ROBOT_MAX( + new Translation2d( + LocationMath.CHARGING_STATION_BLUE_MIN_X_M + + Constants.CRobot.drive.dims.halfFrameLength_M, + LocationMath.CHARGING_STATION_MIN_Y_M + Constants.CRobot.drive.dims.halfFrameWidth_M + ), + new Translation2d( + LocationMath.CHARGING_STATION_BLUE_MAX_X_M + - Constants.CRobot.drive.dims.halfFrameLength_M, + LocationMath.CHARGING_STATION_MAX_Y_M - Constants.CRobot.drive.dims.halfFrameWidth_M + ), + new Translation2d( + LocationMath.CHARGING_STATION_RED_MIN_X_M + Constants.CRobot.drive.dims.halfFrameLength_M, + LocationMath.CHARGING_STATION_MIN_Y_M + Constants.CRobot.drive.dims.halfFrameWidth_M + ), + new Translation2d( + LocationMath.CHARGING_STATION_RED_MAX_X_M - Constants.CRobot.drive.dims.halfFrameLength_M, + LocationMath.CHARGING_STATION_MAX_Y_M - Constants.CRobot.drive.dims.halfFrameWidth_M + ) + ), + CHARGING_STATION_ROBOT_BALANCE( + new Translation2d( + LocationMath.CHARGING_STATION_CENTER_BLUE_X_M, + LocationMath.CHARGING_STATION_MIN_Y_M + Constants.CRobot.drive.dims.halfFrameWidth_M + ), + new Translation2d( + LocationMath.CHARGING_STATION_CENTER_BLUE_X_M, + LocationMath.CHARGING_STATION_MAX_Y_M - Constants.CRobot.drive.dims.halfFrameWidth_M + ), + new Translation2d( + LocationMath.CHARGING_STATION_CENTER_RED_X_M, + LocationMath.CHARGING_STATION_MIN_Y_M + Constants.CRobot.drive.dims.halfFrameWidth_M + ), + new Translation2d( + LocationMath.CHARGING_STATION_CENTER_RED_X_M, + LocationMath.CHARGING_STATION_MAX_Y_M - Constants.CRobot.drive.dims.halfFrameWidth_M + ) + ); + + public final Translation2d blueMin, blueMax, redMin, redMax; + + Zones(Translation2d blueMin, Translation2d blueMax, Translation2d redMin, Translation2d redMax) { + this.blueMin = blueMin; + this.blueMax = blueMax; + this.redMin = redMin; + this.redMax = redMax; + } + + public boolean inBlueZone(Translation2d cur) { + return inZone(cur, false); + } + + public boolean inRedZone(Translation2d cur) { + return inZone(cur, true); + } + + boolean inZone(Translation2d cur, boolean isRed) { + Translation2d min = isRed ? redMin : blueMin; + Translation2d max = isRed ? redMax : blueMax; + return cur.getX() < max.getX() && cur.getY() < max.getY() && cur.getX() > min.getX() + && cur.getY() > min.getY(); + } + + public static final class LocationMath { + /* ========== COMMUNITY ZONE DIMENSIONS ========== */ + public static final double COMMUNITY_ZONE_MIN_Y_M = Units.inchesToMeters(0.0); + public static final double COMMUNITY_ZONE_MAX_Y_M = Units.inchesToMeters(216.03); + + /* ========== DERIVED COMMUNITY ZONE COORDINATES ========== */ + public static final double COMMUNITY_ZONE_BLUE_MIN_X_M = Units.inchesToMeters(0.0); + public static final double COMMUNITY_ZONE_BLUE_MAX_X_M = + FieldDims.GRID_HARD_STOPS_TO_COMMUNITY_ZONE + FieldDims.GRID_HARD_STOPS_LENGTH_M; + + public static final double COMMUNITY_ZONE_RED_MIN_X_M = + FieldDims.THEORETICAL_X_M - COMMUNITY_ZONE_BLUE_MAX_X_M; + public static final double COMMUNITY_ZONE_RED_MAX_X_M = FieldDims.THEORETICAL_X_M; + + /* ========== DERIVED CHARGING STATION DIMENSIONS ========== */ + public static final double CHARGING_STATION_BLUE_MIN_X_M = + FieldDims.GRID_HARD_STOPS_LENGTH_M + FieldDims.GRID_HARD_STOPS_TO_CHARGE_STATION_M; + public static final double CHARGING_STATION_BLUE_MAX_X_M = + COMMUNITY_ZONE_BLUE_MAX_X_M - FieldDims.TAPE_WIDTH_M; + public static final double CHARGING_STATION_RED_MIN_X_M = + COMMUNITY_ZONE_RED_MIN_X_M + FieldDims.TAPE_WIDTH_M; + public static final double CHARGING_STATION_RED_MAX_X_M = + FieldDims.THEORETICAL_X_M - CHARGING_STATION_BLUE_MIN_X_M; + public static final double CHARGING_STATION_CENTER_BLUE_X_M = + (CHARGING_STATION_BLUE_MIN_X_M + CHARGING_STATION_BLUE_MAX_X_M) / 2; + public static final double CHARGING_STATION_CENTER_RED_X_M = + (CHARGING_STATION_RED_MIN_X_M + CHARGING_STATION_RED_MAX_X_M) / 2; + + public static final double BORDER_TO_CHARGING_STATION_M = Units.inchesToMeters(59.39); + public static final double CHARGING_STATION_TO_DIVIDER_WALL_M = Units.inchesToMeters(59.39); + public static final double CHARGING_STATION_MIN_Y_M = BORDER_TO_CHARGING_STATION_M; + public static final double CHARGING_STATION_MAX_Y_M = + COMMUNITY_ZONE_MAX_Y_M - CHARGING_STATION_TO_DIVIDER_WALL_M; + public static final double CHARING_STATION_CENTER_Y_M = + (CHARGING_STATION_MIN_Y_M + CHARGING_STATION_MAX_Y_M) / 2.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java new file mode 100644 index 0000000..1e5c437 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -0,0 +1,327 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.sensors.CANCoder; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.constants.ClawConfs; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.ClawPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.utils.LoopTimer; + +public class Claw extends SubsystemBase { + /* --- Motors, sensors, and hardware variables --- */ + private DigitalInput _clawBeamBreak; + + private WPI_TalonSRX _primaryClawRollerMotor; + private WPI_TalonSRX _leftClawRollerMotor; + private DoubleSolenoid _clawSolenoid; + + // Motorized Wrist Klaw + // private WPI_TalonFX _wristMotor; + // private WPI_TalonFX _clawRollerMotor; + // private CANCoder _clawCancoder; + + /* --- Configuration Sets --- */ + private TalonSRXConfiguration _rollerMotorsConfig; + private TalonFXConfiguration _wristMotorConfig; + + /* --- State Variables --- */ + private ClawPositions _goalPosition = ClawPositions.CLAW_POSITION_UNKNOWN; + + /* --- Shuffleboard Entries --- */ + private GenericEntry _clawRollersEntry; + private GenericEntry _beamBreakEntry; + + public Claw() { + if (Constants.hasClaw) { + initRollerMotorConfig(); + initWristMotorConifg(); + + _clawBeamBreak = new DigitalInput(Constants.CRobot.claw.ports.breakBeam); + + if (Constants.isKlaw || Constants.isPneumaticClaw) { + _clawSolenoid = new DoubleSolenoid( + Constants.CRobot.air.moduleID, + Constants.CRobot.air.moduleType, + Constants.CRobot.air.clawForward, + Constants.CRobot.air.clawReverse + ); + _primaryClawRollerMotor = new WPI_TalonSRX(Constants.CRobot.claw.ports.leftRoller); + _leftClawRollerMotor = new WPI_TalonSRX(Constants.CRobot.claw.ports.rightRoller); + } + // else { + // _wristMotor = new WPI_TalonFX(Constants.CRobot.claw.ports.); + // _clawCancoder = new CANCoder(ClawPorts.CLAW_CANCODER_ID); + // } + + waitForCAN(); + + configClawMotor(); + configLeftClawRollerMotor(); + configRightClawRollerMotor(); + } + configShuffleboard(); + LoopTimer.markEvent(" Claw Initialization Complete: "); + } + + @Override + public void periodic() { + _beamBreakEntry.setBoolean(getBeamBreak()); + } + + public void disabledPeriodic() {} + + public void setClawPower(double speed) {} + + public int getTargetPosition() { + return _goalPosition.ticks; + } + + public void setTargetPosition(ClawPositions pos) { + if (pos.ticks < ClawPositions.CLAW_OPEN_POS_TICKS.ticks) { + closeClawSolenoid(); + } else { + openClawSolenoid(); + } + } + + public double getClawPower() { + return 0.0; + } + + public int getClawPosition() { + return _goalPosition.ticks; + } + + public void closeClawSolenoid() { + if (Constants.hasClaw && Constants.isPneumaticClaw) { + _goalPosition = ClawPositions.CLAW_MIN_TICKS; + _clawSolenoid.set(ClawConfs.CLAW_SOLENOID_CLOSED); + } + } // close arm + + public void openClawSolenoid() { + if (Constants.hasClaw && Constants.isPneumaticClaw) { + _goalPosition = ClawPositions.CLAW_OPEN_POS_TICKS; + _clawSolenoid.set(ClawConfs.CLAW_SOLENOID_OPEN); + } + } // extend arm + + public void setLeftClawRollerPower(double speed) { + if (Constants.hasClaw) { + _primaryClawRollerMotor.set(speed); + } + } + + public double getLeftClawRollerPower() { + if (Constants.hasClaw) { + return _primaryClawRollerMotor.getMotorOutputPercent(); + } + return 0.0; + } + + public void setRightClawRollerPower(double speed) { + if (Constants.hasClaw) { + _leftClawRollerMotor.set(speed); + } + } + + public double getRightClawRollerPower() { + if (Constants.hasClaw) { + return _leftClawRollerMotor.getMotorOutputPercent(); + } + return 0.0; + } + + public void setClawRollersPower(double speed) { + if (Constants.hasClaw) { + _primaryClawRollerMotor.set(speed); + _leftClawRollerMotor.set(speed); + } + } + + public void setClawRollersStall(GamePieceType piece) { + setClawPower( + piece.isCube() ? -ClawConfs.CLAW_CUBE_STALL_POWER : ClawConfs.CLAW_CONE_STALL_POWER + ); + } + + public double getClawRollersPower() { + if (Constants.hasClaw) { + return _leftClawRollerMotor.getMotorOutputPercent(); + } + return 0.0; + } + + public boolean getBeamBreak() { + if (Constants.hasClaw) { + return !_clawBeamBreak.get(); + } + return true; + } + + public boolean coneDetected(double threshold) { + if (Constants.hasClaw) { + return threshold <= _leftClawRollerMotor.getSupplyCurrent() + || threshold <= _primaryClawRollerMotor.getSupplyCurrent(); + } + return true; + } + + public boolean coneDetected() { + if (Constants.hasClaw) { + return coneDetected(ClawConfs.IntakeSpeed.CONE_DETECTION_CURRENT_THRESHOLD_A); + } + return true; + } + + public boolean inTolerance(int lowerBound, int upperBound) { + return true; + } + + public boolean inTolerance(int tolerance) { + return true; + } + + public boolean motorResetConfig() { + if (Constants.hasClaw) { + if (_leftClawRollerMotor.hasResetOccurred()) { + configRightClawRollerMotor(); + return true; + } + if (_primaryClawRollerMotor.hasResetOccurred()) { + configLeftClawRollerMotor(); + return true; + } + } + return false; + } + + public boolean isExtended() { + return getClawPosition() >= ClawPositions.CLAW_OPEN_POS_THRESHOLD.ticks; + } + + public boolean isRetracted() { + return getClawPosition() <= ClawPositions.CLAW_CLOSED_POS_THRESHOLD.ticks; + } + + /* --- Initialization Functions --- */ + private boolean checkInitStatus() { + if (Constants.hasClaw) { + ErrorCode initStatus = _primaryClawRollerMotor.configFactoryDefault(); + return initStatus == ErrorCode.OK; + } + return true; + } + + private void waitForCAN() { + if (Constants.hasClaw && Robot.isReal()) { + int counter = 0; + while (!checkInitStatus()) { + System.out.println("CLAW Check Init Status : " + counter++); + } + } + } + + private void configShuffleboard() { + ShuffleboardTab tuningTab = Shuffleboard.getTab("Tuning tab"); + + _beamBreakEntry = tuningTab.add("Break Beam", getBeamBreak()).withPosition(1, 0).getEntry(); + } + + private void initRollerMotorConfig() { + if (Constants.hasClaw) { + _rollerMotorsConfig = new TalonSRXConfiguration(); + // _rollerMotorsConfig.slot0.kP = CLAW_ROLLERS_PFAC; + // _rollerMotorsConfig.slot0.kI = CLAW_ROLLERS_IFAC; + // _rollerMotorsConfig.slot0.kD = CLAW_ROLLERS_DFAC; + // _rollerMotorsConfig.slot0.kF = CLAW_ROLLERS_FFAC; + // _rollerMotorsConfig.voltageCompSaturation = 12.0; + _rollerMotorsConfig.openloopRamp = ClawConfs.CLAW_OPEN_LOOP_RAMP; + _rollerMotorsConfig.closedloopRamp = ClawConfs.CLAW_CLOSED_LOOP_RAMP; + _rollerMotorsConfig.peakCurrentLimit = ClawConfs.CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_LIMIT; + _rollerMotorsConfig.peakCurrentDuration = + ClawConfs.CLAW_ROLLERS_SUPPLY_PEAK_CURRENT_DURATION_MS; + _rollerMotorsConfig.continuousCurrentLimit = + ClawConfs.CLAW_ROLLERS_SUPPLY_CONTINUOUS_CURRENT_LIMIT; + } + } + + private void initWristMotorConifg() { + if (Constants.hasClaw) { + _wristMotorConfig = new TalonFXConfiguration(); + } + } + + private void configClawMotor() { + // _wristMotor.configFactoryDefault(); + // TODO + } + + private void configLeftClawRollerMotor() { + if (Constants.hasClaw) { + _leftClawRollerMotor.configFactoryDefault(); + _leftClawRollerMotor.configAllSettings(_rollerMotorsConfig); + _leftClawRollerMotor.setInverted(ClawConfs.LEFT_ROLLERS_INVERTED); + _leftClawRollerMotor.setNeutralMode(ClawConfs.ROLLER_NEUTRAL_MODE); + _leftClawRollerMotor.follow(_primaryClawRollerMotor); + } + } + + private void configRightClawRollerMotor() { + if (Constants.hasClaw) { + _primaryClawRollerMotor.configFactoryDefault(); + _primaryClawRollerMotor.configAllSettings(_rollerMotorsConfig); + _primaryClawRollerMotor.setInverted(ClawConfs.PRIMARY_ROLLERS_INVERTED); + _primaryClawRollerMotor.setNeutralMode(ClawConfs.ROLLER_NEUTRAL_MODE); + } + } + + /* --- Basic Instant Commands --- */ + public CommandBase closeClawCommand() { + return closeClawCommand(false); + } + public CommandBase closeClawCommand(boolean require) { + return require ? runOnce(() -> closeClawSolenoid()) + : Commands.runOnce(() -> closeClawSolenoid()); + } + + public CommandBase openClawCommand() { + return openClawCommand(false); + } + public CommandBase openClawCommand(boolean require) { + return require ? runOnce(() -> openClawSolenoid()) : Commands.runOnce(() -> openClawSolenoid()); + } + + public CommandBase setClawPowerCommand(double speed) { + return setClawPowerCommand(speed, false); + } + public CommandBase setClawPowerCommand(double speed, boolean require) { + return require ? runOnce(() -> setClawPower(speed)) + : Commands.runOnce(() -> setClawPower(speed)); + } + + public CommandBase setClawRollersPowerCommand(double speed) { + return runOnce(() -> setClawRollersPower(speed)); + } + + public CommandBase clawRollersOffCommand() { + return runOnce(() -> setClawRollersPower(0.0)); + } +} diff --git a/src/main/java/frc/robot/subsystems/Controlboard.java b/src/main/java/frc/robot/subsystems/Controlboard.java new file mode 100644 index 0000000..6c0f5d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Controlboard.java @@ -0,0 +1,272 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Ports; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.utils.ControlboardViz; + +public class Controlboard { + public CommandXboxController _xboxDrive = new CommandXboxController(Ports.DRIVER_XBOX_USB_PORT); + // TODO: change xbox op to PS3 or equivalent non-xbox controller + public CommandPS4Controller _ps4Op = new CommandPS4Controller(Ports.OPERATOR_PS4_USB_PORT); + // frc2::CommandXboxController _xboxPit{PIT_XBOX_USB_PORT}; + + private Alliance _alliance; + private ScoringLocations _desiredScoringLocation = Constants.DEFAULT_SCORING_LOCATION; + private ScoringLevels _desiredScoringLevel = Constants.DEFAULT_SCORING_LEVEL; + private frc.robot.constants.enums.HPIntakeStations _desiredIntakeLocation = + Constants.DEFAULT_INTAKE_STATION; + private GamePieceType _desiredGamePiece = Constants.DEFAULT_GAME_PIECE; + private GamePieceType _heldGamePiece = GamePieceType.UNKNOWN_GAME_PIECE; + private StowPositionStrategy _stowStrategy = StowPositionStrategy.PARTIAL_STOW_STRATEGY; + private int _lastOperatorPOVPressed = -1; + + private Field2d _field; + + private boolean VIZCONF_TEXT = true; + private boolean VIZCONF_GLASS = true; + private boolean VIZCONF_FIELD = true; + + // display on text & glass, not on field (which isn't built yet anyway) + ControlboardViz _viz = new ControlboardViz(_field, VIZCONF_TEXT, VIZCONF_GLASS, VIZCONF_FIELD); + + public Controlboard(Field2d field) {} + + public void configShuffleboard() { + updateVisualizer(); + // frc::ShuffleboardTab& controlboardTab = frc::Shuffleboard::GetTab("Competition HUD"); + // _clawPowerEntry = controlboardTab.Add("Cone Shooter Power", "UNKNOWN HELD PIECE").GetEntry(); + } + + public void updateShuffleboard() {} + + public void updateVisualizer() { + _viz.update( + _desiredScoringLocation, + _desiredScoringLevel, + _desiredIntakeLocation, + _desiredGamePiece, + _heldGamePiece + ); + } + + // Left is positive X in terms of field, negative so our controller aligns + public double getDriveX() { + return _alliance == Alliance.Red ? _xboxDrive.getLeftY() : -_xboxDrive.getLeftY(); + } + + public double getDriveY() { + return _alliance == Alliance.Red ? _xboxDrive.getLeftX() : -_xboxDrive.getLeftX(); + } + + public double getRotX() { + return -_xboxDrive.getRightX(); + } + + public double getRotY() { + return _xboxDrive.getRightY(); + } + + public boolean getBackButtonPressed() { + return _xboxDrive.getHID().getBackButtonPressed(); + } + + public void updateAlliance(Alliance alliance) { + _alliance = alliance; + _viz.updateAlliance(alliance); + } + + public boolean isRedAlliance() { + return _alliance == Alliance.Red; + } + + public boolean isPieceInIntake(boolean beamBreak, boolean currentSpike) { + if ((getDesiredGamePiece().isCube() && beamBreak) + || (getDesiredGamePiece() == GamePieceType.CONE_GAME_PIECE && currentSpike)) { + setHeldGamePiece(); + return true; + } else { + return false; + } + } + + public ScoringLocations getDesiredScoringLocation() { + return _desiredScoringLocation; + } + + public ScoringLevels getDesiredScoringLevel() { + return _desiredScoringLevel; + } + + public HPIntakeStations getDesiredIntakeLocation() { + return _desiredIntakeLocation; + } + + public GamePieceType getDesiredGamePiece() { + return _desiredGamePiece; + } + + public void setDesiredGamePiece(GamePieceType piece) { + _desiredGamePiece = piece; + updateVisualizer(); + } + + public GamePieceType getHeldGamePiece() { + return _heldGamePiece; + } + + public void setHeldGamePiece() { + setHeldGamePiece(_desiredGamePiece); + } + + public void setHeldGamePiece(GamePieceType piece) { + _heldGamePiece = piece; + updateVisualizer(); + } + + public void unsetHeldGamePiece() { + setHeldGamePiece(GamePieceType.UNKNOWN_GAME_PIECE); + } + + public StowPositionStrategy getStowStrategy() { + return _stowStrategy; + } + + public void setStowStrategy(StowPositionStrategy strategy) { + _stowStrategy = strategy; + } + + public void toggleStowStrategy() { + _stowStrategy = (_stowStrategy == StowPositionStrategy.FULL_STOW_STRATEGY) + ? StowPositionStrategy.PARTIAL_STOW_STRATEGY + : StowPositionStrategy.FULL_STOW_STRATEGY; + } + + public void driverRumble() { + _xboxDrive.getHID().setRumble(RumbleType.kBothRumble, 1.0); + } + + public void driverResetRumble() { + _xboxDrive.getHID().setRumble(RumbleType.kBothRumble, 0.0); + } + + public void incrementLevel() { + _desiredScoringLevel = _desiredScoringLevel.getIncr(); + updateVisualizer(); + } + + public void decrementLevel() { + _desiredScoringLevel = _desiredScoringLevel.getDecr(); + updateVisualizer(); + } + + public void incrementCol() { + _desiredScoringLocation = _desiredScoringLocation.getIncr(); + updateVisualizer(); + } + + public void decrementCol() { + _desiredScoringLocation = _desiredScoringLocation.getDecr(); + updateVisualizer(); + } + + public void swapDesiredGamePiece() { + _desiredGamePiece = + !_desiredGamePiece.isCube() ? GamePieceType.CUBE_GAME_PIECE : GamePieceType.CONE_GAME_PIECE; + updateVisualizer(); + } + + public void swapIntakeLocation() { + switch (_desiredIntakeLocation) { + case DOUBLE_STATION_INNER: + _desiredIntakeLocation = HPIntakeStations.DOUBLE_STATION_OUTER; + break; + case DOUBLE_STATION_OUTER: + case SINGLE_STATION: + case UNKNOWN_INTAKE_STATION: + default: + _desiredIntakeLocation = HPIntakeStations.DOUBLE_STATION_INNER; + break; + } + _viz.update( + _desiredScoringLocation, + _desiredScoringLevel, + _desiredIntakeLocation, + _desiredGamePiece, + _heldGamePiece + ); + } + + public void updateOperatorSelections() { + int prev = _lastOperatorPOVPressed; + _lastOperatorPOVPressed = getOperatorPOV(); + if (prev == -1) { + switch (_lastOperatorPOVPressed) { + case 0: + incrementLevel(); + break; + case 180: + decrementLevel(); + break; + case 90: + if (_alliance == Alliance.Red) { + incrementCol(); + } else { + decrementCol(); + } + break; + case 270: + if (_alliance == Alliance.Red) { + decrementCol(); + } else { + incrementCol(); + } + break; + default: + break; + } + } + } + + public int getOperatorPOV() { + return _ps4Op.getHID().getPOV(); + } + + public enum StowPositionStrategy { FULL_STOW_STRATEGY, PARTIAL_STOW_STRATEGY } + + public InstantCommand driverRumbleCommand() { + return new InstantCommand(() -> driverRumble()); + } + + public InstantCommand driverResetRumbleCommand() { + return new InstantCommand(() -> driverResetRumble()); + } + + public CommandBase swapDesiredGamePieceCommand() { + return new InstantCommand(() -> swapDesiredGamePiece()).ignoringDisable(true); + } + + public CommandBase swapIntakeLocationCommand() { + return new InstantCommand(() -> swapIntakeLocation()).ignoringDisable(true); + } + + public InstantCommand setHeldGamePieceCommand() { + return new InstantCommand(() -> setHeldGamePiece()); + } + + public InstantCommand unsetHeldGamePieceCommand() { + return new InstantCommand(() -> unsetHeldGamePiece()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..c8237b7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -0,0 +1,1301 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.sensors.Pigeon2Configuration; +import com.ctre.phoenix.sensors.WPI_Pigeon2; +import com.team254.lib.swerve.SwerveDriveKinematics; +import com.team254.lib.swerve.SwerveSetpoint; +import com.team254.lib.swerve.SwerveSetpointGenerator; +import com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.*; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.GenericPublisher; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Ports.DrivetrainPorts; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.Constants.Control; +import frc.robot.constants.Constants.Sensors; +import frc.robot.constants.DrivetrainConfs; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.*; +import frc.robot.constants.enums.DriveModes; +import frc.robot.utils.AlliancePose2d; +import frc.robot.utils.InputUtils; +import frc.robot.utils.LoopTimer; +import frc.robot.utils.MultiCameraController; + +public class Drivetrain extends SubsystemBase { + /* --- Constant configuration shortcuts --- */ + private final DrivetrainConfs DRIVE_CONSTS = Constants.CRobot.drive; + private boolean disabled = DRIVE_CONSTS == null; + private final DrivetrainDimensions DRIVE_DIMS = disabled ? null : DRIVE_CONSTS.dims; + private final DrivetrainControl DRIVE_CONTROL = disabled ? null : DRIVE_CONSTS.control; + private final DrivetrainPorts DRIVE_PORTS = disabled ? null : DRIVE_CONSTS.ports; + + private boolean wantOrientationMode = false; + private final boolean USE_POSE_ESTIMATION_ANGLE = false; // change if true? + + /* --- Sensors, motors, and hardware --- */ + MultiCameraController _multiCameraController; + private WPI_Pigeon2 _gyro = new WPI_Pigeon2(DRIVE_PORTS.pigeonID, DRIVE_PORTS.pigeonCanBus); + private Pigeon2Configuration _gyroConfigs; + + private SwerveModule[] _modules = new SwerveModule[] { + new SwerveModule(0, DRIVE_PORTS.moduleCanBus), + new SwerveModule(1, DRIVE_PORTS.moduleCanBus), + new SwerveModule(2, DRIVE_PORTS.moduleCanBus), + new SwerveModule(3, DRIVE_PORTS.moduleCanBus)}; + + /* --- State and Physical Property variables --- */ + private ChassisSpeeds _chassisSpeeds = new ChassisSpeeds(); + // Initialize with real values regardless of canbus state to ensure good launch to pose + // estimator + private SwerveModulePosition[] _modulePositions = new SwerveModulePosition[] { + _modules[0].getPosition(), + _modules[1].getPosition(), + _modules[2].getPosition(), + _modules[3].getPosition()}; + + /* --- Odometry Utils --- */ + // TODO: move to configurations + private edu.wpi.first.math.kinematics.SwerveDriveKinematics _swerveKinematics = + new edu.wpi.first.math.kinematics.SwerveDriveKinematics( + new Translation2d(DRIVE_DIMS.halfTrackLength_M, DRIVE_DIMS.halfTrackWidth_M), + new Translation2d(DRIVE_DIMS.halfTrackLength_M, -DRIVE_DIMS.halfTrackWidth_M), + new Translation2d(-DRIVE_DIMS.halfTrackLength_M, DRIVE_DIMS.halfTrackWidth_M), + new Translation2d(-DRIVE_DIMS.halfTrackLength_M, -DRIVE_DIMS.halfTrackWidth_M) + ); + + SwerveDrivePoseEstimator _robotPoseEstimator = new SwerveDrivePoseEstimator( + _swerveKinematics, Rotation2d.fromDegrees(0.0), _modulePositions, new Pose2d() + ); + + /* --- Control Utils --- */ + private ProfiledPIDController _xController = DRIVE_CONTROL.getTranslationaProfiledPIDController(); + private ProfiledPIDController _yController = DRIVE_CONTROL.getTranslationaProfiledPIDController(); + private ProfiledPIDController _angleController = DRIVE_CONTROL.getAngularProfiledPIDController(); + + private SlewRateLimiter _xSlewRateFilter = + DRIVE_CONTROL.slewingLimits.getTranslationalSlewRateLimiter(); + private SlewRateLimiter _ySlewRateFilter = + DRIVE_CONTROL.slewingLimits.getTranslationalSlewRateLimiter(); + private SlewRateLimiter _angleSlewRateFilter = + DRIVE_CONTROL.slewingLimits.getAngularSlewRateLimiter(); + + /* --- Game State variables --- */ + private Field2d _field; + private Alliance _alliance; + private boolean _isAuto = false; + private boolean _autoPrepScore = false; + + /* --- Simulation resources and variables --- */ + private Pose2d _simPose = new Pose2d(); + + /* --- Logging variables --- */ + private double[] _desiredSwerveStates = new double[DRIVE_CONSTS.numModules * 2]; + private double[] _entry = new double[2 * DRIVE_CONSTS.numModules]; + + /* --- Shuffleboard entries --- */ + private SendableChooser _staticTargetChooser = + new SendableChooser(); + private GenericEntry _desiredSpeed, _actualSpeed; + private GenericEntry _actualSpeedX, _actualSpeedY, _actualSpeedTheta; + private GenericEntry _desiredRobotTheta, _actualRobotTheta, _errorRobotTheta; + + private GenericPublisher _xPoseError, _yPoseError, _thetaPoseError; + private GenericEntry _usingPoseTuning; + + private GenericEntry _anglePFac, _angleIFac, _angleDFac; + private GenericEntry _xPFac, _xIFac, _xDFac; + private GenericEntry _yPFac, _yIFac, _yDFac; + + private GenericEntry _steerTargetAngle, _steerErrorAngle; + private GenericEntry _steerAnglePFac, _steerAngleIFac, _steerAngleDFac; + private GenericEntry _steerPFac, _steerIFac, _steerDFac; + + private Pose2d TARGET_RELATIVE_POSE = new Pose2d(0.0, 1.0, Rotation2d.fromDegrees(0.0)); + + public Drivetrain(Field2d field) { + _field = field; + + resetDefaultTolerance(); + _angleController.enableContinuousInput(0.0, Units.degreesToRadians(360.0)); + + if (Robot.isReal()) { + int counter = 0; + while (!checkInitStatus()) { + System.out.println("DRIVETRAIN Check Init Status : " + counter++); + } + } else { + } + + _gyro.configFactoryDefault(); + zeroGyro(); + configShuffleboard(); + // initialize all cameras + _multiCameraController = new MultiCameraController(_field, _robotPoseEstimator); + + if (RobotAltModes.isPoseTuning) { + // _xPoseError; + // _yPoseEntry; + // _thetaPoseError; + // _usingPoseTuning; + } + + if (RobotAltModes.isPIDTuningMode) { + // _anglePFac; + // _angleIFac; + // _angleDFac; + // _xPFac; + // _xIFac; + // _xDFac; + // _yPFac; + // _yIFac; + // _yDFac; + // _steerTargetAngle + // _steerAnglePFac; + // _steerAngleIFac; + // _steerAngleDFac; + // _steerPFac; + // _steerIFac; + // _steerDFac; + } + + LoopTimer.markEvent(" Drivetrain Initialization Complete: "); + } + + @Override + public void periodic() { + LoopTimer.markLoopStart(); + + // get module positions ensures the order of data beign collected + _robotPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), getYaw(), getModulePositions()); + + LoopTimer.markEvent(" PoseEstimator "); + + _multiCameraController.updateOdom(_isAuto); + + LoopTimer.markEvent(" Vision "); + + updateShuffleboard(); + + LoopTimer.markCompletion(" Drivetrain Shuffleboard ", "\n Total Drivetrain "); + + // TODO: call an update function and pre-construct the array + _chassisSpeeds = _swerveKinematics.toChassisSpeeds( + _modules[0].getState(), + _modules[1].getState(), + _modules[2].getState(), + _modules[3].getState() + ); + } + + public void updateModulePositions() { + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) { + _modulePositions[i] = _modules[i].getPosition(); + } + } + + public SwerveModulePosition[] getModulePositions() { + updateModulePositions(); + return _modulePositions; + } + + public void enableAuto() { + _isAuto = true; + } + + public void disableAuto() { + _isAuto = false; + } + + // public void initGyroConfigs() { + // _gyroConfigs = new Pigeon2Configuration(); + // } + + // private void configGyro() {} + + public void configShuffleboard() { + ShuffleboardTab tab = Shuffleboard.getTab("SwerveModules"); + _steerTargetAngle = tab.add("target angle", 0.0).getEntry(); + + ShuffleboardTab drivetrainTab = Shuffleboard.getTab("Drivetrain"); + + if (RobotAltModes.isPoseTuning) { + ShuffleboardLayout poseLayout = drivetrainTab.getLayout("Go To Pose", BuiltInLayouts.kGrid) + .withSize(1, 2) + .withPosition(7, 0); + + _usingPoseTuning = poseLayout.add("using pose tuning", false).withPosition(0, 0).getEntry(); + _xPoseError = poseLayout.add("x Pose Error", 0.0).withPosition(0, 1).getEntry(); + _yPoseError = poseLayout.add("y Pose Error", 0.0).withPosition(0, 2).getEntry(); + _thetaPoseError = poseLayout.add("theta Pose Error", 0.0).withPosition(0, 3).getEntry(); + } + + if (RobotAltModes.isPIDTuningMode) { + int PIDCOL = 3; + int PIDROW = 1; + + _errorRobotTheta = drivetrainTab.add("Error Angle", 0.0).getEntry(); + ShuffleboardLayout anglePIDLayout = drivetrainTab.getLayout("Angle PID", BuiltInLayouts.kGrid) + .withSize(1, 2) + .withPosition(PIDCOL, PIDROW); + _anglePFac = anglePIDLayout.add("P", DRIVE_CONTROL.theta.p).withPosition(0, 0).getEntry(); + _angleIFac = anglePIDLayout.add("I", DRIVE_CONTROL.theta.i).withPosition(0, 1).getEntry(); + _angleDFac = anglePIDLayout.add("D", DRIVE_CONTROL.theta.d).withPosition(0, 2).getEntry(); + + ShuffleboardLayout xLayout = drivetrainTab.getLayout("X PID", BuiltInLayouts.kGrid) + .withSize(1, 2) + .withPosition(PIDCOL + 1, PIDROW); + _xPFac = xLayout.add("P", DRIVE_CONTROL.xy.p).withPosition(0, 0).getEntry(); + _xIFac = xLayout.add("I", DRIVE_CONTROL.xy.i).withPosition(0, 1).getEntry(); + _xDFac = xLayout.add("D", DRIVE_CONTROL.xy.d).withPosition(0, 2).getEntry(); + + ShuffleboardLayout yLayout = drivetrainTab.getLayout("Y PID", BuiltInLayouts.kGrid) + .withSize(1, 2) + .withPosition(PIDCOL + 2, PIDROW); + _yPFac = yLayout.add("P", DRIVE_CONTROL.xy.p).withPosition(0, 0).getEntry(); + _yIFac = yLayout.add("I", DRIVE_CONTROL.xy.i).withPosition(0, 1).getEntry(); + _yDFac = yLayout.add("D", DRIVE_CONTROL.xy.d).withPosition(0, 2).getEntry(); + } + + ShuffleboardTab controlboardTab = Shuffleboard.getTab("Competition HUD"); + controlboardTab.add("Field", _field).withSize(11, 5).withPosition(1, 1); + } + + public void updateShuffleboard() { + for (SwerveModule module : _modules) { + module.updateShuffleboard(); + } + + // _actualAngle->SetDouble(ToAbsoluteAngle(GetYaw())); + + // _actualSpeedX->SetDouble(_chassisSpeeds.vx.value()); + // _actualSpeedY->SetDouble(_chassisSpeeds.vy.value()); + // _actualAngularVelocity->SetDouble(_chassisSpeeds.omega.value()); + + if (RobotAltModes.isVisionMode) { + _multiCameraController.updateShuffleboard(); + } + + _field.setRobotPose(_robotPoseEstimator.getEstimatedPosition()); + + if (RobotAltModes.isPIDTuningMode) { + _errorRobotTheta.setDouble(Units.degreesToRadians(_angleController.getPositionError())); + } + } + + public void simulationInit() { + if (RobotAltModes.isSim) { + for (SwerveModule module : _modules) module.simulationInit(); + } + } + + public void simulationPeriodic() { + if (RobotAltModes.isSim) { + for (SwerveModule module : _modules) module.simulationPeriodic(); + + _simPose = _simPose.transformBy(new Transform2d( + new Translation2d( + _chassisSpeeds.vxMetersPerSecond * Constants.LOOP_PERIOD_S, + _chassisSpeeds.vyMetersPerSecond * Constants.LOOP_PERIOD_S + ), + Rotation2d.fromRadians(_chassisSpeeds.omegaRadiansPerSecond * Constants.LOOP_PERIOD_S) + )); + + _gyro.getSimCollection().setRawHeading(_simPose.getRotation().getDegrees()); + } + } + + public boolean isRedAlliance() { + return _alliance == Alliance.Red; + } + + public Rotation2d getYaw() { + return Sensors.INVERT_GYRO ? Rotation2d.fromDegrees(360.0 - _gyro.getYaw()) + : Rotation2d.fromDegrees(_gyro.getYaw()); + } + + public Rotation2d getPitch() { + return Rotation2d.fromDegrees(_gyro.getPitch()); + } + + public Rotation2d getRoll() { + return Rotation2d.fromDegrees(_gyro.getRoll()); + } + + public void getYawPitchRoll(double[] ypr) { + _gyro.getYawPitchRoll(ypr); + } + + public Pose2d getPose() { + return _robotPoseEstimator.getEstimatedPosition(); + } + + public double[] swerveMeasuredIO() { + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) { + SwerveModuleState moduleState = _modules[i].getState(); + _entry[i * 2] = moduleState.angle.getDegrees(); + _entry[(i * 2) + 1] = moduleState.speedMetersPerSecond; + } + return _entry; + } + + public double[] swerveSetpointsIO() { + return _desiredSwerveStates; + } + + public void setDesiredSwerveState(SwerveModuleState[] goalModuleStates) { + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) { + SwerveModuleState state = goalModuleStates[i]; + _desiredSwerveStates[i * 2] = state.angle.getDegrees(); + _desiredSwerveStates[(i * 2) + 1] = state.speedMetersPerSecond; + } + } + + public SwerveDrivePoseEstimator getPoseEstimator() { + return _robotPoseEstimator; + } + + public edu.wpi.first.math.kinematics.SwerveDriveKinematics getSwerveKinematics() { + return _swerveKinematics; + } + + public boolean areWheelsAligned(SwerveModuleState[] goalStates) { + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) { + if (!_modules[i].isAlignedTo(goalStates[i])) + return false; + } + return true; + } + + public boolean areWheelsAligned(SwerveModuleState goalState) { + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) { + if (!_modules[i].isAlignedTo(goalState)) + return false; + } + return true; + } + + public void resetModulesToAbsolute() { + for (SwerveModule module : _modules) { + module.resetToAbsolute(); + } + } + + public void alignModulesToZero() { + for (SwerveModule module : _modules) { + module.alignToZero(); + } + } + + public void updateAlliance(Alliance alliance) { + _alliance = alliance; + } + + public Rotation2d allianceScoreAngle() { + return ScoringLocations.getScoreAngle(_alliance); + } + + public double POVZeroOffsetDeg() { + return _alliance == Alliance.Red ? Constants.Sensors.POV_ZERO_RED_DEG + : Constants.Sensors.POV_ZERO_BLUE_DEG; + } + + public Rotation2d allianceGyroAngle() { + return _alliance == Alliance.Red ? Constants.Sensors.GYRO_ZERO_RED + : Constants.Sensors.GYRO_ZERO_BLUE; + } + + public void zeroGyro() { + setPose(_robotPoseEstimator.getEstimatedPosition(), allianceGyroAngle()); + } + + public void zeroPose() { + setPose(new Pose2d()); + } + + public void autoZeroGyro() { + setPose( + _robotPoseEstimator.getEstimatedPosition(), + getYaw().plus(Rotation2d.fromDegrees(POVZeroOffsetDeg())) + ); + } + + public void setPose(Pose2d pose, Rotation2d yaw) { + _gyro.setYaw(yaw.getDegrees()); + _robotPoseEstimator.resetPosition( + yaw, _modulePositions, new Pose2d(pose.getTranslation(), yaw) + ); + } + + public void setPose(Pose2d pose) { + Rotation2d yaw = getYaw(); + _robotPoseEstimator.resetPosition( + yaw, _modulePositions, new Pose2d(pose.getTranslation(), yaw) + ); + } + + // TODO: implement + public void setGyroAndPose(double yaw, Pose2d pose) {} + + public boolean motorResetConfig() { + for (SwerveModule module : _modules) { + if (module.motorResetConfig()) + return true; + } + return false; + } + + public void setAutoPrepScore(boolean enable) { + _autoPrepScore = enable; + } + + public boolean getAutoPrepScore() { + return _autoPrepScore; + } + + /* Used by SwerveControllerCommand in Auto */ + public void setModuleStates(SwerveModuleState[] desiredStates, double maxSpeed) { + // Desaturate based of max theoretical or functional rather than current max + edu.wpi.first.math.kinematics.SwerveDriveKinematics.desaturateWheelSpeeds( + desiredStates, DRIVE_CONSTS.model.theoreticalMaxWheelSpeed + ); + + for (SwerveModule module : _modules) { + module.setDesiredState(desiredStates[module._moduleNumber], Constants.Control.IS_OPEN_LOOP); + } + + setDesiredSwerveState(desiredStates); + } + + public void setModuleStates(SwerveModuleState[] desiredStates) { + setModuleStates(desiredStates, DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS); + } + + public void drive( + double joystickX, + double joystickY, + double compositeXY, + double joystickTheta, + DriveModes mode, + double maxSpeed_MPS, + Rotation2d maxAngularSpeed + ) { + switch (mode) { + case ROBOT_CENTRIC: + robotCentricDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed) + ); + break; + case FIELD_RELATIVE: + // fieldRelative254( + fieldRelativeDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed) + + ); + break; + case FIELD_RELATIVE_ROTATION_COMPENSATION_SCALING: + rotationCompensationFieldRelativeDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, 1.0), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, 1.0), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, 1.0) + ); + case SNAP_TO_ANGLE: + snapToAngleDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS) + ); + break; + case SNAKE: + snakeDrive(joystickX, joystickY, compositeXY, maxSpeed_MPS); + break; + case TARGET_RELATIVE: + targetCentricDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed), + new Translation2d(Constants.CField.dims.halfX_M, Constants.CField.dims.halfY_M), + maxSpeed_MPS + ); + break; + case CHASE_STATIC_TARGET: + chaseStaticTargetDrive(maxSpeed_MPS); + break; + case SLEWING_FIELD_RELATIVE: + slewingFieldRelativeDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed) + ); + break; + case FIELD_RELATIVE_SKEW_COMPENSATION: + fieldRelativeDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed) + ); + break; + case FIELD_RELATIVE_ANTI_TIP: + slewingFieldRelativeDrive( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed) + ); + break; + case CHASE_DYNAMIC_TARGET: + // TODO update this function to use the AprilTag layout + // ChaseDynamicTargetDrive(FIELD_TO_TARGET, TARGET_RELATIVE_POSE, true, maxSpeed); + // ChaseDynamicTargetDrive(FIELD_TO_TARGET, TARGET_RELATIVE_POSE, _hasTarget); + break; + case FIELD_RELATIVE_254: + fieldRelative254( + InputUtils.scaleJoystickX_MPS(joystickX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(joystickY, compositeXY, maxSpeed_MPS), + InputUtils.ScaleJoystickTheta_RADPS(joystickTheta, maxAngularSpeed) + ); + break; + default: + // ERROR + // throw, error message, or default behavior + // return; + break; + } + } + + public void drive(double joystickX, double joystickY, double joystickTheta) { + drive( + joystickX, + joystickY, + Math.hypot(joystickX, joystickY), + joystickTheta, + DriveModes.FIELD_RELATIVE + ); + } + + public void drive(double joystickX, double joystickY, double joystickTheta, DriveModes mode) { + drive(joystickX, joystickY, Math.hypot(joystickX, joystickY), joystickTheta, mode); + } + + public void drive(double joystickX, double joystickY, double compositeXY, double joystickTheta) { + drive(joystickX, joystickY, compositeXY, joystickTheta, DriveModes.FIELD_RELATIVE); + } + + public void drive( + double joystickX, double joystickY, double compositeXY, double joystickTheta, DriveModes mode + ) { + drive( + joystickX, + joystickY, + compositeXY, + joystickTheta, + mode, + DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void drive( + double joystickX, + double joystickY, + double compositeXY, + double joystickTheta, + DriveModes mode, + double maxSpeed_MPS + ) { + drive( + joystickX, + joystickY, + compositeXY, + joystickTheta, + mode, + maxSpeed_MPS, + DRIVE_CONTROL.defaultLimits.maxAngularVelocityPS + ); + } + + // individual drive functions + public void robotCentricDrive( + double translationX_MPS, double translationY_MPS, double rotation_RADPS, double maxSpeed + ) { + SwerveModuleState[] goalModuleStates = + _swerveKinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds( + translationX_MPS, translationY_MPS, rotation_RADPS, getYaw() + )); + + // Desaturate based of max theoretical or functional rather than current max + edu.wpi.first.math.kinematics.SwerveDriveKinematics.desaturateWheelSpeeds( + goalModuleStates, DRIVE_CONSTS.model.theoreticalMaxWheelSpeed + ); + + for (SwerveModule module : _modules) { + module.setDesiredState( + goalModuleStates[module._moduleNumber], Constants.Control.IS_OPEN_LOOP + ); + } + setDesiredSwerveState(goalModuleStates); + } + + public void robotCentricDrive( + double translationX_MPS, double translationY_MPS, double rotation_RADPS + ) { + robotCentricDrive( + translationX_MPS, + translationY_MPS, + rotation_RADPS, + DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public boolean isOversaturated(SwerveModuleState[] states) { + for (SwerveModuleState state : states) { + if (state.speedMetersPerSecond > DRIVE_CONSTS.model.theoreticalMaxWheelSpeed) + return true; + } + return false; + } + + public void fieldRelativeDrive( + double translationX_MPS, double translationY_MPS, double rotation_RADPS + ) { + SwerveModuleState[] goalModuleStates = + _swerveKinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds( + translationX_MPS, + translationY_MPS, + rotation_RADPS, + USE_POSE_ESTIMATION_ANGLE ? _robotPoseEstimator.getEstimatedPosition().getRotation() + : getYaw() + )); + + // Desaturate based of max theoretical or functional rather than current max + edu.wpi.first.math.kinematics.SwerveDriveKinematics.desaturateWheelSpeeds( + goalModuleStates, DRIVE_CONSTS.model.theoreticalMaxWheelSpeed + ); + for (SwerveModule module : _modules) { + module.setDesiredState( + goalModuleStates[module._moduleNumber], Constants.Control.IS_OPEN_LOOP + ); + } + // _desiredSpeed.setDouble(goalModuleStates[0].speedMetersPerSecond); + setDesiredSwerveState(goalModuleStates); + } + + public void rotationCompensationFieldRelativeDrive( + double percentX, double percentY, double percentR + ) { + // args + double configuredMaxTranslationalSpeed = 0; + double configuredMaxRotationalSpeed = 0; + + // consts + // these need to be robot relative if guaranteeing the angle + ChassisSpeeds robotRelative = + ChassisSpeeds.fromFieldRelativeSpeeds(percentX, percentY, percentR, getYaw()); + // robotRelative. + double scaledX = robotRelative.vxMetersPerSecond * configuredMaxTranslationalSpeed; + double scaledY = robotRelative.vyMetersPerSecond * configuredMaxTranslationalSpeed; + double percentXY = Math.hypot(percentX, percentY); + double scaledXY = percentXY * configuredMaxTranslationalSpeed; + double absScaledX = Math.abs(scaledX); + double absScaledY = Math.abs(scaledY); + double scaledR = robotRelative.omegaRadiansPerSecond * configuredMaxRotationalSpeed; + double rWheel = + scaledR * configuredMaxTranslationalSpeed / DRIVE_CONSTS.theoreticalMaxRotationalSpeed; + double projected = + (Math.min(absScaledX, absScaledY) * 2 + Math.abs(absScaledX - absScaledY)) / Math.sqrt(2.0); + + // We should rescale + if (projected + rWheel > DRIVE_CONSTS.theoreticalMaxTranslationSpeed) { + // // projected oversaturation values (using projected vector value) + // // no because this is dependent on the direction of translation + // // which will change with constant input + // double maxOverSaturation = + // (-1 + (configuredMaxRotationalSpeed / DRIVE_CONSTS.theoreticalMaxRotationalSpeed) + // + (configuredMaxTranslationalSpeed / DRIVE_CONSTS.theoreticalMaxTranslationSpeed)); + // double overSaturation = + // -1 + (projected + rWheel) / DRIVE_CONSTS.theoreticalMaxTranslationSpeed; + // double scalingCoefficient = overSaturation / maxOverSaturation; + + // unprojected value + double scalingCoefficient = -1 + (scaledXY / DRIVE_CONSTS.theoreticalMaxTranslationSpeed) + + (scaledR / DRIVE_CONSTS.theoreticalMaxRotationalSpeed); + double newScaledXY = scaledXY + + (DRIVE_CONSTS.theoreticalMaxTranslationSpeed - configuredMaxTranslationalSpeed) + * percentXY * scalingCoefficient; + double newScaledR = + scaledR + (DRIVE_CONSTS.theoreticalMaxRotationalSpeed - configuredMaxRotationalSpeed); + scaledX = scaledX * newScaledXY / scaledXY; + scaledY = scaledY * newScaledXY / scaledXY; + scaledR = newScaledR; + } + + fieldRelativeDrive(scaledX, scaledY, scaledR); + } + + public void slewingFieldRelativeDrive( + double translationX_MPS, double translationY_MPS, double rotation_RADPS + ) { + fieldRelativeDrive( + _xSlewRateFilter.calculate(translationX_MPS), + _ySlewRateFilter.calculate(translationY_MPS), + _angleSlewRateFilter.calculate(rotation_RADPS) + ); + } + + public void snapToAngleDrive( + double translationX_MPS, double translationY_MPS, double maxSpeed_MPS + ) { + fieldRelativeDrive( + translationX_MPS, + translationY_MPS, + // calculate snap to angle rotation here + // TODO: reconfigure deadband logic to limit jittering + _angleController.atGoal() ? 0.0 : _angleController.calculate(getYaw().getRadians()) + ); + } + + public void snapToAngleDrive(double translationX_MPS, double translationY_MPS) { + snapToAngleDrive( + translationX_MPS, translationY_MPS, DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void snakeDrive( + double controllerX, double controllerY, double compositeXY, double maxSpeed_MPS + ) { + if (compositeXY > Control.STICK_NET_DEADBAND) { + _angleController.reset( + getPose().getRotation().getRadians(), _chassisSpeeds.omegaRadiansPerSecond + ); + if (controllerX == 0.0) { + _angleController.setGoal( + controllerY > 0.0 ? Units.degreesToRadians(90.0) : Units.degreesToRadians(270.0) + ); + } else { + _angleController.setGoal(Math.atan2(controllerY, controllerX)); + } + } + + snapToAngleDrive( + InputUtils.scaleJoystickX_MPS(controllerX, compositeXY, maxSpeed_MPS), + InputUtils.scaleJoystickY_MPS(controllerY, compositeXY, maxSpeed_MPS), + maxSpeed_MPS + // InputUtils.scaleJoystickY_MPS(controllerY, compositeXY, maxSpeed_MPS), + // DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void snakeDrive(double controllerX, double controllerY) { + snakeDrive(controllerX, controllerY, Math.hypot(controllerX, controllerY)); + } + + public void snakeDrive(double translationX_MPS, double translationY_MPS, double compositeXY) { + snakeDrive( + translationX_MPS, + translationY_MPS, + compositeXY, + DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void fieldRelativeDriveCompensation( + double translationX_MPS, double translationY_MPS, double rotation_RADPS, double maxSpeed_MPS + ) { + double x_meter = translationX_MPS * Constants.LOOP_PERIOD_S; + double y_meter = translationY_MPS * Constants.LOOP_PERIOD_S; + double theta_deg = rotation_RADPS * Constants.LOOP_PERIOD_S; + Pose2d goal = new Pose2d(x_meter, y_meter, Rotation2d.fromDegrees(theta_deg)); + Twist2d twistTranslation = new Pose2d().log(goal); + fieldRelativeDrive( + twistTranslation.dx / Constants.LOOP_PERIOD_S, + twistTranslation.dy / Constants.LOOP_PERIOD_S, + twistTranslation.dtheta / Constants.LOOP_PERIOD_S + ); + } + + public void fieldRelativeDriveCompensation( + double translationX_MPS, double translationY_MPS, double rotation_RADPS + ) { + fieldRelativeDriveCompensation( + translationX_MPS, + translationY_MPS, + rotation_RADPS, + DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void setTolerance(double xTolerance, double yTolerance, Rotation2d thetaTolerance) { + _xController.setTolerance(xTolerance); + _yController.setTolerance(yTolerance); + _angleController.setTolerance(thetaTolerance.getRadians()); + } + + public void resetDefaultTolerance() { + setTolerance( + DRIVE_CONTROL.xy.toleranceM, DRIVE_CONTROL.xy.toleranceM, DRIVE_CONTROL.theta.tolerance + ); + } + + /* + * @param translationX, translationY apply to entire robot + * @param rotation measured from center of rotation + * @param target point of rotation + * @param maxSpeed + */ + public void targetCentricDrive( + double translationX, + double translationY, + double rotation, + Translation2d target, + double maxSpeed + ) { + SwerveModuleState[] goalModuleStates = _swerveKinematics.toSwerveModuleStates( + new ChassisSpeeds(translationX, translationY, rotation), target + ); + + // Desaturate based of max theoretical or functional rather than current max + edu.wpi.first.math.kinematics.SwerveDriveKinematics.desaturateWheelSpeeds( + goalModuleStates, DRIVE_CONSTS.model.theoreticalMaxWheelSpeed + ); + + for (SwerveModule module : _modules) { + module.setDesiredState( + goalModuleStates[module._moduleNumber], Constants.Control.IS_OPEN_LOOP + ); + } + setDesiredSwerveState(goalModuleStates); + } + + public void chaseStaticTargetDrive(double maxSpeed_MPS) { + Pose2d currentPose = getPose(); + + if (RobotAltModes.isUnprofiledPIDMode) { + // TODO Test/replace with slewing drive (?) + fieldRelativeDrive( + _xController.atSetpoint() ? 0.0 : _xController.calculate(currentPose.getX()), + _yController.atSetpoint() ? 0.0 : _yController.calculate(currentPose.getY()), + _angleController.atSetpoint() ? 0.0 : _angleController.calculate(getYaw().getRadians()) + ); + } else { + // Profiled controllers naturally filter/slew + if (RobotAltModes.isPoseTuning) { + _xPoseError.setDouble(_xController.getPositionError()); + _yPoseError.setDouble(_yController.getPositionError()); + _thetaPoseError.setDouble(Units.radiansToDegrees(_angleController.getPositionError())); + } + + fieldRelativeDrive( + _xController.calculate(currentPose.getX()), + _yController.calculate(currentPose.getY()), + _angleController.calculate(getYaw().getRadians()) + ); + } + } + + public void chaseStaticTargetDrive() { + chaseStaticTargetDrive(DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS); + } + + public void forceWheelChaseStaticTargetDrive(double maxSpeed_MPS) { + Pose2d currentPose = getPose(); + + if (RobotAltModes.isUnprofiledPIDMode) { + // TODO Test/replace with slewing drive (?) + forceWheelDirection( + _xController.atSetpoint() ? 0 : _xController.calculate(currentPose.getX()), + _yController.atSetpoint() ? 0 : _yController.calculate(currentPose.getY()), + _angleController.atSetpoint() ? 0 : _angleController.calculate(getYaw().getRadians()), + maxSpeed_MPS + ); + } else { + // Profiled controllers naturally filter/slew + forceWheelDirection( + _xController.calculate(currentPose.getX()), + _yController.calculate(currentPose.getY()), + _angleController.calculate(getYaw().getRadians()), + maxSpeed_MPS + ); + } + } + + public void forceWheelChaseStaticTargetDrive() { + forceWheelChaseStaticTargetDrive(DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS); + } + + public AlliancePose2d getStaticTargetPose() { + return _staticTargetChooser.getSelected().getAllianceTarget(getPose()); + } + + public Pose2d getStaticTargetPose(boolean isRed) { + return _staticTargetChooser.getSelected().getAllianceTarget(getPose(), isRed); + } + + public void chaseDynamicTargetDrive( + Pose2d visionTarget, Pose2d relativeGoal, boolean targetMoved, double maxSpeed_MPS + ) { + if (targetMoved) { + Transform2d goalPose = visionTarget.minus(relativeGoal); + + if (RobotAltModes.isUnprofiledPIDMode) { + _xController.reset(goalPose.getX()); + _yController.reset(goalPose.getY()); + _angleController.reset(goalPose.getRotation().getRadians()); + } else { + Pose2d currentPose = getPose(); + _xController.reset(currentPose.getX(), _chassisSpeeds.vxMetersPerSecond); + _yController.reset(currentPose.getY(), _chassisSpeeds.vyMetersPerSecond); + _angleController.reset( + currentPose.getRotation().getRadians(), _chassisSpeeds.omegaRadiansPerSecond + ); + _xController.setGoal(goalPose.getX()); + _yController.setGoal(goalPose.getY()); + _angleController.setGoal(goalPose.getRotation().getRadians()); + } + } + chaseStaticTargetDrive(maxSpeed_MPS); + } + + public void chaseDynamicTargetDrive( + Pose2d visionTarget, Pose2d relativeGoal, boolean targetMoved + ) { + chaseDynamicTargetDrive( + visionTarget, + relativeGoal, + targetMoved, + DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void forceWheelDirection( + double translationX_MPS, double translationY_MPS, double rotation_RADPS, double maxSpeed_MPS + ) { + SwerveModuleState[] goalModuleStates = + _swerveKinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds( + translationX_MPS, translationY_MPS, rotation_RADPS, getYaw() + )); + + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) + _modules[i].setLastAngle(goalModuleStates[i].angle); + + if (areWheelsAligned(goalModuleStates)) { + // Desaturate based of max theoretical or functional rather than current max + edu.wpi.first.math.kinematics.SwerveDriveKinematics.desaturateWheelSpeeds( + goalModuleStates, DRIVE_CONSTS.model.theoreticalMaxWheelSpeed + ); + + for (SwerveModule module : _modules) { + module.setDesiredState( + goalModuleStates[module._moduleNumber], Constants.Control.IS_OPEN_LOOP + ); + } + + // _desiredSpeed->SetDouble(goalModuleStates[0].speed()); + setDesiredSwerveState(goalModuleStates); + } else { + fieldRelativeDrive(0.0, 0.0, 0.0); + // _desiredSpeed->SetDouble(0.0); + } + } + + public void forceWheelDirection( + double translationX_MPS, double translationY_MPS, double rotation_RADPS + ) { + forceWheelDirection( + translationX_MPS, + translationY_MPS, + rotation_RADPS, + DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void forceWheelDirectionDrive( + double moduleAngle_DEG, double speed_MPS, double maxSpeed_MPS + ) { + speed_MPS = Math.min(speed_MPS, maxSpeed_MPS); + Rotation2d moduleAngle = Rotation2d.fromDegrees(moduleAngle_DEG); + SwerveModuleState goalModuleState = new SwerveModuleState(speed_MPS, moduleAngle); + + for (SwerveModule module : _modules) module.setLastAngle(moduleAngle); + + if (areWheelsAligned(goalModuleState)) { + for (SwerveModule module : _modules) + module.setDesiredState(goalModuleState, Constants.Control.IS_OPEN_LOOP); + + // _desiredSpeed->SetDouble(goalModuleState.speed()); + } else { + fieldRelativeDrive(0.0, 0.0, 0.0); + // _desiredSpeed->SetDouble(0.0); + } + } + + public void forceWheelDirectionDrive(double moduleAngle_DEG, double speed_MPS) { + forceWheelDirectionDrive( + moduleAngle_DEG, speed_MPS, DRIVE_CONTROL.defaultLimits.maxTranslationalVelocityMPS + ); + } + + public void setSnapAngleDeg(double angle_DEG) { + setSnapAngleRad(Units.degreesToRadians(angle_DEG)); + } + + public void setSnapAngle(Rotation2d angle) { + setSnapAngleRad(angle.getRadians()); + } + + public void setSnapScoringAngle() { + setSnapAngleRad(allianceScoreAngle().getRadians()); + } + + public void setSnapAngleRad(double angle_rad) { + _angleController.reset( + getPose().getRotation().getRadians(), _chassisSpeeds.omegaRadiansPerSecond + ); + _angleController.setGoal(angle_rad); + } + + public void setStaticTarget(Pose2d goalPose) { + Pose2d currentPose = getPose(); + _xController.reset(currentPose.getX(), _chassisSpeeds.vxMetersPerSecond); + _yController.reset(currentPose.getY(), _chassisSpeeds.vyMetersPerSecond); + _angleController.reset( + currentPose.getRotation().getRadians(), _chassisSpeeds.omegaRadiansPerSecond + ); + _xController.setGoal(goalPose.getX()); + _yController.setGoal(goalPose.getY()); + _angleController.setGoal(goalPose.getRotation().getRadians()); + } + + public Rotation2d toAbsoluteAngle(Rotation2d angle) { + return toAbsoluteAngle(angle); + } + + public double toAbsoluteAngleRad(double angle_rad) { + return Units.degreesToRadians(toAbsoluteAngleDeg(Units.radiansToDegrees(angle_rad))); + } + + public double toAbsoluteAngleDeg(double angle_deg) { + double scaled = (angle_deg % 360.0); + return scaled + (scaled < 0 ? 360.0 : 0.0); + } + + public void testSteer(double angle_deg) { + for (var module : _modules) module.setLastAngleDeg(angle_deg); + } + + public void forceScoreWheelDirection() { + var angle = isRedAlliance() ? 0.0 : 180.0; + for (var module : _modules) module.setLastAngleDeg(angle); + } + + public void forceScoreWheelDirection(double angle_DEG) {} + + public void forceChargingWheelDirection() { + var goalModuleStates = _swerveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 90, getYaw()) + ); + for (int i = 0; i < DRIVE_CONSTS.numModules; i++) { + goalModuleStates[i].angle = goalModuleStates[i].angle.plus(Rotation2d.fromDegrees(90.0)); + _modules[i].setLastAngle(goalModuleStates[i]); + } + } + + public void autonomousDriveMode(boolean enable) { + for (SwerveModule module : _modules) { + module.autonomousDriveMode(enable); + } + } + + public boolean inRange() { + return (_xController.atGoal() && _yController.atGoal() && _angleController.atGoal()); + } + + public boolean thetaInRange() { + return _angleController.atGoal(); + } + + public Pose2d getAlliancePose(Pose2d red, Pose2d blue) { + return _alliance == Alliance.Red ? red : blue; + } + + public void updateTuneScoringShuffleboard(double xError, double yError, double thetaError) { + if (RobotAltModes.isPoseTuning) { + _xPoseError.setDouble(xError); + _yPoseError.setDouble(yError); + _thetaPoseError.setDouble(thetaError); + } + } + + public void updateTuneScoringStatus(boolean tuneScoring) { + if (RobotAltModes.isPoseTuning) { + _usingPoseTuning.setBoolean(tuneScoring); + } + } + + public InstantCommand zeroGyroCommand() { + return new InstantCommand(() -> zeroGyro()); + } + + public InstantCommand zeroPoseCommand() { + return new InstantCommand(() -> zeroPose()); + } + + public InstantCommand forceChargingWheelDirectionCommand() { + return new InstantCommand(() -> forceChargingWheelDirection()); + } + + private boolean checkInitStatus() { + ErrorCode initStatus = _gyro.configFactoryDefault(); + return (initStatus == ErrorCode.OK); + } + + private void resetSimPose(Pose2d pose) { + _simPose = pose; + } + + // Gets vision pose of the primary camera + public Pose2d getVisionPose() { + return _multiCameraController.getVisionPose(); + } + + // Gets vision pose for the camera with the passed id + public Pose2d getVisionPose(int id) { + return _multiCameraController.getVisionPose(id); + } + + public void setDesiredCameras(CameraSets camera) { + _multiCameraController.setDesiredCameras(camera); + } + + public CameraSets getDesiredCameras() { + return _multiCameraController.getDesiredCameras(); + } + + public InstantCommand forceSetInitalPoseCommand() { + return new InstantCommand(() -> { + setPose(new Pose2d( + ScoringLocations.SCORING_LOCATION_2.pose.get(true).getTranslation().plus( + new Translation2d(-1.0, 0.0) + ), + Rotation2d.fromDegrees(180.0) + )); + }); + } + + public CommandBase forceAllianceBasedFieldRelativeMovementCommand( + double blueX_mps, double blueY_mps, double timeout_s + ) { + return forceFieldRelativeMovementCommand( + blueX_mps, blueY_mps, -blueX_mps, -blueY_mps, timeout_s + ); + } + + public CommandBase forceFieldRelativeMovementCommand( + double x_mps, double y_mps, double timeout_s + ) { + return forceFieldRelativeMovementCommand(x_mps, y_mps, x_mps, y_mps, timeout_s); + } + + public CommandBase forceFieldRelativeMovementCommand( + double blueX_mps, double blueY_mps, double redX_mps, double redY_mps, double timeout_s + ) { + // run rather than Commands.run so this implicity requires the current subsystem (drivertrain) + return run(() -> { + fieldRelativeDrive( + isRedAlliance() ? redX_mps : blueX_mps, isRedAlliance() ? redY_mps : blueY_mps, 0.0 + ); + } + ).withTimeout(timeout_s); + } + + // TODO: CLEANUP and consolidate + // Ryan's 254 work + private static final KinematicLimits _limits254 = + Constants.CRobot.drive.control.defaultLimits.get254(); + + private SwerveDriveKinematics _swerveKinematics254 = new SwerveDriveKinematics( + new com.team254.lib.geometry.Translation2d( + DRIVE_DIMS.halfTrackLength_M, DRIVE_DIMS.halfTrackWidth_M + ), + new com.team254.lib.geometry.Translation2d( + DRIVE_DIMS.halfTrackLength_M, -DRIVE_DIMS.halfTrackWidth_M + ), + new com.team254.lib.geometry.Translation2d( + -DRIVE_DIMS.halfTrackLength_M, DRIVE_DIMS.halfTrackWidth_M + ), + new com.team254.lib.geometry.Translation2d( + -DRIVE_DIMS.halfTrackLength_M, -DRIVE_DIMS.halfTrackWidth_M + ) + ); + + SwerveSetpointGenerator _generator254 = new SwerveSetpointGenerator(_swerveKinematics254); + SwerveSetpoint _goal = new SwerveSetpoint( + new com.team254.lib.swerve.ChassisSpeeds(), + new com.team254.lib.swerve.SwerveModuleState[] { + new com.team254.lib.swerve.SwerveModuleState(), + new com.team254.lib.swerve.SwerveModuleState(), + new com.team254.lib.swerve.SwerveModuleState(), + new com.team254.lib.swerve.SwerveModuleState()} + ); + + private void fieldRelative254(double xVelo_MPS, double yVelo_MPS, double theta_RDS) { + ChassisSpeeds speeds = + ChassisSpeeds.fromFieldRelativeSpeeds(xVelo_MPS, yVelo_MPS, theta_RDS, getYaw()); + updateDesiredStates254( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond + ); + + for (SwerveModule module : _modules) { + module.setDesiredState( + _goal.mModuleStates[module._moduleNumber].getWPI(), Constants.Control.IS_OPEN_LOOP, true + ); + // module.setWithVelocityShortestPath( + // _goal.mModuleStates[module._moduleNumber].speedMetersPerSecond, + // Rotation2d.fromDegrees(_goal.mModuleStates[module._moduleNumber].angle.getDegrees()) + // ); + } + } + + private void updateDesiredStates254(double xVelo_MPS, double yVelo_MPS, double theta_RDS) { + if (wantOrientationMode) + return; + // Set the des_states to account for robot traversing arc. + com.team254.lib.geometry.Pose2d robot_pose_vel = new com.team254.lib.geometry.Pose2d( + xVelo_MPS * Constants.LOOP_PERIOD_S, + yVelo_MPS * Constants.LOOP_PERIOD_S, + com.team254.lib.geometry.Rotation2d254.fromRadians(theta_RDS * Constants.LOOP_PERIOD_S) + ); + + com.team254.lib.geometry.Twist2d twist_vel = + com.team254.lib.geometry.Pose2d.log(robot_pose_vel); + com.team254.lib.swerve.ChassisSpeeds updated_chassis_speeds = + new com.team254.lib.swerve.ChassisSpeeds( + twist_vel.dx / Constants.LOOP_PERIOD_S, + twist_vel.dy / Constants.LOOP_PERIOD_S, + twist_vel.dtheta / Constants.LOOP_PERIOD_S + ); + + _goal = _generator254.generateSetpoint( + _limits254, _goal, updated_chassis_speeds, Constants.LOOP_PERIOD_S + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..c2a6eec --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,535 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.GenericPublisher; +import edu.wpi.first.networktables.GenericSubscriber; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.ElevatorConfs; +import frc.robot.constants.ElevatorConfs.ElevatorControl; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.ElevatorPositions; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.utils.CTREConversion; +import frc.robot.utils.LoopTimer; + +public class Elevator extends SubsystemBase { + /* --- Constant configuration shortcuts --- */ + public final ElevatorConfs ELEVATOR_CONSTS = Constants.CRobot.elevator; + + /* --- Sensors, motors, and hardware --- */ + private final WPI_TalonFX _elevatorMotor; + private final WPI_TalonFX _followerMotor; + + /* --- Robot State and Physical Properties variables --- */ + private boolean _followingProfile = false; + private Timer _timer = new Timer(); + private double _prevVelocity_MPS; + private ElevatorPositions _goalPosition = ElevatorPositions.ELEVATOR_UNKNOWN_POSITION; + private int _clampedPosition = ElevatorPositions.ELEVATOR_UNKNOWN_POSITION.ticks; + + /* --- Control Utils --- */ + private TrapezoidProfile _profile; + private ElevatorFeedforward _elevatorFeedForward; + + private double _prevVelo; + + private TalonFXConfiguration _elevatorMotorFXConfig = new TalonFXConfiguration(); + + /* --- Shuffleboard Entries --- */ + private GenericSubscriber _pFactorEntry, _iFactorEntry, _dFactorEntry; + private GenericSubscriber _kSEntry, _kGEntry, _kVEntry, _kAEntry; + private GenericPublisher _currentTicksEntry, _targetTicksEntry; + private GenericPublisher _elevatorCurrentVelocityEntry, _elevatorTargetVelocityEntry; + private GenericPublisher _elevatorProfilePositionEntry; + private GenericPublisher _elevatorAccelEntry; + private GenericPublisher _elevatorPositionErrorEntry, _elevatorVelocityErrorEntry; + private GenericPublisher _elevatorPercentEntry, _statorCurrentEntry, _supplyCurrentEntry; + private GenericEntry _elevatorPowerEntry; + private GenericSubscriber _maxVeloEntry, _maxAccelEntry; + + /* --- Simulation Resources --- */ + private Mechanism2d _mech2d; + private MechanismRoot2d _elevatorRaise; + private MechanismLigament2d _elevatorTower; + private MechanismLigament2d _elevatorStage2; + + static {} + + public Elevator() { + if (Constants.hasElevator) { + initMotorConfigs(); + + _elevatorMotor = new WPI_TalonFX(ELEVATOR_CONSTS.ports.primaryMotor); + _followerMotor = new WPI_TalonFX(ELEVATOR_CONSTS.ports.followerMotor); + + _profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints( + ELEVATOR_CONSTS.control.ELEVATOR_MAX_VELO_MPS, + ELEVATOR_CONSTS.control.ELEVATOR_MAX_ACC_MPSQ + ), + new TrapezoidProfile.State(0.0, 0.0) + ); + _elevatorFeedForward = new ElevatorFeedforward( + ELEVATOR_CONSTS.control.ELEVATOR_KS_V, + ELEVATOR_CONSTS.control.ELEVATOR_KG_V, + ELEVATOR_CONSTS.control.ELEVATOR_KV_V_PER_MPS, + ELEVATOR_CONSTS.control.ELEVATOR_KA_V_PER_MPSQ + ); + + if (Robot.isReal()) { + int counter = 0; + while (!checkInitStatus()) { + System.out.println("ELEVATOR Check Init Status : " + counter++); + } + } else { + simulationConfig(); + } + + configElevatorMotor(); + configFollowerMotor(); + + configShuffleboard(); + + LoopTimer.markEvent(" Elevator Initialization Complete: "); + } else { + _elevatorMotor = null; + _followerMotor = null; + } + } + + public void initMotorConfigs() { + /* Elevator Motor Configuration */ + _elevatorMotorFXConfig.slot0.kP = ElevatorConfs.ElevatorControl.ELEVATOR_PFAC; + _elevatorMotorFXConfig.slot0.kI = ElevatorConfs.ElevatorControl.ELEVATOR_IFAC; + _elevatorMotorFXConfig.slot0.kD = ElevatorConfs.ElevatorControl.ELEVATOR_DFAC; + _elevatorMotorFXConfig.slot0.kF = ElevatorConfs.ElevatorControl.ELEVATOR_FFAC; + _elevatorMotorFXConfig.supplyCurrLimit = new SupplyCurrentLimitConfiguration( + ElevatorConfs.ELEVATOR_SUPPLY_ENABLE_CURRENT_LIMIT, + ElevatorConfs.ELEVATOR_SUPPLY_CONTINUOUS_CURRENT_LIMIT, + ElevatorConfs.ELEVATOR_SUPPLY_PEAK_CURRENT_LIMIT, + ElevatorConfs.ELEVATOR_SUPPLY_PEAK_CURRENT_DURATION + ); + _elevatorMotorFXConfig.statorCurrLimit = new StatorCurrentLimitConfiguration( + ElevatorConfs.ELEVATOR_STATOR_ENABLE_CURRENT_LIMIT, + ElevatorConfs.ELEVATOR_STATOR_CONTINUOUS_CURRENT_LIMIT, + ElevatorConfs.ELEVATOR_STATOR_PEAK_CURRENT_LIMIT, + ElevatorConfs.ELEVATOR_STATOR_PEAK_CURRENT_DURATION + ); + _elevatorMotorFXConfig.initializationStrategy = SensorInitializationStrategy.BootToZero; + _elevatorMotorFXConfig.openloopRamp = ElevatorConfs.ElevatorControl.ELEVATOR_OPEN_LOOP_RAMP; + _elevatorMotorFXConfig.closedloopRamp = ElevatorConfs.ElevatorControl.ELEVATOR_CLOSED_LOOP_RAMP; + _elevatorMotorFXConfig.voltageCompSaturation = + ElevatorConfs.ElevatorControl.ELEVATOR_VOLTAGE_COMP_SATURATION; + _elevatorMotorFXConfig.neutralDeadband = ElevatorConfs.ELEVATOR_MOTOR_DEADBAND; + } + + private void configElevatorMotor() { + if (Constants.hasElevator) { + _elevatorMotor.configFactoryDefault(); + _elevatorMotor.configAllSettings(_elevatorMotorFXConfig); + _elevatorMotor.setInverted(ELEVATOR_CONSTS.ELEVATOR_MOTOR_INVERTED); + _elevatorMotor.setNeutralMode(ELEVATOR_CONSTS.ELEVATOR_NEUTRAL_MODE); + _elevatorMotor.setSelectedSensorPosition(0); + _elevatorMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + } + } + + private void configFollowerMotor() { + if (Constants.hasElevator && Constants.isCompBot) { + _followerMotor.configFactoryDefault(); + _followerMotor.configAllSettings(_elevatorMotorFXConfig); + _followerMotor.setInverted(ELEVATOR_CONSTS.ELEVATOR_FOLLOWER_MOTOR_INVERTED); + _followerMotor.setNeutralMode(ELEVATOR_CONSTS.ELEVATOR_NEUTRAL_MODE); + _followerMotor.follow(_elevatorMotor); + } + } + + public void configShuffleboard() { + if (Constants.hasElevator) { + ShuffleboardTab tuningTab = Shuffleboard.getTab("Tuning tab"); + if (RobotAltModes.isVerboseMode) { + ShuffleboardLayout elevatorLayout = + tuningTab.getLayout("Elevator", BuiltInLayouts.kGrid).withSize(1, 4).withPosition(0, 0); + + _currentTicksEntry = + elevatorLayout.add("Current Position", 0.0).withPosition(0, 0).getEntry(); + _targetTicksEntry = + elevatorLayout.add("Target Position", 0.0).withPosition(0, 1).getEntry(); + + _elevatorPercentEntry = + elevatorLayout.add("Percent output", 0.0).withPosition(0, 2).getEntry(); + + _elevatorCurrentVelocityEntry = + elevatorLayout.add("Current Velocity", 0.0).withPosition(0, 3).getEntry(); + _elevatorTargetVelocityEntry = + elevatorLayout.add("Target Velocity", 0.0).withPosition(0, 4).getEntry(); + _elevatorProfilePositionEntry = + elevatorLayout.add("Profile Position", 0.0).withPosition(0, 5).getEntry(); + + _elevatorAccelEntry = + elevatorLayout.add("Current Accelerations", 0.0).withPosition(0, 5).getEntry(); + + _statorCurrentEntry = + elevatorLayout.add("Current Stator Current", 0.0).withPosition(0, 6).getEntry(); + _supplyCurrentEntry = + elevatorLayout.add("Current Supply Current", 0.0).withPosition(0, 7).getEntry(); + } + if (RobotAltModes.isElevatorTuningMode) { + _kGEntry = tuningTab.add("kG", ELEVATOR_CONSTS.control.ELEVATOR_KG_V) + .withPosition(2, 0) + .getEntry(); + _kSEntry = tuningTab.add("kS", ELEVATOR_CONSTS.control.ELEVATOR_KS_V) + .withPosition(2, 1) + .getEntry(); + _kVEntry = tuningTab.add("kV", ELEVATOR_CONSTS.control.ELEVATOR_KV_V_PER_MPS) + .withPosition(2, 2) + .getEntry(); + _kAEntry = tuningTab.add("kA", ELEVATOR_CONSTS.control.ELEVATOR_KA_V_PER_MPSQ) + .withPosition(2, 3) + .getEntry(); + _elevatorPowerEntry = tuningTab.add("Power", 0.0).withPosition(1, 3).getEntry(); + _elevatorPositionErrorEntry = + tuningTab.add("Elevator Pos Error", 0.0).withPosition(0, 4).getEntry(); + _elevatorVelocityErrorEntry = + tuningTab.add("Elevator Velo Error", 0.0).withPosition(0, 5).getEntry(); + _maxVeloEntry = + tuningTab.add("Elevator Max Velo", ELEVATOR_CONSTS.control.ELEVATOR_MAX_VELO_MPS) + .withPosition(10, 0) + .getEntry(); + _maxAccelEntry = + tuningTab.add("Elevator Max Accel", ELEVATOR_CONSTS.control.ELEVATOR_MAX_ACC_MPSQ) + .withPosition(10, 1) + .getEntry(); + } + if (RobotAltModes.isPIDTuningMode || RobotAltModes.isElevatorTuningMode) { + _pFactorEntry = tuningTab.add("Height P Fac", ElevatorConfs.ElevatorControl.ELEVATOR_PFAC) + .withPosition(3, 0) + .getEntry(); + _iFactorEntry = tuningTab.add("Height I Fac", ElevatorConfs.ElevatorControl.ELEVATOR_IFAC) + .withPosition(4, 0) + .getEntry(); + _dFactorEntry = tuningTab.add("Height D Fac", ElevatorConfs.ElevatorControl.ELEVATOR_DFAC) + .withPosition(5, 0) + .getEntry(); + } + } + } + + public void updateShuffleboard() { + if (Constants.hasElevator && RobotAltModes.isElevatorTuningMode) { + _currentTicksEntry.setInteger(getCurrentHeight()); + _targetTicksEntry.setInteger(getTargetHeight()); + _elevatorPositionErrorEntry.setInteger(getTargetHeight() - getCurrentHeight()); + _elevatorPercentEntry.setDouble(getPower()); + + _statorCurrentEntry.setDouble(_elevatorMotor.getStatorCurrent()); + _supplyCurrentEntry.setDouble(_elevatorMotor.getSupplyCurrent()); + } + } + + @Override + public void periodic() { + if (Constants.hasElevator) { + double currentVelo = CTREConversion.falconToMPS( + _elevatorMotor.getSelectedSensorVelocity(), + ElevatorConfs.ElevatorDims.ELEVATOR_CIRCUMFERENCE_M, + ElevatorConfs.ElevatorDims.ELEVATOR_GEAR_RATIO + ); + + if (RobotAltModes.isElevatorTuningMode) { + _elevatorCurrentVelocityEntry.setDouble(currentVelo); + + _elevatorAccelEntry.setDouble(currentVelo - _prevVelo); + _prevVelo = currentVelo; + } + LoopTimer.markLoopStart(); + if (_followingProfile) { + State goalState = _profile.calculate(_timer.get()); + double goalVelo = goalState.velocity; + + if (RobotAltModes.isElevatorTuningMode) { + _elevatorTargetVelocityEntry.setDouble(goalState.velocity); + + _elevatorVelocityErrorEntry.setDouble(goalVelo - currentVelo); + } + + double desiredAccelMPS2 = + (goalState.velocity - _prevVelocity_MPS) / Constants.LOOP_PERIOD_S; + _prevVelocity_MPS = goalState.velocity; + + double ff = _elevatorFeedForward.calculate(goalState.velocity, desiredAccelMPS2) + / ElevatorControl.ELEVATOR_VOLTAGE_COMP_SATURATION; + + _elevatorMotor.set( + ControlMode.Position, + // TODO check if this is supposed to be _lastExpectedHeight + CTREConversion.metersToFalcon( + goalState.position, + ElevatorConfs.ElevatorDims.ELEVATOR_CIRCUMFERENCE_M, + ElevatorConfs.ElevatorDims.ELEVATOR_GEAR_RATIO + ), + DemandType.ArbitraryFeedForward, + ff + ); + _elevatorProfilePositionEntry.setDouble(CTREConversion.metersToFalcon( + goalState.position, + ElevatorConfs.ElevatorDims.ELEVATOR_CIRCUMFERENCE_M, + ElevatorConfs.ElevatorDims.ELEVATOR_GEAR_RATIO + )); + } + + updateShuffleboard(); + LoopTimer.markCompletion(" Elevator Profile Following ", "\n Total Elevator: "); + } + } + + public void setPower(double power) { + if (Constants.hasElevator) { + _followingProfile = false; + _elevatorMotor.set(power); + } + } + + public double getPower() { + if (Constants.hasElevator) { + return _elevatorMotor.getMotorOutputPercent(); + } + return 0.0; + } + + public boolean isClearOfIntake() { + if (Constants.hasElevator) { + return getCurrentHeight() > ElevatorConfs.ELEVATOR_CLEAR_OF_INTAKE_TICKS; + } + return true; + } + + public int getCurrentHeight() { + if (Constants.hasElevator) { + return (int) _elevatorMotor.getSelectedSensorPosition(); + } + return _clampedPosition; + } + + public int getTargetHeight() { + return _clampedPosition; + } + + public void setTargetHeight(ElevatorPositions target) { + _followingProfile = false; + _goalPosition = target; + _clampedPosition = clamp(target.ticks); + if (Constants.hasElevator) { + _elevatorMotor.set(ControlMode.Position, _clampedPosition); + } + } + + public double getCurrentVelocity() { + if (Constants.hasElevator) { + return _elevatorMotor.getSelectedSensorVelocity(); + } + return 0.0; + } + + public boolean inTolerance(int lowerTolerance, int upperTolerance) { + if (Constants.hasElevator) { + int error = getCurrentHeight() - getTargetHeight(); + return error >= lowerTolerance && error <= upperTolerance; + } + return true; + } + + public void setEncoder(int ticks) { + if (Constants.hasElevator) { + _elevatorMotor.setSelectedSensorPosition(ticks); + _goalPosition = ElevatorPositions.ELEVATOR_MIN_TICKS; + _clampedPosition = ticks; + _followingProfile = false; + } + } + + public boolean motorResetConfig() { + if (Constants.hasElevator) { + if (_elevatorMotor.hasResetOccurred()) { + configElevatorMotor(); + return true; + } + if (Constants.isCompBot && _followerMotor.hasResetOccurred()) { + configFollowerMotor(); + return true; + } + } + return false; + } + + // we should just map these to levels + public ElevatorPositions getElevatorScoringHeight(GamePieceType heldPiece, ScoringLevels level) { + if (heldPiece.isUnknown() || level == ScoringLevels.UNKNOWN_SCORING_LEVEL) { + return _goalPosition; + } + return level.getPosition(heldPiece); + } + + public void onEnable() { + if (Constants.hasElevator) { + _followingProfile = false; + _elevatorMotor.setNeutralMode(ElevatorConfs.ELEVATOR_NEUTRAL_MODE); + if (Constants.isCompBot) { + _followerMotor.setNeutralMode(ElevatorConfs.ELEVATOR_NEUTRAL_MODE); + } + if (RobotAltModes.isPIDTuningMode || RobotAltModes.isElevatorTuningMode) { + _elevatorMotor.config_kP( + 0, _pFactorEntry.getDouble(ElevatorConfs.ElevatorControl.ELEVATOR_PFAC) + ); + _elevatorMotor.config_kI( + 0, _iFactorEntry.getDouble(ElevatorConfs.ElevatorControl.ELEVATOR_IFAC) + ); + _elevatorMotor.config_kD( + 0, _dFactorEntry.getDouble(ElevatorConfs.ElevatorControl.ELEVATOR_DFAC) + ); + } + if (RobotAltModes.isElevatorTuningMode) { + _elevatorFeedForward = new ElevatorFeedforward( + _kSEntry.getDouble(ELEVATOR_CONSTS.control.ELEVATOR_KS_V), + _kGEntry.getDouble(ELEVATOR_CONSTS.control.ELEVATOR_KG_V), + _kVEntry.getDouble(ELEVATOR_CONSTS.control.ELEVATOR_KV_V_PER_MPS), + _kAEntry.getDouble(ELEVATOR_CONSTS.control.ELEVATOR_KA_V_PER_MPSQ) + ); + } + } + } + + public void onDisable() { + if (Constants.hasElevator) { + _followingProfile = false; + _elevatorMotor.setNeutralMode(ElevatorConfs.ELEVATOR_DISABLED_MODE); + if (Constants.isCompBot) { + _followerMotor.setNeutralMode(ElevatorConfs.ELEVATOR_DISABLED_MODE); + } + _profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints( + ELEVATOR_CONSTS.control.ELEVATOR_MAX_VELO_MPS, + ELEVATOR_CONSTS.control.ELEVATOR_MAX_ACC_MPSQ + ), + new TrapezoidProfile.State(0, 0) + ); + } + } + + public void setPositionalArbitraryFF(double position, double ff) { + if (Constants.hasElevator) { + _followingProfile = false; + _elevatorMotor.set(ControlMode.Position, position, DemandType.ArbitraryFeedForward, ff); + } + } + + public void setElevatorProfile(ElevatorPositions position) { + setElevatorProfile(position, 0.0); + } + + public void setElevatorProfile(ElevatorPositions position, double velocity) { + setElevatorProfile( + position, velocity, _maxVeloEntry.getDouble(ELEVATOR_CONSTS.control.ELEVATOR_MAX_VELO_MPS) + ); + } + + public void setElevatorProfile(ElevatorPositions position, double velocity, double maxV) { + setElevatorProfile( + position, + velocity, + maxV, + _maxAccelEntry.getDouble(ELEVATOR_CONSTS.control.ELEVATOR_MAX_ACC_MPSQ) + ); + } + + public void setElevatorProfile( + ElevatorPositions goalPosition, double goalVelocityMPS, double maxV, double maxA + ) { + if (Constants.hasElevator) { + _followingProfile = true; + _goalPosition = goalPosition; + _clampedPosition = clamp(goalPosition.ticks); + TrapezoidProfile.State currentState = _profile.calculate(_timer.get()); + currentState.position = CTREConversion.falconToMeters( + _elevatorMotor.getSelectedSensorPosition(), + ElevatorConfs.ElevatorDims.ELEVATOR_CIRCUMFERENCE_M, + ElevatorConfs.ElevatorDims.ELEVATOR_GEAR_RATIO + ); + _profile = new TrapezoidProfile( + new TrapezoidProfile.Constraints(maxV, maxA), + new TrapezoidProfile.State( + CTREConversion.falconToMeters( + _clampedPosition, + ElevatorConfs.ElevatorDims.ELEVATOR_CIRCUMFERENCE_M, + ElevatorConfs.ElevatorDims.ELEVATOR_GEAR_RATIO + ), + goalVelocityMPS + ), + currentState + ); + + _prevVelocity_MPS = currentState.velocity; + _timer.restart(); + } + } + + private boolean checkInitStatus() { + if (Constants.hasElevator) { + ErrorCode initStatus = _elevatorMotor.configFactoryDefault(); + return initStatus == ErrorCode.OK; + } + return true; + } + + private int clamp(int target, int min, int max) { + return Math.max(min, Math.min(max, target)); + } + + private int clamp(int target) { + return clamp( + target, + ElevatorPositions.ELEVATOR_MIN_FALCON_TICKS, + ElevatorPositions.ELEVATOR_MAX_FALCON_TICKS + ); + } + + public void simulationConfig() { + if (Constants.hasElevator && RobotAltModes.isSim) { + if (RobotAltModes.isSimElevator) { + /* TODO: fill in with InclinedSimElevator objects */ + } + _mech2d = new Mechanism2d(80, 80); + _elevatorRaise = _mech2d.getRoot("ElevatorRaise", 30, 30); + _elevatorTower = _elevatorRaise.append( + new MechanismLigament2d("BaseStage", 30, 225, 6, new Color8Bit(Color.kBlue)) + ); + _elevatorStage2 = _elevatorRaise.append( + new MechanismLigament2d("Stage1", 30, 45, 6, new Color8Bit(Color.kLightPink)) + ); + + SmartDashboard.putData("Elevator Sim", _mech2d); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/LedController.java b/src/main/java/frc/robot/subsystems/LedController.java new file mode 100644 index 0000000..8ba4e78 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LedController.java @@ -0,0 +1,351 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import com.ctre.phoenix.led.CANdle.VBatOutputMode; +import com.ctre.phoenix.led.CANdleConfiguration; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.LedConfs; +import frc.robot.constants.LedConfs.LedSections; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.constants.enums.LedColors; +import frc.robot.constants.enums.LedModes; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; +import frc.robot.utils.LoopTimer; +import java.util.LinkedList; + +public class LedController extends SubsystemBase { + /* --- Hardware and Configuration --- */ + private CANdle _candle; + private CANdleConfiguration _candleConfigs; + + /* --- State and Physical Property variables --- */ + private LedModes _curMode = LedModes.COLOR_FLOW; + private ScoringLocations _tempMode = ScoringLocations.SCORING_LOCATION_1; + private HPIntakeStations _hpMode = HPIntakeStations.UNKNOWN_INTAKE_STATION; + private boolean _disabled; + private boolean _lastVision = false; + private boolean _isOverriden = false; + + /* --- Control Utils --- */ + /* Generic Animations */ + LinkedList colorsQueue = new LinkedList(); + LinkedList sectionsQueue = new LinkedList(); + + public LedController() { + if (Constants.hasLed) { + _candle = + new CANdle(Constants.CRobot.led.ports.candle, Constants.CRobot.led.ports.candleCanBus); + _candleConfigs = new CANdleConfiguration(); + _candleConfigs.statusLedOffWhenActive = true; + _candleConfigs.disableWhenLOS = false; + _candleConfigs.stripType = LEDStripType.GRB; + _candleConfigs.brightnessScalar = 0.1; + _candleConfigs.vBatOutputMode = VBatOutputMode.Modulated; + + if (Robot.isReal()) { + int counter = 0; + while (!checkInitStatus()) { + System.out.println("CANDLE Check Init Status : " + counter++); + } + } else { + } + + configCandle(); + _candle.animate(LedModes.RAINBOW.animation); + } + LoopTimer.markEvent(" LED Initialization Complete: "); + } + + @Override + public void periodic() { + LoopTimer.markLoopStart(); + + // colorsQueue, sectionsQueue + if (colorsQueue.size() > 0 && sectionsQueue.size() > 0 && !_disabled) { + LedColors desiredLEDColor = colorsQueue.getFirst(); + LedSections desiredLEDSection = sectionsQueue.getFirst(); + + colorsQueue.removeFirst(); + sectionsQueue.removeFirst(); + setSolidColor(desiredLEDColor, desiredLEDSection); + } + + LoopTimer.markCompletion("\n Total LED Controller: "); + } + + public boolean checkInitStatus() { + return Constants.isCompBot && _candle.configFactoryDefault() == ErrorCode.OK; + } + + public void configCandle() { + if (Constants.hasLed) { + _candle.configFactoryDefault(); + _candle.configAllSettings(_candleConfigs, 100); + } + } + + public void disabledPeriodic(Pose2d currPose, Pose2d desiredPose, Boolean blueAlliance) { + Transform2d transform = new Transform2d(currPose, desiredPose); + double transformX = transform.getX(); + boolean xCondition = Math.abs(transformX) <= Constants.CRobot.drive.control.xy.toleranceM * 3; + double transformY = transform.getY(); + boolean yCondition = Math.abs(transformY) <= Constants.CRobot.drive.control.xy.toleranceM; + boolean angleCondition = Math.abs(transform.getRotation().getRadians()) + <= Constants.CRobot.drive.control.theta.tolerance.getRadians() * 3; + + if (xCondition && yCondition && angleCondition) { + setSolidColor(LedColors.GREEN, LedSections.CANDLE); + setSolidColor(LedColors.GREEN, LedSections.STRIP); + setSolidColor(LedColors.GREEN, LedSections.STRIP_2); + return; + } + + if (xCondition) { + setSolidColor(LedColors.SPACE_COOKIE, LedSections.STRIP); + setSolidColor(LedColors.SPACE_COOKIE, LedSections.STRIP_2); + } else { + int ledChange = Math.max(0, Math.min(7, (int) Math.abs(transformX))); + if (transformX > 0) { + setSolidColor( + LedColors.OFF, + LedConfs.STRIP_1_START, + LedConfs.STRIP_1_START + LedConfs.STRIP_LENGTH - ledChange + ); + setSolidColor( + LedColors.BLUE, + LedConfs.STRIP_1_START + LedConfs.STRIP_LENGTH - ledChange, + LedConfs.STRIP_1_START + LedConfs.STRIP_LENGTH + ); + setSolidColor( + LedColors.OFF, + LedConfs.STRIP_2_START + ledChange, + LedConfs.STRIP_2_START + LedConfs.STRIP_LENGTH + ); + setSolidColor(LedColors.BLUE, LedConfs.STRIP_2_START, LedConfs.STRIP_2_START + ledChange); + } else { + setSolidColor( + LedColors.OFF, + LedConfs.STRIP_1_START + ledChange, + LedConfs.STRIP_1_START + LedConfs.STRIP_LENGTH + ); + setSolidColor(LedColors.PURPLE, LedConfs.STRIP_1_START, LedConfs.STRIP_1_START + ledChange); + setSolidColor( + LedColors.OFF, + LedConfs.STRIP_2_START, + LedConfs.STRIP_2_START + LedConfs.STRIP_LENGTH - ledChange + ); + setSolidColor(LedColors.PURPLE, LedConfs.LED_LENGTH - ledChange, LedConfs.LED_LENGTH); + } + } + + if (yCondition) { + setSolidColor(LedColors.SPACE_COOKIE, LedSections.INDICATOR); + } else { + if (transformY > 0) { + setSolidColor(LedColors.BLUE, LedSections.INDICATOR_HALF_2); + turnOffLeds(LedSections.INDICATOR_HALF_1); + } else { + setSolidColor(LedColors.BLUE, LedSections.INDICATOR_HALF_1); + turnOffLeds(LedSections.INDICATOR_HALF_2); + } + } + } + + public void changeAnimation(LedModes desiredAnimation) { + if (Constants.hasLed) { + _candle.animate(desiredAnimation.animation); + _curMode = desiredAnimation; + } + } + + // testing + public void incrementAnimation() { + _tempMode = _tempMode.getIncr(); + // _hpMode = _hpMode.getIncr(); + // changeAnimation(_curMode); + // setIntakeMode(_hpMode, GamePieceType.CUBE_GAME_PIECE); + setScoringMode(ScoringLevels.SCORING_LEVEL_2, _tempMode); + } + // testing + public void decrementAnimation() { + _curMode = _curMode.getDecr(); + changeAnimation(_curMode); + } + + public void setSolidColor(int r, int g, int b, int w, int start, int end) { + if (Constants.hasLed) { + _candle.clearAnimation(0); + _candle.setLEDs(r, g, b, w, start, end - start); + } + } + + public void setSolidColor(int r, int g, int b, int w, LedSections ledSection) { + setSolidColor(r, g, b, w, ledSection.start, ledSection.end); + } + + public void setSolidColor(LedColors ledColor, int start, int end) { + setSolidColor(ledColor.r, ledColor.g, ledColor.b, LedConfs.LED_WHITE_LEVEL, start, end); + } + + public void setSolidColor(LedColors ledColor, LedConfs.LedSections ledSection) { + setSolidColor(ledColor.r, ledColor.g, ledColor.b, LedConfs.LED_WHITE_LEVEL, ledSection); + } + + public void setScoringColor(ScoringLevels scoringLevel, ScoringLocations scoringLocation) { + setScoringColor(scoringLevel, scoringLocation.section, scoringLocation.subsection); + } + + public CommandBase setSolidColorCommand(LedColors ledColor, LedConfs.LedSections ledSection) { + return Commands.runOnce(() -> setSolidColor(ledColor, ledSection)); + } + + public void setScoringColor( + ScoringLevels scoringLevel, + LedConfs.ScoringSections scoringSection, + LedConfs.ScoringSubsections scoringSubsection + ) { + // pushQueueCounter(LedColors.OFF, LedSections.STRIP); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP_2); + + pushQueueCounter(scoringSection.sectionColor, LedSections.STRIP_HALF_1); + pushQueueCounter(scoringSubsection.subsectionColor, LedSections.STRIP_HALF_2); + pushQueueCounter(scoringSection.sectionColor, LedSections.STRIP_2_HALF_1); + pushQueueCounter(scoringSubsection.subsectionColor, LedSections.STRIP_2_HALF_2); + + pushQueueCounter(LedColors.OFF, LedSections.INDICATOR); + + switch (scoringLevel) { + case SCORING_LEVEL_0: + pushQueueCounter(LedColors.GREEN, LedSections.INDICATOR_THIRD_1); + // pushQueueCounter(LedColors.GREEN, LedSections.STRIP_THIRD_1); + // pushQueueCounter(LedColors.GREEN, LedSections.STRIP_2_THIRD_1); + + // pushQueueCounter(LedColors.OFF, LedSections.STRIP); + // pushQueueCounter(LedColors.sectionColor, LedSections.STRIP_THIRD_1); + // pushQueueCounter(LedColors.subsectionColor, LedSections.STRIP_THIRD_2); + break; + case SCORING_LEVEL_1: + pushQueueCounter(LedColors.BLUE, LedSections.INDICATOR_THIRD_1); + pushQueueCounter(LedColors.BLUE, LedSections.INDICATOR_THIRD_2); + + // pushQueueCounter(LedColors.OFF, LedSections.STRIP); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP_2); + // pushQueueCounter(LedColors.BLUE, LedSections.STRIP_THIRD_1); + // pushQueueCounter(LedColors.BLUE, LedSections.STRIP_2_THIRD_1); + // pushQueueCounter(LedColors.BLUE, LedSections.STRIP_THIRD_2); + // pushQueueCounter(LedColors.BLUE, LedSections.STRIP_2_THIRD_2); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP); + // pushQueueCounter(LedColors.sectionColor, LedSections.STRIP_THIRD_1); + // pushQueueCounter(LedColors.subsectionColor, LedSections.STRIP_THIRD_2); + // pushQueueCounter(LedColors.sectionColor, LedSections.STRIP_THIRD_3); + // pushQueueCounter(LedColors.subsectionColor, LedSections.STRIP_THIRD_4); + break; + case SCORING_LEVEL_2: + pushQueueCounter(LedColors.PINK, LedSections.INDICATOR); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP_2); + // pushQueueCounter(LedColors.PINK, LedSections.STRIP); + // pushQueueCounter(LedColors.PINK, LedSections.STRIP_2); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP); + // pushQueueCounter(LedColors.sectionColor, LedSections.STRIP); + // pushQueueCounter(LedColors.subsectionColor, LedSections.STRIP_THIRD_2); + // pushQueueCounter(LedColors.subsectionColor, LedSections.STRIP_THIRD_4); + // pushQueueCounter(LedColors.subsectionColor, LedSections.STRIP_THIRD_6); + break; + default: + pushQueueCounter(LedColors.OFF, LedSections.INDICATOR); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP)); + // pushQueueCounter(LedColors.OFF, LedSections.STRIP_2); + break; + } + pushQueueCounter(LedColors.BLUE, LedSections.CANDLE); + } + + public void setScoringMode(ScoringLevels scoringLevel, ScoringLocations scoringLocation) { + if (scoringLocation == ScoringLocations.UNKNOWN_SCORING_LOCATION) { + turnOffLeds(LedSections.NON_CANDLE); + } else { + setScoringColor(scoringLevel, scoringLocation.section, scoringLocation.subsection); + } + } + + public void turnOffLeds(int start, int end) { + setSolidColor(0, 0, 0, 0, start, end); + } + + public void turnOffLeds(LedSections ledSection) { + setSolidColor(LedColors.OFF, ledSection); + } + + public void setIntakeMode(HPIntakeStations HPIntakeStation, GamePieceType gamePieceType) { + pushQueueCounter(gamePieceType.intakeColor, LedSections.INDICATOR_HALF_2); + pushQueueCounter(gamePieceType.intakeColor, LedSections.STRIP_2); + + pushQueueCounter(HPIntakeStation.stationColor, LedSections.INDICATOR_HALF_1); + pushQueueCounter(HPIntakeStation.stationColor, LedSections.STRIP); + + pushQueueCounter(LedColors.RED_ORANGE, LedSections.CANDLE); + } + + public boolean ledResetConfig() { + if (Constants.hasLed && _candle.hasResetOccurred()) { + configCandle(); + return true; + } + return false; + } + + public void pushQueueCounter(LedColors color, LedConfs.LedSections section) { + colorsQueue.addLast(color); + sectionsQueue.addLast(section); + } + + public void setDisabled(Boolean disabled) { + _disabled = disabled; + } + + public void setVisionLed(Boolean hasVision) { + if (hasVision != _lastVision) { + pushQueueCounter(hasVision ? LedColors.GREEN : LedColors.YELLOW, LedSections.CANDLE); + } + _lastVision = hasVision; + } + + public void setOverride(Boolean override) { + _isOverriden = override; + + // Clear any accumulated teleop LED queue + if (_isOverriden == false) { + colorsQueue.clear(); + sectionsQueue.clear(); + } + } + + public InstantCommand incrementAnimationCommand() { + return new InstantCommand(() -> incrementAnimation()); + } + + public InstantCommand setColorCommand(LedColors ledColor, LedSections ledSection) { + return new InstantCommand(() -> setSolidColor(ledColor, ledSection)); + } + + public InstantCommand setSolidColorCommand(LedColors ledColor) { + return new InstantCommand(() -> setSolidColor(ledColor, LedSections.ALL)); + } + + public InstantCommand changeAnimationCommand(LedModes ledMode) { + return new InstantCommand(() -> changeAnimation(ledMode)); + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..11596d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,496 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.AbsoluteSensorRange; +import com.ctre.phoenix.sensors.CANCoderConfiguration; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; +import com.ctre.phoenix.sensors.SensorTimeBase; +import com.ctre.phoenix.sensors.WPI_CANCoder; +import edu.wpi.first.math.VecBuilder; +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.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.Robot; +import frc.robot.constants.Constants; +import frc.robot.constants.DrivetrainConfs; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.ModuleControl; +import frc.robot.utils.CTREConversion; +import frc.robot.utils.CTREModuleState; + +public class SwerveModule { + public static final DrivetrainConfs DRIVE_CONSTS = Constants.CRobot.drive; + + /* --- Module Identifiers --- */ + public final int _moduleNumber; + public final String _moduleNumberStr; + + /* --- Sensors, motors, and hardware --- */ + private final WPI_TalonFX _angleMotor; + private final WPI_TalonFX _driveMotor; + + private AnalogInput _analogInput; + private AnalogPotentiometer _angleEncoder; + private WPI_CANCoder _angleEncoderCAN; + + /* --- State and Physical Property variables --- */ + private SwerveModuleState _desiredState; + private SwerveModuleState _curState = new SwerveModuleState(); + private SwerveModulePosition _curPosition = new SwerveModulePosition(); + private final double _angleOffsetDeg; + private double _lastAngleDeg; + private double _percentOutput = 0.0; + private double _velocity = 0.0; + private double _angleDeg = 0.0; + private double _absolutePosition; + private final String _canBusName; + + /* --- Control Utils --- */ + private static CANCoderConfiguration _swerveCanCoderConfig; + private static StatorCurrentLimitConfiguration _driveStatorLimit; + private static StatorCurrentLimitConfiguration _autonomousDriveStatorLimit; + + // TODO: Move constants but not class into separate enum or configuration + private final SimpleMotorFeedforward _feedforward = new SimpleMotorFeedforward( + ModuleControl.ElectricalConf.DRIVE_KS_VOLT, + ModuleControl.ElectricalConf.DRIVE_KV_VOLTPMPS, + ModuleControl.ElectricalConf.DRIVE_KA_VOLTPMPS_SQ + ); + + /* --- Simulation resources and variables --- */ + private TalonFXSimCollection _driveMotorSim; + private TalonFXSimCollection _angleMotorSim; + + private FlywheelSim _driveWheelSim; + + private SingleJointedArmSim _moduleAngleSim; + + /* --- Shuffleboard entries --- */ + public GenericEntry _steerAnglePFac, _steerAngleIFac, _steerAngleDFac; + public GenericEntry _steerPFac, _steerIFac, _steerDFac; + + // telemetry widgets + public GenericEntry _shuffleboardModuleCANcoder; + public GenericEntry _shuffleboardModuleAngle; + public GenericEntry _shuffleboardModuleSpeed; + + public SwerveModule(int moduleNumber, String canBusName) { + _moduleNumber = moduleNumber; + _canBusName = canBusName; + _moduleNumberStr = "M" + Integer.toString(_moduleNumber); + _angleOffsetDeg = DRIVE_CONSTS.moduleOffsetsDeg[_moduleNumber]; + initCanCoderConfigs(); + + _angleMotor = new WPI_TalonFX(DRIVE_CONSTS.ports.steer[_moduleNumber], canBusName); + _driveMotor = new WPI_TalonFX(DRIVE_CONSTS.ports.drive[_moduleNumber], canBusName); + + waitForCAN(); + + // Must be done before the angle motor is configured + configAngleEncoder(); + + configAngleMotor(); + + configDriveMotor(); + + configShuffleboard(); + + // probably redundant + resetToAbsolute(); + + simulationInit(); + } + + public SwerveModule(int moduleNumber) { + this(moduleNumber, ""); + } + + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + setDesiredState(desiredState, isOpenLoop, false); + } + + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean is254) { + if (_angleMotor.hasResetOccurred()) { + resetToAbsolute(); + } + + // TODO cleanup + _desiredState = is254 ? CTREModuleState.optimize254(desiredState, getState().angle) + : CTREModuleState.optimize(desiredState, getState().angle); + // _desiredState = desiredState; + + if (isOpenLoop) { + _percentOutput = _desiredState.speedMetersPerSecond + / Constants.CRobot.drive.model.theoreticalMaxWheelSpeed; + _driveMotor.set(ControlMode.PercentOutput, _percentOutput); + } else { + _velocity = CTREConversion.MPSToFalcon( + _desiredState.speedMetersPerSecond, + DRIVE_CONSTS.type.wheelCircumferenceM, + DRIVE_CONSTS.model.driveRatio + ); + _driveMotor.set( + ControlMode.Velocity, + _velocity, + DemandType.ArbitraryFeedForward, + _feedforward.calculate(_desiredState.speedMetersPerSecond) + ); + } + + _angleDeg = Math.abs(_desiredState.speedMetersPerSecond) + <= (Constants.CRobot.drive.model.theoreticalMaxWheelSpeed * 0.01) + ? _lastAngleDeg + : _desiredState.angle.getDegrees(); + _angleMotor.set( + ControlMode.Position, + CTREConversion.degreesToFalcon(_angleDeg, DRIVE_CONSTS.model.steerRatio) + ); + _lastAngleDeg = _angleDeg; + } + + public double getDegreeAngleEncoder() { + if (Constants.isCANEncoder) { + return _angleEncoderCAN.getAbsolutePosition(); + } + return _angleEncoder.get() + _angleOffsetDeg; + } + + public SwerveModuleState getState() { + _curState.speedMetersPerSecond = CTREConversion.falconToMPS( + _driveMotor.getSelectedSensorVelocity(), + DRIVE_CONSTS.type.wheelCircumferenceM, + DRIVE_CONSTS.model.driveRatio + ); + _curState.angle = CTREConversion.falconToRotation2d( + _angleMotor.getSelectedSensorPosition(), DRIVE_CONSTS.model.steerRatio + ); + + return _curState; + } + + public SwerveModulePosition getPosition() { + _curPosition.distanceMeters = CTREConversion.falconToMeters( + _driveMotor.getSelectedSensorPosition(), + DRIVE_CONSTS.type.wheelCircumferenceM, + DRIVE_CONSTS.model.driveRatio + ); + _curPosition.angle = CTREConversion.falconToRotation2d( + _angleMotor.getSelectedSensorPosition(), DRIVE_CONSTS.model.steerRatio + ); + // might be necessary + // Rotation2d.fromDegrees(CTREConversion.falconToDegrees( + // _angleMotor.getSelectedSensorPosition(), DRIVE_CONSTS.model.steerRatio + // )); + + return _curPosition; + } + + private void waitForCAN() { + if (Robot.isReal()) { + int counter = 0; + String identifier = "SWERVE " + _moduleNumberStr + " Check Init Status : "; + while (!checkInitStatus()) { + System.out.println(identifier + counter++); + } + } else { + } + } + + public void resetToAbsolute() { + _lastAngleDeg = getDegreeAngleEncoder(); + _absolutePosition = + CTREConversion.degreesToFalcon(_lastAngleDeg, DRIVE_CONSTS.model.steerRatio); + _angleMotor.setSelectedSensorPosition(_absolutePosition); + } + + public void configShuffleboard() { + if (RobotAltModes.isVerboseMode) { + ShuffleboardTab tab = Shuffleboard.getTab("SwerveModules"); + ShuffleboardLayout layout = tab.getLayout(_moduleNumberStr, BuiltInLayouts.kGrid) + .withSize(1, 3) + .withPosition(_moduleNumber, 0); + + _shuffleboardModuleAngle = layout.add("angleState", 0.0).withPosition(0, 0).getEntry(); + _shuffleboardModuleCANcoder = layout.add("angleCANcoder", 0.0).withPosition(0, 1).getEntry(); + _shuffleboardModuleSpeed = layout.add("speed", 0.0).withPosition(0, 2).getEntry(); + } + } + + public void updateShuffleboard() { + if (RobotAltModes.isVerboseMode) { + _shuffleboardModuleAngle.setDouble(_curState.angle.getDegrees()); + _shuffleboardModuleCANcoder.setDouble(getDegreeAngleEncoder()); + _shuffleboardModuleSpeed.setDouble(_curState.speedMetersPerSecond); + } + } + + public void alignToZero() { + resetToAbsolute(); + _lastAngleDeg = 0; + } + + public boolean motorResetConfig() { + boolean result = false; + if (Constants.isCANEncoder && _angleEncoderCAN.hasResetOccurred()) { + configCanEncoder(); + result = true; + } + if (_angleMotor.hasResetOccurred()) { + configAngleMotor(); + result = true; + } + if (_driveMotor.hasResetOccurred()) { + configDriveMotor(); + result = true; + } + return result; + } + + public void configPID(double ap, double ai, double ad, double p, double i, double d) { + if (RobotAltModes.isPIDTuningMode) { + Constants.CRobot.drive.moduleControl.steerConf.slot0.kP = ap; + Constants.CRobot.drive.moduleControl.steerConf.slot0.kI = ai; + Constants.CRobot.drive.moduleControl.steerConf.slot0.kD = ad; + + Constants.CRobot.drive.moduleControl.driveConf.slot0.kP = p; + Constants.CRobot.drive.moduleControl.driveConf.slot0.kI = i; + Constants.CRobot.drive.moduleControl.driveConf.slot0.kD = d; + + configAngleMotor(); + configDriveMotor(); + } + } + + public void setLastAngle(SwerveModuleState desiredState) { + SwerveModuleState goalModuleState = CTREModuleState.optimize(desiredState, getState().angle); + _lastAngleDeg = goalModuleState.angle.getDegrees(); + } + + public void setLastAngle(Rotation2d angle) { + setLastAngleDeg(angle.getDegrees()); + } + + public void setLastAngleDeg(double angle_deg) { + _lastAngleDeg = angle_deg; + } + + public boolean isAlignedTo(SwerveModuleState goalState, double tolerance_deg) { + return Math.abs(_angleDeg - goalState.angle.getDegrees()) < tolerance_deg; + } + + public boolean isAlignedTo(SwerveModuleState goalState) { + return isAlignedTo(goalState, DRIVE_CONSTS.alignmentToleranceDeg); + } + + public void autonomousDriveMode(boolean enable) { + _driveMotor.enableVoltageCompensation(enable); + _driveMotor.configStatorCurrentLimit( + enable ? ModuleControl.ElectricalConf.AUTO_DRIVE_STATOR_LIMIT + : ModuleControl.ElectricalConf.DRIVE_STATOR_LIMIT + ); + _angleMotor.enableVoltageCompensation(enable); + } + + private void configAngleEncoder() { + if (Constants.isCANEncoder) { + initCanEncoder(); + configCanEncoder(); + } else { + configAnalogEncoder(); + } + } + + private void initCanEncoder() { + _angleEncoderCAN = new WPI_CANCoder(DRIVE_CONSTS.ports.encoder[_moduleNumber], _canBusName); + } + + private void configCanEncoder() { + _angleEncoderCAN.configFactoryDefault(100); + _angleEncoderCAN.configAllSettings(_swerveCanCoderConfig); + _angleEncoderCAN.configMagnetOffset(_angleOffsetDeg); + } + + private void configAnalogEncoder() { + _analogInput = new AnalogInput(DRIVE_CONSTS.ports.encoder[_moduleNumber]); + _angleEncoder = new AnalogPotentiometer(_analogInput, 360.0); + _analogInput.setAverageBits(2); + } + + private void configAngleMotor() { + _angleMotor.configFactoryDefault(); + _angleMotor.configAllSettings(Constants.CRobot.drive.moduleControl.steerConf); + _angleMotor.setInverted(Constants.CRobot.drive.model.type.invertSteer); + _angleMotor.setNeutralMode(ModuleControl.STEER_NEUTRAL_MODE); + + resetToAbsolute(); + } + + private void configDriveMotor() { + _driveMotor.configFactoryDefault(); + _driveMotor.configAllSettings(Constants.CRobot.drive.moduleControl.driveConf); + _driveMotor.setInverted(Constants.CRobot.drive.model.type.invertDrive); + _driveMotor.setNeutralMode(ModuleControl.DRIVE_NEUTRAL_MODE); + _driveMotor.setSelectedSensorPosition(0); + } + + private boolean checkInitStatus() { + return _angleMotor.configFactoryDefault() == ErrorCode.OK; + } + + public void simulationInit() { + if (!Robot.isReal()) + _driveMotorSim = _driveMotor.getSimCollection(); + _angleMotorSim = _angleMotor.getSimCollection(); + + _driveWheelSim = new FlywheelSim( + DCMotor.getFalcon500(1), + DRIVE_CONSTS.model.driveRatio, + 0.01, + VecBuilder.fill(2.0 * Math.PI / 2048) + ); + + _moduleAngleSim = new SingleJointedArmSim( + DCMotor.getFalcon500(1), + DRIVE_CONSTS.model.steerRatio, + 0.001, + 0.0, + -Double.MAX_VALUE, + Double.MAX_VALUE, + false, + VecBuilder.fill(2.0 * Math.PI / 2048) + ); + } + + public void simulationPeriodic() { + if (RobotAltModes.isSim) { + // Update the model motor sim _driveWheelSim, and read its angular velocity + _driveWheelSim.setInputVoltage(_driveMotor.getMotorOutputVoltage()); + _driveWheelSim.update(Constants.LOOP_PERIOD_MS); + + double driveSimOmega = _driveWheelSim.getAngularVelocityRPM(); + int driveTicksPer100ms = (_driveMotor.getInverted() ? -1 : 1) + * CTREConversion.RPMToFalcon(driveSimOmega, DRIVE_CONSTS.model.driveRatio); + + // Update integrated sensor sim in _driveMotor + _driveMotorSim.setIntegratedSensorVelocity(driveTicksPer100ms); + _driveMotorSim.addIntegratedSensorPosition((int + ) (driveTicksPer100ms * Constants.LOOP_PERIOD_MS / 100.0)); + + // Update _steeringSim single-arm simulation + _moduleAngleSim.setInputVoltage(_angleMotor.getMotorOutputVoltage()); + _moduleAngleSim.update(Constants.LOOP_PERIOD_MS); + + // Update integratedSensor sim in _angleMotor + int angleSign = _angleMotor.getInverted() ? -1 : 1; + _angleMotorSim.setIntegratedSensorVelocity( + angleSign + * CTREConversion.RPMToFalcon( + Units.radiansPerSecondToRotationsPerMinute(_moduleAngleSim.getVelocityRadPerSec()), + DRIVE_CONSTS.model.steerRatio + ) + ); + _angleMotorSim.setIntegratedSensorRawPosition( + angleSign + * CTREConversion.radiansToFalcon( + _moduleAngleSim.getAngleRads(), DRIVE_CONSTS.model.steerRatio + ) + ); + } + } + + private static void initCanCoderConfigs() { + if (Constants.isCANEncoder && _swerveCanCoderConfig == null) { + /* Swerve CANCoder Configuration */ + _swerveCanCoderConfig = new CANCoderConfiguration(); + + _swerveCanCoderConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; + _swerveCanCoderConfig.sensorDirection = Constants.CRobot.drive.moduleControl.invertEncoder; + _swerveCanCoderConfig.initializationStrategy = + SensorInitializationStrategy.BootToAbsolutePosition; + _swerveCanCoderConfig.sensorTimeBase = SensorTimeBase.PerSecond; + } + } + + // Ryan's 254 work + public double getUnclampedSteerAngleRadians() { + return (_angleMotor.getSelectedSensorPosition() * kSteerPositionCoefficient) + - Units.degreesToRadians(_angleOffsetDeg); + } + + private void setSteerAngleUnclamped(double steerAngleRadians) { + _angleMotor.set( + TalonFXControlMode.Position, + (steerAngleRadians + Units.degreesToRadians(_angleOffsetDeg)) / kSteerPositionCoefficient + ); + } + + public void setWithVoltageShortestPath(double drivePercentage, Rotation2d steerAngle) { + final boolean flip = setSteerAngleShortestPath(steerAngle); + _driveMotor.set(TalonFXControlMode.PercentOutput, flip ? -drivePercentage : drivePercentage); + } + + public void setWithVelocityShortestPath(double driveVelocity, Rotation2d steerAngle) { + final boolean flip = setSteerAngleShortestPath(steerAngle); + + _velocity = CTREConversion.MPSToFalcon( + driveVelocity, DRIVE_CONSTS.type.wheelCircumferenceM, DRIVE_CONSTS.model.driveRatio + ); + _driveMotor.set( + TalonFXControlMode.Velocity, (flip ? -_velocity : _velocity) / (kDriveVelocityCoefficient) + ); + } + + private final double kDrivePositionCoefficient = Math.PI + * Constants.CRobot.drive.model.type.wheelDiameterM * Constants.CRobot.drive.model.driveRatio + / 2048.0; + private final double kDriveVelocityCoefficient = + kDrivePositionCoefficient * Constants.LOOP_PERIOD_MS; + private final double kSteerPositionCoefficient = + 2.0 * Math.PI / 2048.0 * Constants.CRobot.drive.model.steerRatio; + + public void setWithVoltageUnclamped(double drivePercentage, double steerAngleRadians) { + setSteerAngleUnclamped(steerAngleRadians); + _driveMotor.set(TalonFXControlMode.PercentOutput, drivePercentage); + } + + public void setWithVelocityUnclamped(double driveVelocity, double steerAngleRadians) { + setSteerAngleUnclamped(steerAngleRadians); + _driveMotor.set(TalonFXControlMode.Velocity, driveVelocity / kDriveVelocityCoefficient); + } + + // Returns true if the drive velocity should be inverted. + private boolean setSteerAngleShortestPath(Rotation2d steerAngle) { + boolean flip = false; + final double unclampedPosition = getUnclampedSteerAngleRadians(); + final Rotation2d clampedPosition = Rotation2d.fromRadians(unclampedPosition); + final Rotation2d relativeRotation = steerAngle.rotateBy(clampedPosition.unaryMinus()); + double relativeRadians = relativeRotation.getRadians(); + final double kPiOver2 = Math.PI / 2.0; + if (relativeRadians > kPiOver2) { + // Flipping drive direction would be the shorter path. + flip = true; + relativeRadians -= Math.PI; + } else if (relativeRadians < -kPiOver2) { + // Flipping drive direction would be the shorter path. + flip = true; + relativeRadians += Math.PI; + } + setSteerAngleUnclamped(unclampedPosition + relativeRadians); + + return flip; + } +} diff --git a/src/main/java/frc/robot/subsystems/VisionController.java b/src/main/java/frc/robot/subsystems/VisionController.java new file mode 100644 index 0000000..13b269f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/VisionController.java @@ -0,0 +1,240 @@ +package frc.robot.subsystems; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.VisionConfs; +import frc.robot.utils.LoopTimer; +import java.util.List; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class VisionController extends SubsystemBase { + private Matrix _stdDevs; + private Pose2d _latestPose = new Pose2d(); + + GenericEntry _photonDistanceEntry; + GenericEntry _photonYawEntry; + GenericEntry _photonPitchEntry; + + private static final AprilTagFieldLayout _aprilTagFieldLayout = new AprilTagFieldLayout( + Constants.CField.aprilTags, Constants.CField.dims.x_M, Constants.CField.dims.y_M + ); + + // Cameras: "OV5647" "Microsoft_LifeCam_HD-3000" + // Cameras: "Arducam_Forward", "Arducam_Backward" + private String _cameraName; + private PhotonCamera _camera; + private Transform3d _robotToCamera; + private Transform3d _multiTagRobotToCamera; + private PhotonPoseEstimator _visionPoseEstimator; + + private boolean _usingMultiTagTransform = false; + private Matrix _maxDevs = + VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + private Timer timer = new Timer(); + + public VisionController(String cameraName, Transform3d robotToCamera) { + _cameraName = cameraName; + _robotToCamera = robotToCamera; + + // Has to happen after the cameraName and _robotToCamera is given a value + _camera = new PhotonCamera(_cameraName); + _multiTagRobotToCamera = new Transform3d( + _robotToCamera.getTranslation(), + // This may be C++ specific and we can just do + // _robotToCamera.getRotation(); + new Rotation3d( + _robotToCamera.getRotation().getX(), + -_robotToCamera.getRotation().getY(), + _robotToCamera.getRotation().getZ() + ) + ); + _visionPoseEstimator = new PhotonPoseEstimator( + _aprilTagFieldLayout, PoseStrategy.LOWEST_AMBIGUITY, _camera, _robotToCamera + ); + } + + public VisionController() { + this("", new Transform3d()); + } + + public EstimatedRobotPose getVisionPose3d() { + var result = _camera.getLatestResult(); + List targets = result.getTargets(); + + boolean redTarget = false; + boolean blueTarget = false; + + for (final PhotonTrackedTarget target : targets) { + int id = target.getFiducialId(); + if (id > 8 || id < 1) { + // illegal tag + return null; + } + if (id < 5) { + if (blueTarget) { + // Invalid tag combo + return null; + } + redTarget = true; + } else { + // Invalid tag combo + if (redTarget) { + return null; + } + blueTarget = true; + } + } + + // return _visionPoseEstimator.update().orElse(null); + // ^^ Original, C++ could not reuse the result from earlier + return _visionPoseEstimator.update(result).orElse(null); + } + + public double updateVisionPoseStdDevs(Pose2d pose) { + EstimatedRobotPose result = getVisionPose3d(); + + LoopTimer.markEvent("VISION 0"); + + if (result == null) { + _latestPose = pose; + _stdDevs = _maxDevs; + return 0.0; + } + + // System.out.println("----VC" + result.estimatedPose.getX()); + // System.out.println("----VC" + result.estimatedPose.getY()); + + int targetsUsedSize = result.targetsUsed.size(); + + LoopTimer.markEvent("VISION 0.5"); + + if (targetsUsedSize > 1 && !_usingMultiTagTransform) { + _visionPoseEstimator.setRobotToCameraTransform(_multiTagRobotToCamera); + _usingMultiTagTransform = true; + + _latestPose = pose; + _stdDevs = _maxDevs; + return 0.0; + } else if (targetsUsedSize == 1 && _usingMultiTagTransform) { + _visionPoseEstimator.setRobotToCameraTransform(_robotToCamera); + _usingMultiTagTransform = false; + + _latestPose = pose; + _stdDevs = _maxDevs; + return 0.0; + } + + LoopTimer.markEvent("VISION 1"); + + double smallestDistance = Double.MAX_VALUE; + + for (PhotonTrackedTarget target : result.targetsUsed) { + double distance = target.getBestCameraToTarget().getTranslation().getNorm(); + if (distance < smallestDistance) { + smallestDistance = distance; + } + } + + if (smallestDistance > Constants.CField.dims.halfX_M + || (!_usingMultiTagTransform && smallestDistance > VisionConfs.SINGLE_TAG_MAX_DIST_M)) { + _latestPose = pose; + _stdDevs = _maxDevs; + return 0.0; + } + + LoopTimer.markEvent("VISION 2"); + double ambiguity = targetsUsedSize != 1 + ? 1.0 + : VisionConfs.AMBIGUITY_SCALE * result.targetsUsed.get(0).getPoseAmbiguity() + + VisionConfs.AMBIGUITY_CONSTANT; + + double xDev = VisionConfs.X_DISTANCE_SCALE + * Math.exp(VisionConfs.X_DISTANCE_POW * smallestDistance) * ambiguity; + double yDev = VisionConfs.Y_DISTANCE_SCALE + * Math.exp(VisionConfs.Y_DISTANCE_POW * smallestDistance) * ambiguity; + double thetaDev = + VisionConfs.THETA_SCALE * Math.log(smallestDistance) * VisionConfs.THETA_CONSTANT; + + LoopTimer.markEvent("VISION 3"); + + _latestPose = result.estimatedPose.toPose2d(); + _stdDevs = VecBuilder.fill(xDev, yDev, thetaDev); + + return result.timestampSeconds; + } + + public Pose2d getLatestPose() { + return _latestPose; + } + + public double getLatestTimestamp(Pose2d pose) { + return updateVisionPoseStdDevs(pose); + } + + public Matrix getLatestStdDevs() { + return _stdDevs; + } + + public Matrix getMaxDevs() { + return _maxDevs; + } + + public void setDriverMode() { + _camera.setDriverMode(true); + } + + public void set3dMode() { + if (_camera.getPipelineIndex() != VisionConfs.PIPELINE_INDEX_3D) { + _camera.setPipelineIndex(VisionConfs.PIPELINE_INDEX_3D); + } + } + + public void set2dMode() { + if (_camera.getPipelineIndex() != VisionConfs.PIPELINE_INDEX_2D) { + _camera.setPipelineIndex(VisionConfs.PIPELINE_INDEX_2D); + } + } + + public void configShuffleboard() { + ShuffleboardTab drivetrainTab = Shuffleboard.getTab("Drivetrain"); + ShuffleboardLayout photonLayout = + drivetrainTab.getLayout("Photon " + _cameraName, BuiltInLayouts.kGrid) + .withSize(2, 2) + .withPosition(2, 0); + _photonDistanceEntry = photonLayout.add("Photon " + _cameraName + " Distance", 0.0) + .withSize(2, 1) + .withPosition(0, 0) + .getEntry(); + _photonYawEntry = photonLayout.add("Photon " + _cameraName + " Yaw", 0.0) + .withSize(2, 1) + .withPosition(0, 1) + .getEntry(); + _photonPitchEntry = photonLayout.add("Photon " + _cameraName + " Pitch", 0.0) + .withSize(2, 1) + .withPosition(0, 2) + .getEntry(); + } + + public void updateShuffleboard(double yaw, double distance, double pitch) { + _photonPitchEntry.setDouble(pitch); + _photonYawEntry.setDouble(yaw); + _photonDistanceEntry.setDouble(distance); + } +} diff --git a/src/main/java/frc/robot/utils/AlliancePose2d.java b/src/main/java/frc/robot/utils/AlliancePose2d.java new file mode 100644 index 0000000..52d8f90 --- /dev/null +++ b/src/main/java/frc/robot/utils/AlliancePose2d.java @@ -0,0 +1,16 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Pose2d; + +public class AlliancePose2d { + public final Pose2d blue, red; + + public AlliancePose2d(Pose2d blue, Pose2d red) { + this.blue = blue; + this.red = red; + } + + public Pose2d get(boolean isRed) { + return isRed ? red : blue; + } +} diff --git a/src/main/java/frc/robot/utils/CTREConversion.java b/src/main/java/frc/robot/utils/CTREConversion.java new file mode 100644 index 0000000..19f877e --- /dev/null +++ b/src/main/java/frc/robot/utils/CTREConversion.java @@ -0,0 +1,94 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.constants.Constants; + +public class CTREConversion { + // Positional conversions + public static double falconToDegrees(double ticks, double gearRatio) { + return falconToDegrees((int) ticks, gearRatio); + } + + public static double falconToDegrees(int ticks, double gearRatio) { + return ticks * Constants.CTREConstants.FALCON_TO_DEG / gearRatio; + } + + public static int degreesToFalcon(double degrees, double gearRatio) { + return (int) (degrees * Constants.CTREConstants.DEG_TO_FALCON * gearRatio); + } + + public static double falconToRadians(int ticks, double gearRatio) { + return ticks * Constants.CTREConstants.FALCON_TO_RAD / gearRatio; + } + + public static Rotation2d falconToRotation2d(double ticks, double gearRatio) { + return falconToRotation2d((int) ticks, gearRatio); + } + + public static Rotation2d falconToRotation2d(int ticks, double gearRatio) { + return Rotation2d.fromRadians(ticks * Constants.CTREConstants.FALCON_TO_RAD / gearRatio); + } + + public static int radiansToFalcon(double radians, double gearRatio) { + return (int) (radians * Constants.CTREConstants.RAD_TO_FALCON * gearRatio); + } + + public static double degreesToCANCoder(double degrees, double gearRatio) { + return degrees * Constants.CTREConstants.DEG_TO_CANCODER * gearRatio; + } + + public static double falconToMeters(double ticks, double circumference_M, double gearRatio) { + return falconToMeters((int) ticks, circumference_M, gearRatio); + } + + public static double falconToMeters(int ticks, double circumference_M, double gearRatio) { + return ticks * Constants.CTREConstants.FALCON_TICKS_TO_ROT * circumference_M / gearRatio; + } + + public static int metersToFalcon(double meters, double circumference_M, double gearRatio) { + return (int + ) (meters * Constants.CTREConstants.ROT_TO_FALCON_TICKS * gearRatio / circumference_M); + } + + public static double nativeToHeightMeters( + double ticks, double circumference_M, double gearRatio + ) { + return nativeToHeightMeters((int) ticks, circumference_M, gearRatio); + } + + public static double nativeToHeightMeters(int ticks, double circumference_M, double gearRatio) { + return ticks * circumference_M / (Constants.CTREConstants.FALCON_TICKS_TO_ROT * gearRatio); + } + + public static double heightMetersToNative( + double height_M, double circumference_M, double gearRatio + ) { + return height_M * Constants.CTREConstants.FALCON_ENCODER_TICKS * gearRatio / circumference_M; + } + + // Rotational rate conversions (time unit of minutes) + public static double falconToRPM(double tickVelocity, double gearRatio) { + return tickVelocity * Constants.CTREConstants.FALCON_TO_RPM / gearRatio; + } + + public static int RPMToFalcon(double rpm, double gearRatio) { + return (int) (rpm * Constants.CTREConstants.RPM_TO_FALCON * gearRatio); + } + + public static double RPMToCANCoder(double rpm, double gearRatio) { + // RPM --> cancoder RPM --> cancoder ticks per 100ms + return rpm * gearRatio * Constants.CTREConstants.RPM_TO_CANCODER; + } + + // Translational rate conversions (time unit of seconds) + public static double falconToMPS(double tickVelocity, double circumference_M, double gearRatio) { + // ticks per 100ms --> motor RPS --> wheel/drum RPS --> wheel drum surface speed MPS + return tickVelocity * Constants.CTREConstants.FALCON_TO_RPS * circumference_M / gearRatio; + } + + public static double MPSToFalcon(double velocity_MPS, double circumference_M, double gearRatio) { + // RPS_TO_FALCON units = (ticks / 100ms) / (motor rotations/sec) + // MPS / C --> RPS --> * gear ratio --> motor RPS + return Constants.CTREConstants.RPS_TO_FALCON * velocity_MPS * gearRatio / circumference_M; + } +} diff --git a/src/main/java/frc/robot/utils/CTREModuleState.java b/src/main/java/frc/robot/utils/CTREModuleState.java new file mode 100644 index 0000000..9aabf9c --- /dev/null +++ b/src/main/java/frc/robot/utils/CTREModuleState.java @@ -0,0 +1,68 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; + +// TODO: Move all into a relevant class +public class CTREModuleState { + /** + * @param scopeReference Current Angle + * @param newAngle Target Angle + * @return Closest angle within -180 to 180 scope + */ + public static double placeInClosestScopeDeg(double currentAngle, double desiredAngle) { + double delta = (desiredAngle - currentAngle) % 360; + if (delta > 180.0) { + delta += -360.0; + } else if (delta < -180.0) { + delta += 360.0; + } + return currentAngle + delta; + } + + public static double placeInClosestScopeRad(double scopeReference_rad, double newAngle_rad) { + return Units.degreesToRadians(placeInClosestScopeDeg( + Units.radiansToDegrees(scopeReference_rad), Units.radiansToDegrees(newAngle_rad) + )); + } + + public static SwerveModuleState optimize254( + SwerveModuleState desiredState, Rotation2d currentAngle + ) { + // Place in closest scope + double delta = (desiredState.angle.getDegrees() - currentAngle.getDegrees()) % 360; + if (delta > 180.0) { + delta += -360.0; + } else if (delta < -180.0) { + delta += 360.0; + } + double targetAngle_deg = currentAngle.getDegrees() + delta; + double targetSpeed_mps = desiredState.speedMetersPerSecond; + return new SwerveModuleState(targetSpeed_mps, Rotation2d.fromDegrees(targetAngle_deg)); + } + + public static SwerveModuleState optimize( + SwerveModuleState desiredState, Rotation2d currentAngle + ) { + // Place in closest scope + double delta = (desiredState.angle.getDegrees() - currentAngle.getDegrees()) % 360; + if (delta > 180.0) { + delta += -360.0; + } else if (delta < -180.0) { + delta += 360.0; + } + double targetAngle_deg = currentAngle.getDegrees() + delta; + + // Actual optimize + double targetSpeed_mps = desiredState.speedMetersPerSecond; + if (delta > 90.0) { + targetSpeed_mps = -targetSpeed_mps; + targetAngle_deg += -180.0; + } else if (delta < -90.0) { + targetSpeed_mps = -targetSpeed_mps; + targetAngle_deg += 180.0; + } + return new SwerveModuleState(targetSpeed_mps, Rotation2d.fromDegrees(targetAngle_deg)); + } +} diff --git a/src/main/java/frc/robot/utils/ControlboardViz.java b/src/main/java/frc/robot/utils/ControlboardViz.java new file mode 100644 index 0000000..6c5d531 --- /dev/null +++ b/src/main/java/frc/robot/utils/ControlboardViz.java @@ -0,0 +1,376 @@ +package frc.robot.utils; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.enums.GamePieceType; +import frc.robot.constants.enums.HPIntakeStations; +import frc.robot.constants.enums.ScoringLevels; +import frc.robot.constants.enums.ScoringLocations; + +public class ControlboardViz { + private Alliance _alliance; + private Field2d _field; + private boolean _vizText; + private boolean _vizGlass; + private boolean _vizField; + // frc::Mechanism2d _mech2d{CBVIZ_XSIZE, CBVIZ_YSIZE}; + // frc::MechanismLigament2d* _scoreLigament; + // frc::MechanismLigament2d* _heldPieceLigament; + private GenericEntry _desiredScoringLevelEntry; + private GenericEntry _desiredScoringLocationEntry; + private GenericEntry _desiredIntakeEntry; + private GenericEntry _desiredGamePieceEntry; + private GenericEntry _heldGamePieceEntry; + // ConeCube _scorePieceShape, _heldPieceShape, _desiredPieceShape; + // ConeCube _allianceOutline; + + public ControlboardViz(Field2d field, boolean vizNumbers, boolean vizGlass, boolean vizField) { + _field = field; + _vizText = vizNumbers; + _vizGlass = vizGlass; + _vizField = vizField; + + initVizText(); + initGlass(); + + LoopTimer.markEvent(" Visualizer Initialization Complete: "); + } + + public void update( + ScoringLocations desiredLocation, + ScoringLevels desiredLevel, + HPIntakeStations desiredIntake, + GamePieceType desiredGamePiece, + GamePieceType heldGamePiece + ) { + LoopTimer.markLoopStart(); + + final int loc = desiredLocation.ordinal(); + if (_vizText) { + if (desiredLocation == ScoringLocations.UNKNOWN_SCORING_LOCATION) + _desiredScoringLocationEntry.setString("UNKNOWN SCORING LOCATION"); + else + _desiredScoringLocationEntry.setString( + "LOCATION " + Integer.toString(loc) + ((loc % 3 == 2) ? "CUBE" : "CONE") + ); + _desiredScoringLevelEntry.setString(desiredLevel.label); + _desiredGamePieceEntry.setString(desiredGamePiece.label + " DESIRED"); + _heldGamePieceEntry.setString(heldGamePiece.label + " HELD"); + _desiredIntakeEntry.setString(desiredIntake.toString()); + } + // UpdateGlass(desiredLocation, desiredLevel, desiredIntake, desiredGamePiece, + // heldGamePiece); + + LoopTimer.markEvent(" Total Visualizer: "); + } + + public void updateAlliance(Alliance alliance) { + _alliance = alliance; + // frc::Color8Bit color; + // switch (_alliance) { + // case frc::DriverStation::kRed: color = frc::Color8Bit(frc::Color::kFirstRed); + // break; + // case frc::DriverStation::kBlue: color = + // frc::Color8Bit(frc::Color::kFirstBlue); break; + // default: color = frc::Color8Bit(frc::Color::kYellow); break; + // } + // ColorShape(_allianceOutline, color); + } + + // double ControlboardViz::scaleX(meter_t x) { return CBVIZ_XSIZE * (x + 1_ft) / + // 18_ft; } + // double ControlboardViz::scaleY(meter_t y) { return CBVIZ_YSIZE * (y + 1_ft) / + // 8_ft; } + + // frc::MechanismLigament2d* ControlboardViz::Draw( + // meter_t x0, meter_t y0, meter_t x1, meter_t y1, int width, frc::Color8Bit + // color) + // { + + // // static int _id; + + // // _id++; // each segment needs a unique id + + // // double const windowX0 = scaleX(x0); + // // double const windowX1 = scaleX(x1); + // // double const windowY0 = scaleY(y0); + // // double const windowY1 = scaleY(y1); + // // double const deltaX = windowX1 - windowX0; + // // double const deltaY = windowY1 - windowY0; + + // // frc::MechanismRoot2d* start = _mech2d.GetRoot("Line" + + // std::to_string(_id), windowX0, + // // windowY0); auto angle = radian_t{std::atan2(deltaY, deltaX)}; int + // length + // = + // // std::sqrt(deltaX * deltaX + deltaY * deltaY); + + // // return start->Append("Line", length, angle, + // width, color); + // return {}; + // } + + private void initVizText() { + if (_vizText) { + ShuffleboardTab tab = Shuffleboard.getTab("Competition HUD"); + + _desiredScoringLevelEntry = + tab.add("Scoring Level", " ").withSize(2, 1).withPosition(2, 0).getEntry(); + _desiredScoringLocationEntry = + tab.add("Scoring Location", " ").withSize(2, 1).withPosition(0, 0).getEntry(); + _desiredGamePieceEntry = + tab.add("Desired Game Piece", " ").withSize(2, 1).withPosition(6, 0).getEntry(); + _heldGamePieceEntry = + tab.add("Held Game Piece", " ").withSize(2, 1).withPosition(4, 0).getEntry(); + _desiredIntakeEntry = + tab.add("Intake Location", " ").withSize(2, 1).withPosition(8, 0).getEntry(); + } + } + + private void initGlass() { + // if (!_vizGlass) return; + + // grid verticals + // Draw(0_ft, 0_ft, 0_ft, height); // left edge + // for (int i = 0; i < 8; i++) { + // meter_t x = GRID_VIZ_PITCH / 6 + (i + 1) * GRID_VIZ_PITCH; + // Draw(x, 0_ft, x, height, (i % 3 == 2) ? 2 : 1); + // } + // Draw(width, 0_ft, width, height); + + // grid horizontals + // Draw(0_ft, height / 3, width, height / 3); + // Draw(0_ft, 2 * height / 3, width, 2 * height / 3); + // Draw(0_ft, height, width, height); + + // frc::MechanismRoot2d* markerStart = _mech2d.GetRoot("Marker", scaleX(baseX), + // scaleY(baseY)); + // _scoreLigament = markerStart->Append( + // "ScoreLigament", 0, 90_deg, 0, frc::Color8Bit(frc::Color::kBlack)); + // InitRectangle(_scorePieceShape, _scoreLigament); + + // outer border, color-coded for alliance + // _allianceOutline.rt = Draw(0_ft, 0_ft, width, 0_ft, 6); + // _allianceOutline.up = Draw(width, 0_ft, width, height, 6); + // _allianceOutline.lf = Draw(width, height, 0_ft, height, 6); + // _allianceOutline.dn = Draw(0_ft, height, 0_ft, 0_ft, 6); + // UpdateAlliance(_alliance); // sets proper color + + // held game piece + // frc::MechanismRoot2d* heldPieceStart = + // _mech2d.GetRoot("HeldPiece", CBVIZ_XSIZE - cubeSidePix * 2, scaleY(baseY)); + // _heldPieceLigament = heldPieceStart->Append( + // "HeldPieceLigament", CBVIZ_YSIZE / 2, 270_deg, 0, + // frc::Color8Bit(frc::Color::kBlack)); + // InitRectangle(_heldPieceShape, _heldPieceLigament); + + // frc::MechanismLigament2d* clawSupport = + // _heldPieceLigament->Append( + // "ClawSupport", clawSupportLen, CLAW_VIZ_ANGLE, 0, + // frc::Color8Bit(frc::Color::kBlack)); + // clawSupport->Append("ClawLeft", + // clawLenPix, + // 190_deg - 2 * CLAW_VIZ_ANGLE, + // 1, + // frc::Color8Bit(frc::Color::kWhite)); + // clawSupport->Append( + // "ClawRight", clawLenPix, 170_deg, 1, frc::Color8Bit(frc::Color::kWhite)); + + // // desired game piece + // frc::MechanismRoot2d* desiredPieceStart = + // _mech2d.GetRoot("DesiredPiece", CBVIZ_XSIZE - cubeSidePix * 2, + // scaleY(baseY)); + // frc::MechanismLigament2d* desiredPieceLigament = + // desiredPieceStart->Append( + // "DesiredPieceLigament", 0, 270_deg, 0, frc::Color8Bit(frc::Color::kBlack)); + // InitRectangle(_desiredPieceShape, desiredPieceLigament); + // RotateShape(_desiredPieceShape, 90_deg); + + // display it! + // frc::SmartDashboard::PutData("Controlboard", &_mech2d); + } + + private void updateGlass( + ScoringLocations desiredLocation, + ScoringLevels desiredLevel, + HPIntakeStations desiredIntake, + GamePieceType desiredGamePiece, + GamePieceType heldGamePiece + ) { + // if (!_vizGlass) return; + + // 1. update desired location & level on grid + + // left to right: red is 1-9, blue is 9-1 + // int locationIndex = (desiredLocation == UNKNOWN_SCORING_LOCATION) ? 0 + // : (_alliance == frc::DriverStation::kRed) ? desiredLocation + // : 10 - desiredLocation; + + // meter_t x, y; // location in physical world + + // Spacing of the viz grid is like this: + // Middle sections are spaced at GRID_VIZ_PITCH (call that P) + // First and last sections are spaced at 7P/6, ie 16% larger + // switch (locationIndex) { + // // should never happen + // case 0: x = -GRID_VIZ_MARGIN; break; + // // first section goes from 0 to 7P/6, so its midpoint is 7P/12 + // case 1: x = GRID_VIZ_PITCH * 7.0 / 12.0; break; + // // last section: + // // first offset 7P/6 to get past first section: +14P/12 + // // then +P for each section beyone section 2 : +P(location-2) + // // finally +7P/12 to midpoint of last section : +7P/12 + // case 9: x = GRID_VIZ_PITCH * ((21.0 / 12.0) - 2.0 + locationIndex); break; + // // middle sections: + // // first offset 7P/6 to get past first section: +7P/6 + // // then +P for each section beyond section 2 : +P(location-2) + // // finally +0.5P to midpoint of section : +P/2 + // default: x = GRID_VIZ_PITCH * ((7.0 / 6.0) - 1.5 + locationIndex); + // } + + // y = (desiredLevel == UNKNOWN_SCORING_LEVEL) ? baseY + // : GRID_VIZ_MARGIN + (desiredLevel - 1) * height / + // 3; + + // PlaceShape( + // _scorePieceShape, scaleX(x) - scaleX(baseX) - cubeSidePix / 2, scaleY(y) - + // scaleY(baseY)); + + // frc::Color8Bit shapeColor = frc::Color8Bit(frc::Color::kYellow); + + // if (desiredLevel == SCORING_LEVEL_0) { + // MakeHybrid(_scorePieceShape); + // shapeColor = frc::Color8Bit(frc::Color::kSpringGreen); // good location + // } else if (locationIndex % 3 == 2) { + // MakeCube(_scorePieceShape); + // if (heldGamePiece == CUBE_GAME_PIECE) + // shapeColor = frc::Color8Bit(frc::Color::kSpringGreen); + // else if (heldGamePiece == CONE_GAME_PIECE) + // shapeColor = frc::Color8Bit(frc::Color::kRed); + // } else { + // MakeCone(_scorePieceShape); + // if (heldGamePiece == CONE_GAME_PIECE) + // shapeColor = frc::Color8Bit(frc::Color::kSpringGreen); + // else if (heldGamePiece == CUBE_GAME_PIECE) + // shapeColor = frc::Color8Bit(frc::Color::kRed); + // } + // ColorShape(_scorePieceShape, shapeColor); + + // // 2. update held game piece + // PlaceShape(_heldPieceShape, 0, -CBVIZ_YSIZE / 2); + // if (heldGamePiece == CONE_GAME_PIECE) { + // MakeCone(_heldPieceShape); + // } else if (heldGamePiece == CUBE_GAME_PIECE) { + // MakeCube(_heldPieceShape); + // } + // frc::Color8Bit heldColor = + // (heldGamePiece == CONE_GAME_PIECE || heldGamePiece == CUBE_GAME_PIECE) + // ? frc::Color8Bit(frc::Color::kLightBlue) + // : frc::Color8Bit(frc::Color::kBlack); + // ColorShape(_heldPieceShape, heldColor); + + // // 3. update desired game piece + // if (desiredGamePiece == CONE_GAME_PIECE) { + // MakeCone(_desiredPieceShape); + // } else if (desiredGamePiece == CUBE_GAME_PIECE) { + // MakeCube(_desiredPieceShape); + // } + // ColorShape(_desiredPieceShape, frc::Color8Bit(frc::Color::kLightBlue)); + } +} +// void ControlboardViz::InitRectangle(ConeCube& shape, +// frc::MechanismLigament2d* prevLigament) +// { +// // shape.parent = prevLigament; +// // shape.rt = prevLigament->Append("ShapeRt", +// cubeSidePix, +// 0_deg, +// // 2); shape.up = shape.rt->Append("ShapeUp", +// cubeSidePix, +// 90_deg, +// // 2); shape.lf = shape.up->Append("ShapeLf", +// cubeSidePix, +// 90_deg, +// // 2); shape.dn = shape.lf->Append("ShapeDn", +// cubeSidePix, +// 90_deg, +// // 2); +// } + +// void ControlboardViz::MakeCube(ControlboardViz::ConeCube& shape) +// { +// // shape.rt->SetLength(cubeSidePix); +// // shape.up->SetLength(cubeSidePix); +// // shape.lf->SetLength(cubeSidePix); +// // shape.dn->SetLength(cubeSidePix); +// // // don't need to set angle for shape.rt - it is set by rotate function +// // shape.up->SetAngle(90_deg); +// // shape.lf->SetAngle(90_deg); +// // shape.dn->SetAngle(90_deg); +// } + +// void ControlboardViz::MakeCone(ControlboardViz::ConeCube& shape) +// { +// // shape.rt->SetLength(coneBasePix); +// // shape.up->SetLength(coneSlantPix); +// // shape.lf->SetLength(0); +// // shape.dn->SetLength(coneSlantPix); +// // // don't need to set angle for shape.rt - it is set by rotate function +// // shape.up->SetAngle(180_deg - coneAngle); +// // shape.lf->SetAngle(0_deg); +// // shape.dn->SetAngle(2 * coneAngle); +// } + +// void ControlboardViz::MakeHybrid(ControlboardViz::ConeCube& shape) +// { +// // shape.rt->SetLength(cubeSidePix); +// // shape.up->SetLength(coneSlantPix); +// // shape.lf->SetLength(cubeSidePix / 2); +// // shape.dn->SetLength(coneHeightPix); +// // // don't need to set angle for shape.rt - it is set by rotate function +// // shape.up->SetAngle(180_deg - coneAngle); +// // shape.lf->SetAngle(coneAngle); +// // shape.dn->SetAngle(90_deg); +// } + +// void ControlboardViz::MakeUnknown(ControlboardViz::ConeCube& shape) +// { +// // shape.rt->SetLength(cubeSidePix / 2); +// // shape.up->SetLength(cubeSidePix / 2); +// // shape.lf->SetLength(cubeSidePix / 2); +// // shape.dn->SetLength(cubeSidePix / 2); +// // // don't need to set angle for shape.rt - it is set by rotate function +// // shape.up->SetAngle(90_deg); +// // shape.lf->SetAngle(-45_deg); +// // shape.dn->SetAngle(90_deg); +// } + +// void ControlboardViz::ColorShape(ControlboardViz::ConeCube& shape, +// frc::Color8Bit color) +// { +// // shape.rt->SetColor(color); +// // shape.up->SetColor(color); +// // shape.lf->SetColor(color); +// // shape.dn->SetColor(color); +// } + +// void ControlboardViz::RotateShape(ControlboardViz::ConeCube& shape, degree_t +// angle) +// { +// // shape.rt->SetAngle(angle); +// } + +// void ControlboardViz::PlaceShape(ConeCube& shape, double dxPix, double dyPix) +// { +// // double dPix = std::sqrt(dxPix * dxPix + dyPix * dyPix); +// // radian_t angle = radian_t{std::atan2(dyPix, dxPix)}; + +// // shape.parent->SetAngle(angle); +// // shape.parent->SetLength(dPix); +// // RotateShape(shape, -angle); +// } diff --git a/src/main/java/frc/robot/utils/InputUtils.java b/src/main/java/frc/robot/utils/InputUtils.java new file mode 100644 index 0000000..8343c7e --- /dev/null +++ b/src/main/java/frc/robot/utils/InputUtils.java @@ -0,0 +1,37 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.constants.Constants.Control; + +public class InputUtils { + public static double scaleJoystickX_MPS(double rawXAxis, double xyNet, double maxSpeed_mps) { + // double scaledXY = xyNet < Control.STICK_NET_DEADBAND ? 0 : xyNet; + // double scaledXY = + // xyNet < Control.STICK_NET_DEADBAND ? 0 : Math.signum(rawXAxis) * Math.pow(xyNet, 2); + double scaledXY = xyNet < Control.STICK_NET_DEADBAND ? 0 : Math.pow(xyNet, 3); + double scaledX = rawXAxis * scaledXY / xyNet; + return scaledX * maxSpeed_mps; + } + + public static double scaleJoystickY_MPS(double rawYAxis, double xyNet, double maxSpeed_mps) { + // double scaledXY = xyNet < Control.STICK_NET_DEADBAND ? 0 : xyNet; + // double scaledXY = + // xyNet < Control.STICK_NET_DEADBAND ? 0 : Math.signum(rawYAxis) * Math.pow(xyNet, 2); + double scaledXY = Math.abs(xyNet) < Control.STICK_NET_DEADBAND ? 0 : Math.pow(xyNet, 3); + double scaledY = rawYAxis * scaledXY / xyNet; + return scaledY * maxSpeed_mps; + } + + public static double ScaleJoystickTheta_RADPS(double rawAxis, Rotation2d maxAngularSpeed) { + return ScaleJoystickTheta_RADPS(rawAxis, maxAngularSpeed.getRadians()); + } + + public static double ScaleJoystickTheta_RADPS(double rawAxis, double maxAngularSpeed_radps) { + // double angular = Math.abs(rawAxis) < Control.STICK_DEADBAND ? 0 : rawAxis; + double angular = Math.abs(rawAxis) < Control.STICK_DEADBAND + ? 0 + : Math.signum(rawAxis) * Math.pow(rawAxis, 2); + // double angular = Math.abs(rawAxis) < Control.STICK_DEADBAND ? 0 : Math.pow(rawAxis, 3); + return angular * maxAngularSpeed_radps; + } +} diff --git a/src/main/java/frc/robot/utils/KinematicLimits.java b/src/main/java/frc/robot/utils/KinematicLimits.java new file mode 100644 index 0000000..646c279 --- /dev/null +++ b/src/main/java/frc/robot/utils/KinematicLimits.java @@ -0,0 +1,106 @@ +package frc.robot.utils; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; + +public class KinematicLimits extends VelocityLimits2d { + public double maxTranslationalAccelerationMPS2; + public Rotation2d maxAngularAccelerationPS2; + public com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits limits254; + + public com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits get254() { + if (limits254 == null) { + limits254 = new com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits(); + update254(); + } + return limits254; + } + + public com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits update254() { + if (limits254 == null) { + limits254 = new com.team254.lib.swerve.SwerveSetpointGenerator.KinematicLimits(); + } else { + limits254.kMaxDriveVelocity = maxTranslationalVelocityMPS; + limits254.kMaxDriveAcceleration = maxTranslationalAccelerationMPS2; + limits254.kMaxSteeringVelocity = maxAngularVelocityPS.getRadians(); + } + return limits254; + } + + public TrapezoidProfile.Constraints getTranslationalTrapezoidalContraints() { + return new TrapezoidProfile.Constraints( + maxTranslationalVelocityMPS, maxTranslationalAccelerationMPS2 + ); + } + + public TrapezoidProfile.Constraints getAngularTrapezoidalContraints() { + return new TrapezoidProfile.Constraints( + maxAngularVelocityPS.getRadians(), maxAngularAccelerationPS2.getRadians() + ); + } + + public TrapezoidProfile.Constraints getAngularTrapezoidalContraintsDeg() { + return new TrapezoidProfile.Constraints( + maxAngularVelocityPS.getDegrees(), maxAngularAccelerationPS2.getDegrees() + ); + } + + public SlewRateLimiter getTranslationalSlewRateLimiter() { + return new SlewRateLimiter( + maxTranslationalAccelerationMPS2, -maxTranslationalAccelerationMPS2, 0.0 + ); + } + + public SlewRateLimiter getAngularSlewRateLimiter() { + return new SlewRateLimiter( + maxAngularAccelerationPS2.getRadians(), -maxAngularAccelerationPS2.getRadians(), 0.0 + ); + } + + public SlewRateLimiter getAngularSlewRateLimiterDeg() { + return new SlewRateLimiter( + maxAngularAccelerationPS2.getDegrees(), -maxAngularAccelerationPS2.getDegrees(), 0.0 + ); + } + + public KinematicLimits( + double maxTranslationalVelocityMPS, + double maxTranslationalAccelerationMPS2, + Rotation2d maxAngularVelocityPS, + Rotation2d maxAngularAccelerationPS2 + ) { + super(maxTranslationalVelocityMPS, maxAngularVelocityPS); + this.maxTranslationalAccelerationMPS2 = maxTranslationalAccelerationMPS2; + this.maxAngularAccelerationPS2 = maxAngularAccelerationPS2; + } + + public static KinematicLimits fromDegrees( + double maxTranslationalVelocityMPS, + double maxTranslationalAccelerationMPS2, + double maxAngularVelocityDEGPS, + double maxAngularAccelerationDEGPS2 + ) { + return new KinematicLimits( + maxTranslationalVelocityMPS, + maxTranslationalAccelerationMPS2, + Rotation2d.fromDegrees(maxAngularVelocityDEGPS), + Rotation2d.fromDegrees(maxAngularAccelerationDEGPS2) + ); + } + + public static KinematicLimits fromRadians( + double maxTranslationalVelocityMPS, + double maxTranslationalAccelerationMPS2, + double maxAngularVelocityRADPS, + double maxAngularAccelerationPS2 + ) { + return new KinematicLimits( + maxTranslationalVelocityMPS, + maxTranslationalAccelerationMPS2, + Rotation2d.fromRadians(maxAngularVelocityRADPS), + Rotation2d.fromRadians(maxAngularAccelerationPS2) + ); + } +} diff --git a/src/main/java/frc/robot/utils/LoopTimer.java b/src/main/java/frc/robot/utils/LoopTimer.java new file mode 100644 index 0000000..73e712e --- /dev/null +++ b/src/main/java/frc/robot/utils/LoopTimer.java @@ -0,0 +1,41 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.RobotAltModes; + +public class LoopTimer { + private static double _loopStart; + private static double _lastTime; + private static double _currentTime; + private static final double CONVERSION_FACTOR = 1000.0; + + public static void markLoopStart() { + if (RobotAltModes.isLoopTiming) { + _currentTime = RobotController.getFPGATime() * CONVERSION_FACTOR; + _loopStart = _currentTime; + } + } + + public static void markEvent(String event) { + if (RobotAltModes.isLoopTiming) { + _lastTime = _currentTime; + _currentTime = RobotController.getFPGATime() * CONVERSION_FACTOR; + System.out.println(event + (_currentTime - _lastTime)); + } + } + + public static void markCompletion(String event, String system) { + if (RobotAltModes.isLoopTiming) { + _lastTime = _currentTime; + _currentTime = RobotController.getFPGATime() * CONVERSION_FACTOR; + System.out.println(event + (_currentTime - _lastTime) + system + (_currentTime - _loopStart)); + } + } + + public static void markCompletion(String system) { + if (RobotAltModes.isLoopTiming) { + _currentTime = RobotController.getFPGATime() * CONVERSION_FACTOR; + System.out.println(system + (_currentTime - _loopStart)); + } + } +} diff --git a/src/main/java/frc/robot/utils/MultiCameraController.java b/src/main/java/frc/robot/utils/MultiCameraController.java new file mode 100644 index 0000000..89ee3a7 --- /dev/null +++ b/src/main/java/frc/robot/utils/MultiCameraController.java @@ -0,0 +1,96 @@ +package frc.robot.utils; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import frc.robot.constants.Constants; +import frc.robot.constants.RobotAltModes; +import frc.robot.constants.VisionConfs; +import frc.robot.constants.enums.CameraConfigs; +import frc.robot.constants.enums.CameraSets; +import frc.robot.subsystems.VisionController; + +public class MultiCameraController { + private static final VisionConfs VISION_CONSTS = Constants.CRobot.vision; + private static final CameraSets _configuredCameras = VISION_CONSTS.setup; + ; + // When we set the desired camera set, make sure that it's a valid subset of the initialized + // camera set; + CameraSets _desiredCameras = _configuredCameras; + private VisionController[] _cameras = new VisionController[VisionConfs.MAX_NUM_CAMERAS]; + private double[] _lastTimestamp = new double[VisionConfs.MAX_NUM_CAMERAS]; + private FieldObject2d[] _visionFieldPoses = new FieldObject2d[VisionConfs.MAX_NUM_CAMERAS]; + private Pose2d[] _visionPoses = new Pose2d[VisionConfs.MAX_NUM_CAMERAS]; + private SwerveDrivePoseEstimator _robotPoseEstimator; + + public MultiCameraController(Field2d field, SwerveDrivePoseEstimator robotPoseEstimator) { + _robotPoseEstimator = robotPoseEstimator; + // For each id in the camera set, configure the camera + for (int i = 0; i < VISION_CONSTS.setup.ids.length; i++) { + // Camera id array and camera config array are sorted and parallel with a separate tracking + // value for primary camera to remove extra computation from this stage + int id = VISION_CONSTS.setup.ids[i]; + CameraConfigs conf = VISION_CONSTS.getCameraConfig(id); + + // Initialize the camera, each camera knows which ID it is -- ensuring we can have continuity + // when deciding to ignore a camera while testing or debugging + _cameras[id] = new VisionController(conf.cameraName, conf.r2cTransform); + _visionFieldPoses[id] = field.getObject("Vision Pose " + conf.fieldObjectName); + _lastTimestamp[i] = -1.0; + } + + for (int i = 0; i < VISION_CONSTS.MAX_NUM_CAMERAS; i++) { + _visionPoses[i] = new Pose2d(); + } + } + + public void updateOdom(boolean isAuto) { + if (RobotAltModes.isVisionMode && !isAuto) { + for (int i = 0; i < _desiredCameras.ids.length; i++) { + updateOdom(_desiredCameras.ids[i]); + } + } + } + + private void updateOdom(int id) { + double timestamp = _cameras[id].getLatestTimestamp(_visionPoses[id]); + if (_lastTimestamp[id] < timestamp) { + _lastTimestamp[id] = timestamp; + _visionPoses[id] = _cameras[id].getLatestPose(); + _robotPoseEstimator.addVisionMeasurement( + _visionPoses[id], _lastTimestamp[id], _cameras[id].getLatestStdDevs() + ); + } + } + + // Gets vision pose for the camera with the passed id + public Pose2d getVisionPose(int id) { + if (id < VisionConfs.MAX_NUM_CAMERAS && id >= 0 && _cameras[id] != null) + return _visionPoses[id]; + + return new Pose2d(); + } + + // Gets vision pose of the primary camera + public Pose2d getVisionPose() { + // if (_visionPoses[_desiredCameras.primary] == null) + // return new Pose2d(); + return getVisionPose(_desiredCameras.primary); + } + + public void setDesiredCameras(CameraSets set) { + if (_configuredCameras.isCompatible(set)) { + _desiredCameras = set; + } + } + + public void updateShuffleboard() { + for (int i = 0; i < VisionConfs.MAX_NUM_CAMERAS; i++) { + _visionFieldPoses[i].setPose(getVisionPose(_desiredCameras.ids[i])); + } + } + + public CameraSets getDesiredCameras() { + return _desiredCameras; + } +} diff --git a/src/main/java/frc/robot/utils/OdometryUtils.java b/src/main/java/frc/robot/utils/OdometryUtils.java new file mode 100644 index 0000000..a53b324 --- /dev/null +++ b/src/main/java/frc/robot/utils/OdometryUtils.java @@ -0,0 +1,19 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class OdometryUtils { + public static boolean inRange( + Pose2d curPose, + Rotation2d yaw, + Pose2d targetPose, + double xyToleranceM, + double thetaToleranceDeg + ) { + double thetaDiff = targetPose.getRotation().getDegrees() - yaw.getDegrees(); + return Math.abs(targetPose.getX() - curPose.getX()) <= xyToleranceM + && Math.abs(targetPose.getY() - curPose.getY()) <= xyToleranceM + && Math.abs(thetaDiff > 180 ? thetaDiff - 360 : thetaDiff) <= thetaToleranceDeg; + } +} diff --git a/src/main/java/frc/robot/utils/PIDConstants.java b/src/main/java/frc/robot/utils/PIDConstants.java new file mode 100644 index 0000000..21643a4 --- /dev/null +++ b/src/main/java/frc/robot/utils/PIDConstants.java @@ -0,0 +1,31 @@ +package frc.robot.utils; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.robot.constants.Constants; + +public class PIDConstants { + public final double p, i, d; + public PIDConstants(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + } + + public static PIDConstants fromDegrees(double p, double i, double d) { + return new PIDConstants(p, i, d); + } + + public com.pathplanner.lib.auto.PIDConstants getPathPlannerTheta() { + return new com.pathplanner.lib.auto.PIDConstants(p, i, d, Constants.LOOP_PERIOD_S); + } + + public com.pathplanner.lib.auto.PIDConstants getPathPlannerTranslation() { + return new com.pathplanner.lib.auto.PIDConstants(p, i, d, Constants.LOOP_PERIOD_S); + } + + public ProfiledPIDController getProfiledController(TrapezoidProfile.Constraints constraints) { + return new ProfiledPIDController(p, i, d, constraints, Constants.LOOP_PERIOD_S); + } +} diff --git a/src/main/java/frc/robot/utils/PIDFConstants.java b/src/main/java/frc/robot/utils/PIDFConstants.java new file mode 100644 index 0000000..663f09f --- /dev/null +++ b/src/main/java/frc/robot/utils/PIDFConstants.java @@ -0,0 +1,20 @@ +package frc.robot.utils; + +import com.ctre.phoenix.motorcontrol.can.SlotConfiguration; + +public class PIDFConstants extends PIDConstants { + public final double f; + public PIDFConstants(double p, double i, double d, double f) { + super(p, i, d); + this.f = f; + } + + public SlotConfiguration toCTRESlotConfiguration() { + SlotConfiguration configured = new SlotConfiguration(); + configured.kP = p; + configured.kI = i; + configured.kD = d; + configured.kF = f; + return configured; + } +} diff --git a/src/main/java/frc/robot/utils/PIDFConstantsM.java b/src/main/java/frc/robot/utils/PIDFConstantsM.java new file mode 100644 index 0000000..82a6063 --- /dev/null +++ b/src/main/java/frc/robot/utils/PIDFConstantsM.java @@ -0,0 +1,13 @@ +package frc.robot.utils; + +public class PIDFConstantsM extends PIDFConstants { + public final double toleranceM; + public PIDFConstantsM(double p, double i, double d, double toleranceM) { + this(p, i, d, 0.0, toleranceM); + } + + public PIDFConstantsM(double p, double i, double d, double f, double toleranceM) { + super(p, i, d, f); + this.toleranceM = toleranceM; + } +} diff --git a/src/main/java/frc/robot/utils/PIDFConstantsRad.java b/src/main/java/frc/robot/utils/PIDFConstantsRad.java new file mode 100644 index 0000000..d124af9 --- /dev/null +++ b/src/main/java/frc/robot/utils/PIDFConstantsRad.java @@ -0,0 +1,38 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class PIDFConstantsRad extends PIDFConstants { + public final Rotation2d tolerance; + + public PIDFConstantsRad(double p, double i, double d, Rotation2d tolerance) { + this(p, i, d, 0.0, tolerance); + } + + public PIDFConstantsRad(double p, double i, double d, double f, Rotation2d tolerance) { + super(p, i, d, f); + this.tolerance = tolerance; + } + + public static PIDFConstantsRad fromRadians( + double p, double i, double d, double f, double toleranceRad + ) { + return new PIDFConstantsRad(p, i, d, f, Rotation2d.fromRadians(toleranceRad)); + } + + public static PIDFConstantsRad fromDegrees( + double p, double i, double d, double f, double toleranceDeg + ) { + return new PIDFConstantsRad( + p, + i, + d, + // Units.radiansToDegrees(p), + // Units.radiansToDegrees(i), + // Units.radiansToDegrees(d), + f, + Rotation2d.fromDegrees(toleranceDeg) + ); + } +} diff --git a/src/main/java/frc/robot/utils/UnitUtils.java b/src/main/java/frc/robot/utils/UnitUtils.java new file mode 100644 index 0000000..5d88ede --- /dev/null +++ b/src/main/java/frc/robot/utils/UnitUtils.java @@ -0,0 +1,13 @@ +package frc.robot.utils; + +import edu.wpi.first.math.util.Units; + +public class UnitUtils { + public static double[] inchesToMeters(double[] inches) { + double[] res = new double[inches.length]; + for (int i = 0; i < inches.length; i++) { + res[i] = Units.inchesToMeters(inches[i]); + } + return res; + } +} diff --git a/src/main/java/frc/robot/utils/VelocityLimits2d.java b/src/main/java/frc/robot/utils/VelocityLimits2d.java new file mode 100644 index 0000000..c06782e --- /dev/null +++ b/src/main/java/frc/robot/utils/VelocityLimits2d.java @@ -0,0 +1,35 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class VelocityLimits2d { + public double maxTranslationalVelocityMPS; + public Rotation2d maxAngularVelocityPS; + + public VelocityLimits2d(double maxTranslationalVelocityMPS, Rotation2d maxAngularVelocityPS) { + this.maxTranslationalVelocityMPS = maxTranslationalVelocityMPS; + this.maxAngularVelocityPS = maxAngularVelocityPS; + } + + public static VelocityLimits2d fromRadians( + double maxTranslationalVelocityMPS, double maxAngularVelocityRADPS + ) { + return new VelocityLimits2d( + maxTranslationalVelocityMPS, Rotation2d.fromRadians(maxAngularVelocityRADPS) + ); + } + + public static VelocityLimits2d fromDegrees( + double maxTranslationalVelocityMPS, double maxAngularVelocityDPS + ) { + return new VelocityLimits2d( + maxTranslationalVelocityMPS, Rotation2d.fromDegrees(maxAngularVelocityDPS) + ); + } + + public void multiply(double c) { + maxTranslationalVelocityMPS *= c; + maxAngularVelocityPS = maxAngularVelocityPS.times(c); + } +} diff --git a/src/test/java/com/team254/frc2022/ConstantsTest.java b/src/test/java/com/team254/frc2022/ConstantsTest.java new file mode 100644 index 0000000..d985305 --- /dev/null +++ b/src/test/java/com/team254/frc2022/ConstantsTest.java @@ -0,0 +1,18 @@ +package com.team254.frc2022; + +import com.team254.lib.Constants; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +/** + * Ensure we don't push bad constants + */ +@RunWith(JUnit4.class) +public class ConstantsTest { + @Test + public void test() { + Assert.assertFalse("Still in Gamepad Drive Mode", Constants.kForceDriveGamepad); + } +} diff --git a/src/test/java/com/team254/frc2022/TestSystemTest.java b/src/test/java/com/team254/frc2022/TestSystemTest.java new file mode 100644 index 0000000..1875534 --- /dev/null +++ b/src/test/java/com/team254/frc2022/TestSystemTest.java @@ -0,0 +1,18 @@ +package com.team254.frc2022; + +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +/** + * Class that tests the system test + */ +@RunWith(JUnit4.class) +public class TestSystemTest { + @Test + public void test() { + // assert statements + Assert.assertEquals(0, 0); + } +} diff --git a/src/test/java/com/team254/frc2022/paths/TrajectoryGeneratorTest.java b/src/test/java/com/team254/frc2022/paths/TrajectoryGeneratorTest.java new file mode 100644 index 0000000..7244fe4 --- /dev/null +++ b/src/test/java/com/team254/frc2022/paths/TrajectoryGeneratorTest.java @@ -0,0 +1,16 @@ +package com.team254.frc2022.paths; + +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +/** + * Ensure all trajectories are valid + */ +@RunWith(JUnit4.class) +public class TrajectoryGeneratorTest { + // @Test + // public void testGenerateTrajectories() { + // TrajectoryGenerator.getInstance().generateTrajectories(); + // } +} diff --git a/src/test/java/com/team254/lib/geometry/TestSE2Math.java b/src/test/java/com/team254/lib/geometry/TestSE2Math.java new file mode 100644 index 0000000..620f78f --- /dev/null +++ b/src/test/java/com/team254/lib/geometry/TestSE2Math.java @@ -0,0 +1,289 @@ +package com.team254.lib.geometry; + +import com.team254.lib.util.Util; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class TestSE2Math { + public static final double kTestEpsilon = Util.kEpsilon; + + @Test + public void testRotation2d() { + // Test constructors + Rotation2d254 rot1 = new Rotation2d254(); + Assert.assertEquals(1, rot1.cos(), kTestEpsilon); + Assert.assertEquals(0, rot1.sin(), kTestEpsilon); + Assert.assertEquals(0, rot1.tan(), kTestEpsilon); + Assert.assertEquals(0, rot1.getDegrees(), kTestEpsilon); + Assert.assertEquals(0, rot1.getRadians(), kTestEpsilon); + + rot1 = new Rotation2d254(1, 1, true); + Assert.assertEquals(Math.sqrt(2) / 2, rot1.cos(), kTestEpsilon); + Assert.assertEquals(Math.sqrt(2) / 2, rot1.sin(), kTestEpsilon); + Assert.assertEquals(1, rot1.tan(), kTestEpsilon); + Assert.assertEquals(45, rot1.getDegrees(), kTestEpsilon); + Assert.assertEquals(Math.PI / 4, rot1.getRadians(), kTestEpsilon); + + rot1 = Rotation2d254.fromRadians(Math.PI / 2); + Assert.assertEquals(0, rot1.cos(), kTestEpsilon); + Assert.assertEquals(1, rot1.sin(), kTestEpsilon); + Assert.assertTrue(1 / kTestEpsilon < rot1.tan()); + Assert.assertEquals(90, rot1.getDegrees(), kTestEpsilon); + Assert.assertEquals(Math.PI / 2, rot1.getRadians(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(270); + Assert.assertEquals(0, rot1.cos(), kTestEpsilon); + Assert.assertEquals(-1, rot1.sin(), kTestEpsilon); + System.out.println(rot1.tan()); + Assert.assertTrue(-1 / kTestEpsilon > rot1.tan()); + Assert.assertEquals(-90, rot1.getDegrees(), kTestEpsilon); + Assert.assertEquals(-Math.PI / 2, rot1.getRadians(), kTestEpsilon); + + // Test inversion + rot1 = Rotation2d254.fromDegrees(270); + Rotation2d254 rot2 = rot1.inverse(); + Assert.assertEquals(0, rot2.cos(), kTestEpsilon); + Assert.assertEquals(1, rot2.sin(), kTestEpsilon); + Assert.assertTrue(1 / kTestEpsilon < rot2.tan()); + Assert.assertEquals(90, rot2.getDegrees(), kTestEpsilon); + Assert.assertEquals(Math.PI / 2, rot2.getRadians(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(1); + rot2 = rot1.inverse(); + Assert.assertEquals(rot1.cos(), rot2.cos(), kTestEpsilon); + Assert.assertEquals(-rot1.sin(), rot2.sin(), kTestEpsilon); + Assert.assertEquals(-1, rot2.getDegrees(), kTestEpsilon); + + // Test rotateBy + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(45); + Rotation2d254 rot3 = rot1.rotateBy(rot2); + Assert.assertEquals(0, rot3.cos(), kTestEpsilon); + Assert.assertEquals(1, rot3.sin(), kTestEpsilon); + Assert.assertTrue(1 / kTestEpsilon < rot3.tan()); + Assert.assertEquals(90, rot3.getDegrees(), kTestEpsilon); + Assert.assertEquals(Math.PI / 2, rot3.getRadians(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(-45); + rot3 = rot1.rotateBy(rot2); + Assert.assertEquals(1, rot3.cos(), kTestEpsilon); + Assert.assertEquals(0, rot3.sin(), kTestEpsilon); + Assert.assertEquals(0, rot3.tan(), kTestEpsilon); + Assert.assertEquals(0, rot3.getDegrees(), kTestEpsilon); + Assert.assertEquals(0, rot3.getRadians(), kTestEpsilon); + + // A rotation times its inverse should be the identity + Rotation2d254 identity = new Rotation2d254(); + rot1 = Rotation2d254.fromDegrees(21.45); + rot2 = rot1.rotateBy(rot1.inverse()); + Assert.assertEquals(identity.cos(), rot2.cos(), kTestEpsilon); + Assert.assertEquals(identity.sin(), rot2.sin(), kTestEpsilon); + Assert.assertEquals(identity.getDegrees(), rot2.getDegrees(), kTestEpsilon); + + // Test interpolation + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(135); + rot3 = rot1.interpolate(rot2, .5); + Assert.assertEquals(90, rot3.getDegrees(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(135); + rot3 = rot1.interpolate(rot2, .75); + Assert.assertEquals(112.5, rot3.getDegrees(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(-45); + rot3 = rot1.interpolate(rot2, .5); + Assert.assertEquals(0, rot3.getDegrees(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(45); + rot3 = rot1.interpolate(rot2, .5); + Assert.assertEquals(45, rot3.getDegrees(), kTestEpsilon); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(45); + rot3 = rot1.interpolate(rot2, .5); + Assert.assertEquals(45, rot3.getDegrees(), kTestEpsilon); + + // Test parallel. + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(45); + Assert.assertTrue(rot1.isParallel(rot2)); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(-45); + Assert.assertFalse(rot1.isParallel(rot2)); + + rot1 = Rotation2d254.fromDegrees(45); + rot2 = Rotation2d254.fromDegrees(-135); + Assert.assertTrue(rot1.isParallel(rot2)); + } + + @Test + public void testTranslation2d() { + // Test constructors + Translation2d pos1 = new Translation2d(); + Assert.assertEquals(0, pos1.x(), kTestEpsilon); + Assert.assertEquals(0, pos1.y(), kTestEpsilon); + Assert.assertEquals(0, pos1.norm(), kTestEpsilon); + + pos1 = new Translation2d(3, 4); + Assert.assertEquals(3, pos1.x(), kTestEpsilon); + Assert.assertEquals(4, pos1.y(), kTestEpsilon); + Assert.assertEquals(5, pos1.norm(), kTestEpsilon); + + // Test inversion + pos1 = new Translation2d(3.152, 4.1666); + Translation2d pos2 = pos1.inverse(); + Assert.assertEquals(-pos1.x(), pos2.x(), kTestEpsilon); + Assert.assertEquals(-pos1.y(), pos2.y(), kTestEpsilon); + Assert.assertEquals(pos1.norm(), pos2.norm(), kTestEpsilon); + + // Test rotateBy + pos1 = new Translation2d(2, 0); + Rotation2d254 rot1 = Rotation2d254.fromDegrees(90); + pos2 = pos1.rotateBy(rot1); + Assert.assertEquals(0, pos2.x(), kTestEpsilon); + Assert.assertEquals(2, pos2.y(), kTestEpsilon); + Assert.assertEquals(pos1.norm(), pos2.norm(), kTestEpsilon); + + pos1 = new Translation2d(2, 0); + rot1 = Rotation2d254.fromDegrees(-45); + pos2 = pos1.rotateBy(rot1); + Assert.assertEquals(Math.sqrt(2), pos2.x(), kTestEpsilon); + Assert.assertEquals(-Math.sqrt(2), pos2.y(), kTestEpsilon); + Assert.assertEquals(pos1.norm(), pos2.norm(), kTestEpsilon); + + // Test translateBy + pos1 = new Translation2d(2, 0); + pos2 = new Translation2d(-2, 1); + Translation2d pos3 = pos1.translateBy(pos2); + Assert.assertEquals(0, pos3.x(), kTestEpsilon); + Assert.assertEquals(1, pos3.y(), kTestEpsilon); + Assert.assertEquals(1, pos3.norm(), kTestEpsilon); + + // A translation times its inverse should be the identity + Translation2d identity = new Translation2d(); + pos1 = new Translation2d(2.16612, -23.55); + pos2 = pos1.translateBy(pos1.inverse()); + Assert.assertEquals(identity.x(), pos2.x(), kTestEpsilon); + Assert.assertEquals(identity.y(), pos2.y(), kTestEpsilon); + Assert.assertEquals(identity.norm(), pos2.norm(), kTestEpsilon); + + // Test interpolation + pos1 = new Translation2d(0, 1); + pos2 = new Translation2d(10, -1); + pos3 = pos1.interpolate(pos2, .5); + Assert.assertEquals(5, pos3.x(), kTestEpsilon); + Assert.assertEquals(0, pos3.y(), kTestEpsilon); + + pos1 = new Translation2d(0, 1); + pos2 = new Translation2d(10, -1); + pos3 = pos1.interpolate(pos2, .75); + Assert.assertEquals(7.5, pos3.x(), kTestEpsilon); + Assert.assertEquals(-.5, pos3.y(), kTestEpsilon); + } + + @Test + public void testPose2d() { + // Test constructors + Pose2d pose1 = new Pose2d(); + Assert.assertEquals(0, pose1.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(0, pose1.getTranslation().y(), kTestEpsilon); + Assert.assertEquals(0, pose1.getRotation().getDegrees(), kTestEpsilon); + + pose1 = new Pose2d(new Translation2d(3, 4), Rotation2d254.fromDegrees(45)); + Assert.assertEquals(3, pose1.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(4, pose1.getTranslation().y(), kTestEpsilon); + Assert.assertEquals(45, pose1.getRotation().getDegrees(), kTestEpsilon); + + // Test transformation + pose1 = new Pose2d(new Translation2d(3, 4), Rotation2d254.fromDegrees(90)); + Pose2d pose2 = new Pose2d(new Translation2d(1, 0), Rotation2d254.fromDegrees(0)); + Pose2d pose3 = pose1.transformBy(pose2); + Assert.assertEquals(3, pose3.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(5, pose3.getTranslation().y(), kTestEpsilon); + Assert.assertEquals(90, pose3.getRotation().getDegrees(), kTestEpsilon); + + pose1 = new Pose2d(new Translation2d(3, 4), Rotation2d254.fromDegrees(90)); + pose2 = new Pose2d(new Translation2d(1, 0), Rotation2d254.fromDegrees(-90)); + pose3 = pose1.transformBy(pose2); + Assert.assertEquals(3, pose3.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(5, pose3.getTranslation().y(), kTestEpsilon); + Assert.assertEquals(0, pose3.getRotation().getDegrees(), kTestEpsilon); + + // A pose times its inverse should be the identity + Pose2d identity = new Pose2d(); + pose1 = new Pose2d(new Translation2d(3.51512152, 4.23), Rotation2d254.fromDegrees(91.6)); + pose2 = pose1.transformBy(pose1.inverse()); + Assert.assertEquals(identity.getTranslation().x(), pose2.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(identity.getTranslation().y(), pose2.getTranslation().y(), kTestEpsilon); + Assert.assertEquals( + identity.getRotation().getDegrees(), pose2.getRotation().getDegrees(), kTestEpsilon + ); + + // Test interpolation + // Movement from pose1 to pose2 is along a circle with radius of 10 units + // centered at (3, -6) + pose1 = new Pose2d(new Translation2d(3, 4), Rotation2d254.fromDegrees(90)); + pose2 = new Pose2d(new Translation2d(13, -6), Rotation2d254.fromDegrees(0.0)); + pose3 = pose1.interpolate(pose2, .5); + double expected_angle_rads = Math.PI / 4; + Assert.assertEquals( + 3.0 + 10.0 * Math.cos(expected_angle_rads), pose3.getTranslation().x(), kTestEpsilon + ); + Assert.assertEquals( + -6.0 + 10.0 * Math.sin(expected_angle_rads), pose3.getTranslation().y(), kTestEpsilon + ); + Assert.assertEquals(expected_angle_rads, pose3.getRotation().getRadians(), kTestEpsilon); + + pose1 = new Pose2d(new Translation2d(3, 4), Rotation2d254.fromDegrees(90)); + pose2 = new Pose2d(new Translation2d(13, -6), Rotation2d254.fromDegrees(0.0)); + pose3 = pose1.interpolate(pose2, .75); + expected_angle_rads = Math.PI / 8; + Assert.assertEquals( + 3.0 + 10.0 * Math.cos(expected_angle_rads), pose3.getTranslation().x(), kTestEpsilon + ); + Assert.assertEquals( + -6.0 + 10.0 * Math.sin(expected_angle_rads), pose3.getTranslation().y(), kTestEpsilon + ); + Assert.assertEquals(expected_angle_rads, pose3.getRotation().getRadians(), kTestEpsilon); + } + + @Test + public void testTwist() { + // Exponentiation (integrate twist to obtain a Pose2d) + Twist2d twist = new Twist2d(1.0, 0.0, 0.0); + Pose2d pose = Pose2d.exp(twist); + Assert.assertEquals(1.0, pose.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(0.0, pose.getTranslation().y(), kTestEpsilon); + Assert.assertEquals(0.0, pose.getRotation().getDegrees(), kTestEpsilon); + + // Scaled. + twist = new Twist2d(1.0, 0.0, 0.0); + pose = Pose2d.exp(twist.scaled(2.5)); + Assert.assertEquals(2.5, pose.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(0.0, pose.getTranslation().y(), kTestEpsilon); + Assert.assertEquals(0.0, pose.getRotation().getDegrees(), kTestEpsilon); + + // Logarithm (find the twist to apply to obtain a given Pose2d) + pose = new Pose2d(new Translation2d(2.0, 2.0), Rotation2d254.fromRadians(Math.PI / 2)); + twist = Pose2d.log(pose); + Assert.assertEquals(Math.PI, twist.dx, kTestEpsilon); + Assert.assertEquals(0.0, twist.dy, kTestEpsilon); + Assert.assertEquals(Math.PI / 2, twist.dtheta, kTestEpsilon); + + // Logarithm is the inverse of exponentiation. + Pose2d new_pose = Pose2d.exp(twist); + Assert.assertEquals(new_pose.getTranslation().x(), pose.getTranslation().x(), kTestEpsilon); + Assert.assertEquals(new_pose.getTranslation().y(), pose.getTranslation().y(), kTestEpsilon); + Assert.assertEquals( + new_pose.getRotation().getDegrees(), pose.getRotation().getDegrees(), kTestEpsilon + ); + } +} diff --git a/src/test/java/com/team254/lib/motion/MotionProfileGeneratorTest.java b/src/test/java/com/team254/lib/motion/MotionProfileGeneratorTest.java new file mode 100644 index 0000000..3187038 --- /dev/null +++ b/src/test/java/com/team254/lib/motion/MotionProfileGeneratorTest.java @@ -0,0 +1,638 @@ +package com.team254.lib.motion; + +import static com.team254.lib.motion.MotionUtil.kEpsilon; +import static org.junit.Assert.assertThat; +import static org.junit.Assert.assertTrue; + +import com.team254.lib.motion.IMotionProfileGoal.CompletionBehavior; +import com.team254.lib.util.Util; +import org.hamcrest.BaseMatcher; +import org.hamcrest.Description; +import org.hamcrest.Matcher; +import org.junit.Test; + +public class MotionProfileGeneratorTest { + public static Matcher epsilonEqualTo(final double b, final double epsilon) { + return new BaseMatcher() { + @Override + public boolean matches(Object a) { + return Util.epsilonEquals((Double) a, b, epsilon); + } + + @Override + public void describeTo(Description description) { + description.appendText("Should be within +/- ") + .appendValue(epsilon) + .appendText(" of ") + .appendValue(b); + } + }; + } + + protected static void validateProfile( + MotionProfileConstraints constraints, + MotionProfileGoal goal, + MotionState start, + MotionProfile profile + ) { + // Profile should be valid. + assertTrue(profile.isValid()); + // Profile should start at the start state (clamped to accel and velocity + // limits). + assertThat(profile.startState().t(), epsilonEqualTo(start.t(), kEpsilon)); + assertThat(profile.startState().pos(), epsilonEqualTo(start.pos(), kEpsilon)); + assertThat( + profile.startState().vel(), + epsilonEqualTo( + Math.max(Math.min(start.vel(), constraints.max_vel()), constraints.min_vel()), kEpsilon + ) + ); + // Profile should not violate constraints. + for (MotionSegment s : profile.segments()) { + assertTrue(s.start().vel() <= constraints.max_vel()); + assertTrue(s.start().vel() >= constraints.min_vel()); + assertTrue(s.end().vel() <= constraints.max_vel()); + assertTrue(s.end().vel() >= constraints.min_vel()); + if (goal.completion_behavior() != CompletionBehavior.VIOLATE_MAX_ACCEL) { + assertTrue(Math.abs(s.start().acc()) <= constraints.max_abs_acc()); + assertTrue(Math.abs(s.end().acc()) <= constraints.max_abs_acc()); + } + } + // Profile should end at the goal state. + if (goal.completion_behavior() != CompletionBehavior.VIOLATE_MAX_ABS_VEL) { + assertTrue(goal.atGoalState(profile.endState())); + } else { + assertTrue(goal.atGoalPos(profile.endPos())); + } + } + protected static void validateProfile( + MotionProfileConstraints constraints, + LinearTimeVaryingMotionProfileGoal goal, + MotionState start, + MotionProfile profile, + boolean ignore_goal + ) { + // Profile should be valid. + assertTrue(profile.isValid()); + // Profile should start at the start state (clamped to accel and velocity + // limits). + assertThat(profile.startState().t(), epsilonEqualTo(start.t(), kEpsilon)); + assertThat(profile.startState().pos(), epsilonEqualTo(start.pos(), kEpsilon)); + assertThat( + profile.startState().vel(), + epsilonEqualTo( + Math.max(Math.min(start.vel(), constraints.max_vel()), constraints.min_vel()), kEpsilon + ) + ); + // Profile should not violate constraints. + for (MotionSegment s : profile.segments()) { + assertTrue(s.start().vel() <= constraints.max_vel()); + assertTrue(s.start().vel() >= constraints.min_vel()); + assertTrue(s.end().vel() <= constraints.max_vel()); + assertTrue(s.end().vel() >= constraints.min_vel()); + if (goal.completion_behavior() != CompletionBehavior.VIOLATE_MAX_ACCEL) { + assertTrue(Math.abs(s.start().acc()) <= constraints.max_abs_acc()); + assertTrue(Math.abs(s.end().acc()) <= constraints.max_abs_acc()); + } + } + // Profile should end at the goal state. + if (!ignore_goal) { + var goal_state_at_end_time = goal.pos(profile.endTime()); + assertTrue(goal.atGoalState(profile.endState())); + } + } + + protected static void testProfile( + MotionProfileConstraints constraints, + MotionProfileGoal goal, + MotionState start, + double expected_duration, + double expected_length + ) { + testProfileForward(constraints, goal, start, expected_duration, expected_length); + testProfileFlipped( + new MotionProfileConstraints( + -constraints.min_vel(), -constraints.max_vel(), constraints.max_abs_acc() + ), + goal, + start, + expected_duration, + expected_length + ); + } + + protected static void testProfileForward( + MotionProfileConstraints constraints, + MotionProfileGoal goal, + MotionState start, + double expected_duration, + double expected_length + ) { + MotionProfile positive_profile = + MotionProfileGenerator.generateProfile(constraints, goal, start); + assertThat( + positive_profile.toString(), + positive_profile.duration(), + epsilonEqualTo(expected_duration, kEpsilon) + ); + assertThat( + positive_profile.toString(), + positive_profile.length(), + epsilonEqualTo(expected_length, kEpsilon) + ); + validateProfile(constraints, goal, start, positive_profile); + } + + protected static void testProfileFlipped( + MotionProfileConstraints constraints, + MotionProfileGoal goal, + MotionState start, + double expected_duration, + double expected_length + ) { + MotionProfile negative_profile = + MotionProfileGenerator.generateProfile(constraints, goal.flipped(), start.flipped()); + assertThat( + negative_profile.toString(), + negative_profile.duration(), + epsilonEqualTo(expected_duration, kEpsilon) + ); + assertThat( + negative_profile.toString(), + negative_profile.length(), + epsilonEqualTo(expected_length, kEpsilon) + ); + validateProfile(constraints, goal.flipped(), start.flipped(), negative_profile); + } + + protected static void testProfile( + MotionProfileConstraints constraints, + LinearTimeVaryingMotionProfileGoal goal, + MotionState start, + double expected_duration, + double expected_length + ) { + testProfileForward(constraints, goal, start, expected_duration, expected_length); + testProfileFlipped( + new MotionProfileConstraints( + -constraints.min_vel(), -constraints.max_vel(), constraints.max_abs_acc() + ), + goal, + start, + expected_duration, + expected_length + ); + } + + protected static void testProfileForward( + MotionProfileConstraints constraints, + LinearTimeVaryingMotionProfileGoal goal, + MotionState start, + double expected_duration, + double expected_length + ) { + MotionProfile positive_profile = + MotionProfileGenerator.generateProfile(constraints, goal, start); + assertThat( + positive_profile.toString(), + positive_profile.duration(), + epsilonEqualTo(expected_duration, kEpsilon) + ); + assertThat( + positive_profile.toString(), + positive_profile.length(), + epsilonEqualTo(expected_length, kEpsilon) + ); + validateProfile(constraints, goal, start, positive_profile, false); + } + + protected static void testProfileFlipped( + MotionProfileConstraints constraints, + LinearTimeVaryingMotionProfileGoal goal, + MotionState start, + double expected_duration, + double expected_length + ) { + MotionProfile negative_profile = + MotionProfileGenerator.generateProfile(constraints, goal.flipped(), start.flipped()); + assertThat( + negative_profile.toString(), + negative_profile.duration(), + epsilonEqualTo(expected_duration, kEpsilon) + ); + assertThat( + negative_profile.toString(), + negative_profile.length(), + epsilonEqualTo(expected_length, kEpsilon) + ); + validateProfile(constraints, goal.flipped(), start.flipped(), negative_profile, false); + } + + @Test + public void testAlreadyFinished() { + // No initial velocity. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(0.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 0.0, + 0.0 + ); + // Initial velocity matches goal state. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(2.0, 5.0), + new MotionState(0.0, 2.0, 5.0, 7.0), + 0.0, + 0.0 + ); + // Initial velocity is too high, so allow infinite accel. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(2.0, 5.0, CompletionBehavior.VIOLATE_MAX_ABS_VEL), + new MotionState(0.0, 2.0, 9.0, 7.0), + 0.0, + 0.0 + ); + // Initial velocity is too high, so allow infinite accel. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(2.0, 5.0, CompletionBehavior.VIOLATE_MAX_ACCEL), + new MotionState(0.0, 2.0, 9.0, 7.0), + 0.0, + 0.0 + ); + } + + @Test + public void testStationaryToStationary() { + // Trapezoidal move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 11.0, + 100.0 + ); + // Triangle move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(10.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 2.0, + 10.0 + ); + + // Test asymmetric velocity limits. + // Trapezoidal move. + testProfile( + new MotionProfileConstraints(10.0, -5.0, 5.0), + new MotionProfileGoal(100.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 12.0, + 100.0 + ); + + // Triangle move. + testProfile( + new MotionProfileConstraints(10.0, -5.0, 5.0), + new MotionProfileGoal(5.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 2.0, + 5.0 + ); + } + + @Test + public void testMovingTowardsToStationary() { + // Moving towards goal, trapezoidal move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 10.5, + 100.0 + ); + // Moving towards goal, triangle move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(10.0), + new MotionState(0.0, 0.0, 5.0, 0.0), + 1.625, + 10.0 + ); + // Moving towards goal, cruise and stop. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(10.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 1.5, + 10.0 + ); + // Moving towards goal, overshoot and come back. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(0.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 1.0 + Math.sqrt(2.0), + 10.0 + ); + // Moving towards goal, violate max vel. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 0.0, CompletionBehavior.VIOLATE_MAX_ABS_VEL), + new MotionState(0.0, 0.0, 10.0, 0.0), + (10.0 - Math.sqrt(80.0)) / 10.0, + 1.0 + ); + // Moving towards goal, violate max accel. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 0.0, CompletionBehavior.VIOLATE_MAX_ACCEL), + new MotionState(0.0, 0.0, 10.0, 0.0), + 1.0 / 5.0, + 1.0 + ); + } + + @Test + public void testMovingAwayToStationary() { + // Moving away from goal, trapezoidal move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0), + new MotionState(0.0, 0.0, -10.0, 0.0), + 12.5, + 110.0 + ); + // Moving away from goal, triangle move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(5.0), + new MotionState(0.0, 0.0, -10.0, 0.0), + 3.0, + 15.0 + ); + } + + @Test + public void testStationaryToMoving() { + // Accelerate and cruise. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 10.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 10.5, + 100.0 + ); + + // Trapezoidal move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 5.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 10.625, + 100.0 + ); + + // Pure acceleration. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 10.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + Math.sqrt(1.0 / 5.0), + 1.0 + ); + + // Triangle move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(8.75, 5.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 1.5, + 8.75 + ); + } + + @Test + public void testMovingTowardsToMoving() { + // Moving towards goal, pure acceleration. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(3.75, 10.0), + new MotionState(0.0, 0.0, 5.0, 0.0), + 0.5, + 3.75 + ); + // Moving towards goal, pure deceleration. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(10.0, 5.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 1.125, + 10.0 + ); + // Moving towards goal, cruise. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 10.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 10.0, + 100.0 + ); + // Moving towards goal, accelerate and cruise. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 10.0), + new MotionState(0.0, 0.0, 5.0, 0.0), + 10.125, + 100.0 + ); + // Moving towards goal, trapezoidal move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 5.0), + new MotionState(0.0, 0.0, 5.0, 0.0), + 10.25, + 100.0 + ); + // Moving towards goal, triangle move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 10.0), + new MotionState(0.0, 0.0, 4.0, 0.0), + 0.2, + 1.0 + ); + // Moving towards goal, cruise and decelerate. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(10.0, 5.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 1.125, + 10.0 + ); + // Moving towards goal, violate max vel. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 1.0, CompletionBehavior.VIOLATE_MAX_ABS_VEL), + new MotionState(0.0, 0.0, 10.0, 0.0), + (10.0 - Math.sqrt(80.0)) / 10.0, + 1.0 + ); + // Moving towards goal, violate max accel. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 2.0, CompletionBehavior.VIOLATE_MAX_ACCEL), + new MotionState(0.0, 0.0, 10.0, 0.0), + 1.0 / 6.0, + 1.0 + ); + } + + @Test + public void testMovingAwayToMoving() { + // Moving away from goal, stop and accelerate. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(1.0, 10.0), + new MotionState(0.0, 0.0, -4.0, 0.0), + 1.0, + 2.6 + ); + // Moving away from goal, stop, accelerate, and cruise. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 10.0), + new MotionState(0.0, 0.0, -10.0, 0.0), + 12.0, + 110.0 + ); + // Moving away from goal, stop and trapezoid move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(100.0, 5.0), + new MotionState(0.0, 0.0, -10.0, 0.0), + 12.125, + 110.0 + ); + // Moving away from goal, stop and triangle move. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new MotionProfileGoal(8.75, 5.0), + new MotionState(0.0, 5.0, -10.0, 0.0), + 2.5, + 13.75 + ); + } + + @Test + public void problematicCase1() { + MotionProfile profile = MotionProfileGenerator.generateProfile( + new MotionProfileConstraints(50.0, -50.0, 25.0), + new MotionProfileGoal(200.0), + new MotionState(0.0, 0.0, 0.0, 0.0) + ); + System.out.println(profile); + assertTrue(profile.firstStateByPos(160.0).get().vel() > 0.0); + } + + @Test + public void testAsymmetricVelocityLimits() { + // Moving away from goal. + testProfile( + new MotionProfileConstraints(10.0, -5.0, 10.0), + new MotionProfileGoal(10.0, 0.0), + new MotionState(0.0, 0.0, -5.0, 0.0), + 2.625, + 12.5 + ); + + // Moving towards goal. + testProfile( + new MotionProfileConstraints(10.0, -5.0, 10.0), + new MotionProfileGoal(10.0, 0.0), + new MotionState(0.0, 0.0, 5.0, 0.0), + 1.625, + 10.0 + ); + + // Moving towards negative goal. + testProfile( + new MotionProfileConstraints(10.0, -5.0, 10.0), + new MotionProfileGoal(-10.0, 0.0), + new MotionState(0.0, 0.0, 5.0, 0.0), + 3.25, + 12.5 + ); + + // Moving away from goal, stop and trapezoid move. + testProfile( + new MotionProfileConstraints(10.0, -5.0, 10.0), + new MotionProfileGoal(100.0, 5.0), + new MotionState(0.0, 0.0, -5.0, 0.0), + 11.25, + 102.5 + ); + } + + @Test + public void testLinearTimeVaryingGoal() { + // No goal velocity. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new LinearTimeVaryingMotionProfileGoal(0.0, 100.0, 0.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 11.0, + 100.0 + ); + + // Goal moving away. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new LinearTimeVaryingMotionProfileGoal(0.0, 10.0, 5.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 3.25, + 26.25 + ); + + // Goal moving towards us. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new LinearTimeVaryingMotionProfileGoal(0.0, 10.0, -5.0), + new MotionState(0.0, 0.0, 0.0, 0.0), + 1.621320, + 4.393398 + ); + + // Goal moving towards us, we are moving towards goal. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new LinearTimeVaryingMotionProfileGoal(0.0, 10.0, -5.0), + new MotionState(0.0, 0.0, 2.0, 0.0), + 1.5315914, + 4.842043 + ); + + // Goal moving towards us, we are moving towards goal really fast. + testProfile( + new MotionProfileConstraints(10.0, -10.0, 10.0), + new LinearTimeVaryingMotionProfileGoal(0.0, 10.0, -5.0), + new MotionState(0.0, 0.0, 10.0, 0.0), + 2.207107, + 11.0355339 + ); + + // Goal moving away faster than we can move. + { + var constraints = new MotionProfileConstraints(10.0, -10.0, 10.0); + var goal = new LinearTimeVaryingMotionProfileGoal(0.0, 10.0, 10.0); + var start = new MotionState(0.0, 0.0, 0.0, 0.0); + var profile = MotionProfileGenerator.generateProfile(constraints, goal, start); + validateProfile(constraints, goal, start, profile, true); + goal = new LinearTimeVaryingMotionProfileGoal(0.0, -10.0, -10.0); + profile = MotionProfileGenerator.generateProfile(constraints, goal, start); + validateProfile(constraints, goal, start, profile, true); + } + } +} diff --git a/src/test/java/com/team254/lib/motion/MotionProfileTest.java b/src/test/java/com/team254/lib/motion/MotionProfileTest.java new file mode 100644 index 0000000..504ad91 --- /dev/null +++ b/src/test/java/com/team254/lib/motion/MotionProfileTest.java @@ -0,0 +1,227 @@ +package com.team254.lib.motion; + +import static org.hamcrest.CoreMatchers.equalTo; +import static org.hamcrest.CoreMatchers.is; +import static org.junit.Assert.*; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import org.junit.Before; +import org.junit.Test; + +public class MotionProfileTest { + List segments; + List segments_reversed; + List segments_combined; + + @Before + public void setUp() { + segments = new ArrayList<>(); + // Accelerate at 1 unit/s^2 for 1 second + segments.add( + new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, 0.5, 1.0, 1.0)) + ); + // Cruise at 1 unit/s for 1 second + segments.add( + new MotionSegment(new MotionState(1.0, 0.5, 1.0, 0.0), new MotionState(2.0, 1.5, 1.0, 0.0)) + ); + // Decelerate at -1 unit/s^2 for 1 second + segments.add(new MotionSegment( + new MotionState(2.0, 1.5, 1.0, -1.0), new MotionState(3.0, 2.0, 0.0, -1.0) + )); + + segments_reversed = new ArrayList<>(); + // Accelerate at -1 unit/s^2 for 1 second + segments_reversed.add(new MotionSegment( + new MotionState(3.0, 2.0, 0.0, -1.0), new MotionState(4.0, 1.5, -1.0, -1.0) + )); + // Cruise at -1 unit/s for 1 second + segments_reversed.add(new MotionSegment( + new MotionState(4.0, 1.5, -1.0, 0.0), new MotionState(5.0, 0.5, -1.0, 0.0) + )); + // Decelerate at -1 unit/s^2 for 1 second + segments_reversed.add( + new MotionSegment(new MotionState(5.0, 0.5, -1.0, 1.0), new MotionState(6.0, 0.0, 0.0, 1.0)) + ); + + segments_combined = new ArrayList<>(); + segments_combined.addAll(segments); + segments_combined.addAll(segments_reversed); + } + + @Test + public void smoke() { + MotionProfile a = new MotionProfile(); + assertTrue(a.isEmpty()); + assertTrue(a.isValid()); + assertThat(a.length(), is(equalTo(0.0))); + assertThat(a.duration(), is(equalTo(Double.NaN))); + + a = new MotionProfile(segments); + assertFalse(a.isEmpty()); + assertTrue(a.isValid()); + assertThat(a.startPos(), is(equalTo(0.0))); + assertThat(a.endPos(), is(equalTo(2.0))); + assertThat(a.length(), is(equalTo(2.0))); + assertThat(a.startTime(), is(equalTo(0.0))); + assertThat(a.endTime(), is(equalTo(3.0))); + assertThat(a.duration(), is(equalTo(3.0))); + } + + @Test + public void isValid() { + MotionProfile a = new MotionProfile(); + a.appendSegment( + new MotionSegment(new MotionState(0.0, 0.0, 0.0, 0.0), new MotionState(0.0, 0.0, 0.0, 1.0)) + ); + assertFalse(a.isValid()); + + a = new MotionProfile(); + a.appendSegment( + new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, 0.0, 1.0, 1.0)) + ); + assertFalse(a.isValid()); + + a = new MotionProfile(); + a.appendSegment( + new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, .5, 1.0, 1.0)) + ); + assertTrue(a.isValid()); + } + + @Test + public void appendControl() { + MotionProfile a = new MotionProfile(segments); + + // 3 second trapezoid traveling a displacement of 2 units. Peak velocity of 1.0 units/s. + a.appendControl(1.0, 1.0); + a.appendControl(0.0, 1.0); + a.appendControl(-1.0, 1.0); + assertFalse(a.isEmpty()); + assertTrue(a.isValid()); + assertThat(a.startPos(), is(equalTo(0.0))); + assertThat(a.endPos(), is(equalTo(4.0))); + assertThat(a.length(), is(equalTo(4.0))); + assertThat(a.startTime(), is(equalTo(0.0))); + assertThat(a.endTime(), is(equalTo(6.0))); + assertThat(a.duration(), is(equalTo(6.0))); + } + + @Test + public void appendSegment() { + MotionProfile a = new MotionProfile(segments); + + // 3 second trapezoid traveling a displacement of 2 units. Peak velocity of 1.0 units/s. + a.appendSegment( + new MotionSegment(new MotionState(3.0, 2.0, 0.0, 1.0), new MotionState(4.0, 2.5, 1.0, 1.0)) + ); + a.appendSegment( + new MotionSegment(new MotionState(4.0, 2.5, 1.0, 0.0), new MotionState(5.0, 3.5, 1.0, 0.0)) + ); + a.appendSegment(new MotionSegment( + new MotionState(5.0, 3.5, 1.0, -1.0), new MotionState(6.0, 4.0, 0.0, -1.0) + )); + assertFalse(a.isEmpty()); + assertTrue(a.isValid()); + assertThat(a.startPos(), is(equalTo(0.0))); + assertThat(a.endPos(), is(equalTo(4.0))); + assertThat(a.length(), is(equalTo(4.0))); + assertThat(a.startTime(), is(equalTo(0.0))); + assertThat(a.endTime(), is(equalTo(6.0))); + assertThat(a.duration(), is(equalTo(6.0))); + + // Add an invalid segment. + a.appendSegment( + new MotionSegment(new MotionState(3.0, 2.0, 0.0, 1.0), new MotionState(4.0, 2.5, 1.0, 1.0)) + ); + assertFalse(a.isValid()); + } + + @Test + public void appendProfile() { + MotionProfile a = new MotionProfile(segments); + MotionProfile b = new MotionProfile(segments_reversed); + a.appendProfile(b); + assertTrue(a.isValid()); + assertThat(a.endState(), is(equalTo(b.endState()))); + } + + @Test + public void stateByTime() { + MotionProfile a = new MotionProfile(segments); + assertThat(a.stateByTime(-0.1), is(equalTo(Optional.empty()))); + assertThat(a.stateByTime(0.0).get(), is(equalTo(segments.get(0).start()))); + assertThat(a.stateByTime(0.5).get(), is(equalTo(segments.get(0).start().extrapolate(0.5)))); + assertThat(a.stateByTime(1.0).get(), is(equalTo(segments.get(0).end()))); + assertThat(a.stateByTime(1.25).get(), is(equalTo(segments.get(1).start().extrapolate(1.25)))); + assertThat(a.stateByTime(2.0).get(), is(equalTo(segments.get(1).end()))); + assertThat(a.stateByTime(3.0).get(), is(equalTo(segments.get(2).end()))); + assertThat(a.stateByTime(3.1), is(equalTo(Optional.empty()))); + + a = new MotionProfile(segments_reversed); + assertThat(a.stateByTime(2.9), is(equalTo(Optional.empty()))); + assertThat(a.stateByTime(3.0).get(), is(equalTo(segments_reversed.get(0).start()))); + assertThat( + a.stateByTime(3.5).get(), is(equalTo(segments_reversed.get(0).start().extrapolate(3.5))) + ); + assertThat(a.stateByTime(4.0).get(), is(equalTo(segments_reversed.get(0).end()))); + assertThat( + a.stateByTime(4.25).get(), is(equalTo(segments_reversed.get(1).start().extrapolate(4.25))) + ); + assertThat(a.stateByTime(5.0).get(), is(equalTo(segments_reversed.get(1).end()))); + assertThat(a.stateByTime(6.0).get(), is(equalTo(segments_reversed.get(2).end()))); + assertThat(a.stateByTime(6.1), is(equalTo(Optional.empty()))); + } + + @Test + public void firstStateByPos() { + MotionProfile a = new MotionProfile(segments); + assertThat(a.firstStateByPos(-0.1), is(equalTo(Optional.empty()))); + assertThat(a.firstStateByPos(0.0).get(), is(equalTo(segments.get(0).start()))); + assertThat(a.firstStateByPos(0.5).get(), is(equalTo(segments.get(0).end()))); + assertThat(a.firstStateByPos(1.0).get(), is(equalTo(segments.get(1).start().extrapolate(1.5)))); + assertThat(a.firstStateByPos(1.5).get(), is(equalTo(segments.get(1).end()))); + assertThat(a.firstStateByPos(2.0).get(), is(equalTo(segments.get(2).end()))); + assertThat(a.firstStateByPos(2.1), is(equalTo(Optional.empty()))); + + a = new MotionProfile(segments_reversed); + assertThat(a.firstStateByPos(-0.1), is(equalTo(Optional.empty()))); + assertThat(a.firstStateByPos(0.0).get(), is(equalTo(segments_reversed.get(2).end()))); + assertThat(a.firstStateByPos(0.5).get(), is(equalTo(segments_reversed.get(1).end()))); + assertThat( + a.firstStateByPos(1.0).get(), is(equalTo(segments_reversed.get(1).start().extrapolate(4.5))) + ); + assertThat(a.firstStateByPos(1.5).get(), is(equalTo(segments_reversed.get(0).end()))); + assertThat(a.firstStateByPos(2.0).get(), is(equalTo(segments_reversed.get(0).start()))); + assertThat(a.firstStateByPos(2.1), is(equalTo(Optional.empty()))); + + a = new MotionProfile(segments_combined); + assertThat(a.firstStateByPos(-0.1), is(equalTo(Optional.empty()))); + assertThat(a.firstStateByPos(0.0).get(), is(equalTo(segments.get(0).start()))); + assertThat(a.firstStateByPos(0.5).get(), is(equalTo(segments.get(0).end()))); + assertThat(a.firstStateByPos(1.0).get(), is(equalTo(segments.get(1).start().extrapolate(1.5)))); + assertThat(a.firstStateByPos(1.5).get(), is(equalTo(segments.get(1).end()))); + assertThat(a.firstStateByPos(2.0).get(), is(equalTo(segments.get(2).end()))); + assertThat(a.firstStateByPos(2.1), is(equalTo(Optional.empty()))); + } + + @Test + public void trimBeforeTime() { + List segments_copy = new ArrayList<>(); + segments_copy.addAll(segments); + MotionProfile a = new MotionProfile(segments_copy); + a.trimBeforeTime(-1.0); + assertThat(a.stateByTime(0.0).get(), is(equalTo(segments.get(0).start()))); + a.trimBeforeTime(0.0); + assertThat(a.stateByTime(0.0).get(), is(equalTo(segments.get(0).start()))); + a.trimBeforeTime(0.5); + assertThat(a.stateByTime(0.0), is(equalTo(Optional.empty()))); + assertThat(a.stateByTime(0.5).get(), is(equalTo(segments.get(0).start().extrapolate(0.5)))); + a.trimBeforeTime(1.0); + assertThat(a.stateByTime(0.5), is(equalTo(Optional.empty()))); + assertThat(a.stateByTime(1.0).get(), is(equalTo(segments.get(1).start()))); + a.trimBeforeTime(10.0); + assertTrue(a.isEmpty()); + } +} diff --git a/src/test/java/com/team254/lib/motion/MotionSegmentTest.java b/src/test/java/com/team254/lib/motion/MotionSegmentTest.java new file mode 100644 index 0000000..c7f6336 --- /dev/null +++ b/src/test/java/com/team254/lib/motion/MotionSegmentTest.java @@ -0,0 +1,21 @@ +package com.team254.lib.motion; + +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import org.junit.Test; + +public class MotionSegmentTest { + @Test + public void valid() { + MotionSegment a = + new MotionSegment(new MotionState(0.0, 0.0, 0.0, 0.0), new MotionState(0.0, 0.0, 0.0, 1.0)); + assertFalse(a.isValid()); + + a = new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, 0.0, 1.0, 1.0)); + assertFalse(a.isValid()); + + a = new MotionSegment(new MotionState(0.0, 0.0, 0.0, 1.0), new MotionState(1.0, .5, 1.0, 1.0)); + assertTrue(a.isValid()); + } +} diff --git a/src/test/java/com/team254/lib/motion/MotionStateTest.java b/src/test/java/com/team254/lib/motion/MotionStateTest.java new file mode 100644 index 0000000..4ab107d --- /dev/null +++ b/src/test/java/com/team254/lib/motion/MotionStateTest.java @@ -0,0 +1,108 @@ +package com.team254.lib.motion; + +import static org.hamcrest.CoreMatchers.*; +import static org.junit.Assert.*; + +import org.junit.Test; + +public class MotionStateTest { + @Test + public void equals() { + MotionState a = new MotionState(0.0, 0.0, 0.0, 0.0); + MotionState b = new MotionState(0.0, 0.0, 0.0, 0.0); + assertThat(b, is(equalTo(a))); + + final double kSmallEps = 1e-9; + MotionState c = new MotionState(kSmallEps, kSmallEps, kSmallEps, kSmallEps); + assertThat(c, is(equalTo(a))); + assertTrue(a.equals(c, kSmallEps)); + + final double kBigEps = 1e-3; + MotionState d = new MotionState(kBigEps, kBigEps, kBigEps, kBigEps); + assertTrue(a.equals(d, kBigEps)); + assertFalse(a.equals(d, kSmallEps)); + assertThat(d, is(not(equalTo(a)))); + } + + @Test + public void extrapolateConstantVelocity() { + // vel = 1 + MotionState a = new MotionState(0.0, 0.0, 1.0, 0.0); + + MotionState expected_after_0s = new MotionState(0.0, 0.0, 1.0, 0.0); + assertThat(a.extrapolate(0.0), is(equalTo(expected_after_0s))); + + MotionState expected_after_1s = new MotionState(1.0, 1.0, 1.0, 0.0); + assertThat(a.extrapolate(1.0), is(equalTo(expected_after_1s))); + assertThat(expected_after_1s.extrapolate(0.0), is(equalTo(a))); + + MotionState expected_after_neg1s = new MotionState(-1.0, -1.0, 1.0, 0.0); + assertThat(a.extrapolate(-1.0), is(equalTo(expected_after_neg1s))); + assertThat(expected_after_neg1s.extrapolate(0.0), is(equalTo(a))); + } + + @Test + public void extrapolateConstantAccel() { + // vel = 1, acc = 1 + MotionState a = new MotionState(0.0, 0.0, 1.0, 1.0); + + MotionState expected_after_0s = new MotionState(0.0, 0.0, 1.0, 1.0); + assertThat(a.extrapolate(0.0), is(equalTo(expected_after_0s))); + + MotionState expected_after_1s = new MotionState(1.0, 1.5, 2.0, 1.0); + assertThat(a.extrapolate(1.0), is(equalTo(expected_after_1s))); + assertThat(expected_after_1s.extrapolate(0.0), is(equalTo(a))); + + MotionState expected_after_neg1s = new MotionState(-1.0, -0.5, 0.0, 1.0); + assertThat(a.extrapolate(-1.0), is(equalTo(expected_after_neg1s))); + assertThat(expected_after_neg1s.extrapolate(0.0), is(equalTo(a))); + } + + @Test + public void nextTimeAtPosStationary() { + MotionState a = new MotionState(0.0, 3.0, 0.0, 0.0); + assertThat(a.nextTimeAtPos(3.0), is(equalTo(0.0))); + + a = new MotionState(1.0, 3.0, 0.0, 0.0); + assertThat(a.nextTimeAtPos(3.0), is(equalTo(1.0))); + } + + @Test + public void nextTimeAtPosConstantVelocity() { + MotionState a = new MotionState(0.0, 1.0, 1.0, 0.0); + assertThat(a.nextTimeAtPos(1.0), is(equalTo(0.0))); + assertThat(a.nextTimeAtPos(2.0), is(equalTo(1.0))); + assertThat(a.nextTimeAtPos(0.0), is(equalTo(Double.NaN))); + + a = new MotionState(1.0, 1.0, 1.0, 0.0); + assertThat(a.nextTimeAtPos(1.0), is(equalTo(1.0))); + assertThat(a.nextTimeAtPos(2.0), is(equalTo(2.0))); + assertThat(a.nextTimeAtPos(0.0), is(equalTo(Double.NaN))); + } + + @Test + public void nextTimeAtPosConstantAccel() { + MotionState a = new MotionState(0.0, 0.0, 0.0, 1.0); + assertThat(a.nextTimeAtPos(0.0), is(equalTo(0.0))); + assertThat(a.nextTimeAtPos(0.5), is(equalTo(1.0))); + assertThat(a.nextTimeAtPos(-1.0), is(equalTo(Double.NaN))); + + a = new MotionState(1.0, 1.0, 1.0, 1.0); + assertThat(a.nextTimeAtPos(1.0), is(equalTo(1.0))); + assertThat(a.nextTimeAtPos(2.5), is(equalTo(2.0))); + assertThat(a.nextTimeAtPos(-1.0), is(equalTo(Double.NaN))); + + a = new MotionState(1.0, 1.0, 1.0, -1.0); + assertThat(a.nextTimeAtPos(1.0), is(equalTo(1.0))); + assertThat(a.nextTimeAtPos(1.5), is(equalTo(2.0))); + assertThat(a.nextTimeAtPos(1.51), is(equalTo(Double.NaN))); + assertThat(a.nextTimeAtPos(-0.5), is(equalTo(4.0))); + } + + @Test + public void flipped() { + MotionState a = new MotionState(1.0, 1.0, 1.0, 1.0); + assertThat(a.flipped(), is(equalTo(new MotionState(1.0, -1.0, -1.0, -1.0)))); + assertThat(a.flipped().flipped(), is(equalTo(a))); + } +} diff --git a/src/test/java/com/team254/lib/motion/MotionTestUtil.java b/src/test/java/com/team254/lib/motion/MotionTestUtil.java new file mode 100644 index 0000000..b13999c --- /dev/null +++ b/src/test/java/com/team254/lib/motion/MotionTestUtil.java @@ -0,0 +1,88 @@ +package com.team254.lib.motion; + +public class MotionTestUtil { + protected abstract static class Dynamics { + protected MotionState mState; + + public Dynamics(MotionState state) { + mState = state; + } + + public MotionState getState() { + return mState; + } + + public abstract void update(double command_vel, double dt); + } + + protected static class IdealDynamics extends Dynamics { + public IdealDynamics(MotionState state) { + super(state); + } + + @Override + public void update(double command_vel, double dt) { + final double acc = (command_vel - mState.vel()) / dt; + mState = mState.extrapolate(mState.t() + dt, acc); + } + } + + protected static class ScaledDynamics extends Dynamics { + protected double mVelRatio; + + public ScaledDynamics(MotionState state, double vel_ratio) { + super(state); + mVelRatio = vel_ratio; + } + + @Override + public void update(double command_vel, double dt) { + final double acc = (command_vel * mVelRatio - mState.vel()) / dt; + mState = mState.extrapolate(mState.t() + dt, acc); + } + } + + protected static class DeadbandDynamics extends Dynamics { + protected double mDeadband; + + public DeadbandDynamics(MotionState state, double deadband) { + super(state); + mDeadband = deadband; + } + + @Override + public void update(double command_vel, double dt) { + if (command_vel > -mDeadband && command_vel < mDeadband) { + command_vel = 0.0; + } else { + command_vel = Math.signum(command_vel) * (Math.abs(command_vel) - mDeadband); + } + final double acc = (command_vel - mState.vel()) / dt; + mState = mState.extrapolate(mState.t() + dt, acc); + } + } + + public static MotionState followProfile( + ProfileFollower follower, Dynamics dynamics, double dt, int max_iterations + ) { + int i = 0; + for (; i < max_iterations && !follower.onTarget(); ++i) { + MotionState state = dynamics.getState(); + final double t = state.t() + dt; + final double command_vel = follower.update(state, t); + dynamics.update(command_vel, dt); + System.out.println( + "State: " + state + ", Pos error: " + follower.getPosError() + + ", Vel error: " + follower.getVelError() + ", Command: " + command_vel + ); + if (follower.isFinishedProfile()) { + System.out.println("Follower has finished profile"); + } + } + if (i == max_iterations) { + System.out.println("Iteration limit reached"); + } + System.out.println("Final state: " + dynamics.getState()); + return dynamics.getState(); + } +} diff --git a/src/test/java/com/team254/lib/spline/QuinticHermiteOptimizerTest.java b/src/test/java/com/team254/lib/spline/QuinticHermiteOptimizerTest.java new file mode 100755 index 0000000..144d1b8 --- /dev/null +++ b/src/test/java/com/team254/lib/spline/QuinticHermiteOptimizerTest.java @@ -0,0 +1,64 @@ +package com.team254.lib.spline; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.Util; +import java.util.ArrayList; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class QuinticHermiteOptimizerTest { + private static double kEpsilon = Util.kEpsilon; + + @Test + public void test() { + Pose2d a = new Pose2d(new Translation2d(0, 100), Rotation2d254.fromDegrees(270)); + Pose2d b = new Pose2d(new Translation2d(50, 0), Rotation2d254.fromDegrees(0)); + Pose2d c = new Pose2d(new Translation2d(100, 100), Rotation2d254.fromDegrees(90)); + + List splines = new ArrayList<>(); + splines.add(new QuinticHermiteSpline(a, b)); + splines.add(new QuinticHermiteSpline(b, c)); + + long startTime = System.currentTimeMillis(); + Assert.assertTrue(QuinticHermiteSpline.optimizeSpline(splines) < 0.014); + System.out.println("Optimization time (ms): " + (System.currentTimeMillis() - startTime)); + + Pose2d d = new Pose2d(new Translation2d(0, 0), Rotation2d254.fromDegrees(90)); + Pose2d e = new Pose2d(new Translation2d(0, 50), Rotation2d254.fromDegrees(0)); + Pose2d f = new Pose2d(new Translation2d(100, 0), Rotation2d254.fromDegrees(90)); + Pose2d g = new Pose2d(new Translation2d(100, 100), Rotation2d254.fromDegrees(0)); + + List splines1 = new ArrayList<>(); + splines1.add(new QuinticHermiteSpline(d, e)); + splines1.add(new QuinticHermiteSpline(e, f)); + splines1.add(new QuinticHermiteSpline(f, g)); + + startTime = System.currentTimeMillis(); + Assert.assertTrue(QuinticHermiteSpline.optimizeSpline(splines1) < 0.16); + System.out.println("Optimization time (ms): " + (System.currentTimeMillis() - startTime)); + + Pose2d h = new Pose2d(new Translation2d(0, 0), Rotation2d254.fromDegrees(0)); + Pose2d i = new Pose2d(new Translation2d(50, 0), Rotation2d254.fromDegrees(0)); + Pose2d j = new Pose2d(new Translation2d(100, 50), Rotation2d254.fromDegrees(45)); + Pose2d k = new Pose2d(new Translation2d(150, 0), Rotation2d254.fromDegrees(270)); + Pose2d l = new Pose2d(new Translation2d(150, -50), Rotation2d254.fromDegrees(270)); + + List splines2 = new ArrayList<>(); + splines2.add(new QuinticHermiteSpline(h, i)); + splines2.add(new QuinticHermiteSpline(i, j)); + splines2.add(new QuinticHermiteSpline(j, k)); + splines2.add(new QuinticHermiteSpline(k, l)); + + startTime = System.currentTimeMillis(); + Assert.assertTrue(QuinticHermiteSpline.optimizeSpline(splines2) < 0.05); + Assert.assertEquals(splines2.get(0).getCurvature(1.0), 0.0, kEpsilon); + Assert.assertEquals(splines2.get(2).getCurvature(1.0), 0.0, kEpsilon); + System.out.println("Optimization time (ms): " + (System.currentTimeMillis() - startTime)); + } +} diff --git a/src/test/java/com/team254/lib/spline/SplineGeneratorTest.java b/src/test/java/com/team254/lib/spline/SplineGeneratorTest.java new file mode 100755 index 0000000..6d71653 --- /dev/null +++ b/src/test/java/com/team254/lib/spline/SplineGeneratorTest.java @@ -0,0 +1,46 @@ +package com.team254.lib.spline; + +import com.team254.lib.geometry.*; +import com.team254.lib.trajectory.TrajectoryPoint; +import com.team254.lib.trajectory.TrajectoryTest; +import com.team254.lib.util.Util; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class SplineGeneratorTest { + public static final double kTestEpsilon = Util.kEpsilon; + + @Test + public void test() { + // Create the test spline + Pose2d p1 = new Pose2d(new Translation2d(0, 0), new Rotation2d254()); + Pose2d p2 = new Pose2d(new Translation2d(15, 10), new Rotation2d254(1, -5, true)); + Spline s = new QuinticHermiteSpline(p1, p2); + List headings = + List.of(Rotation2d254.fromDegrees(0), Rotation2d254.fromDegrees(90)); + + List> samples = + SplineGenerator.parameterizeSpline(s, headings); + + double arclength = 0; + Rotation2d254 cur_heading = Rotation2d254.identity(); + Pose2dWithCurvature cur_pose = samples.get(0).state(); + for (TrajectoryPoint point : samples) { + Pose2dWithCurvature sample = point.state(); + final Twist2d t = Pose2d.log(cur_pose.getPose().inverse().transformBy(sample.getPose())); + arclength += t.dx; + cur_pose = sample; + cur_heading = point.heading(); + } + + Assert.assertEquals(cur_pose.getTranslation().x(), 15.0, kTestEpsilon); + Assert.assertEquals(cur_pose.getTranslation().y(), 10.0, kTestEpsilon); + Assert.assertEquals(cur_pose.getRotation().getDegrees(), -78.69006752597981, kTestEpsilon); + Assert.assertEquals(arclength, 23.17291953186379, kTestEpsilon); + Assert.assertEquals(cur_heading.getRadians(), headings.get(1).getRadians(), kTestEpsilon); + } +} diff --git a/src/test/java/com/team254/lib/swerve/SwerveSetpointGeneratorTest.java b/src/test/java/com/team254/lib/swerve/SwerveSetpointGeneratorTest.java new file mode 100644 index 0000000..4a4b6ac --- /dev/null +++ b/src/test/java/com/team254/lib/swerve/SwerveSetpointGeneratorTest.java @@ -0,0 +1,100 @@ +package com.team254.lib.swerve; + +import static org.junit.Assert.assertTrue; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.Util; +import org.junit.Test; + +public class SwerveSetpointGeneratorTest { + protected final static double kRobotSide = 0.616; // m + protected final static SwerveDriveKinematics kKinematics = new SwerveDriveKinematics( + // Front left + new Translation2d(kRobotSide / 2.0, kRobotSide / 2.0), + // Front right + new Translation2d(kRobotSide / 2.0, -kRobotSide / 2.0), + // Back left + new Translation2d(-kRobotSide / 2.0, kRobotSide / 2.0), + // Back right + new Translation2d(-kRobotSide / 2.0, -kRobotSide / 2.0) + ); + protected final static SwerveSetpointGenerator.KinematicLimits kKinematicLimits = + new SwerveSetpointGenerator.KinematicLimits(); + static { + kKinematicLimits.kMaxDriveVelocity = 5.0; // m/s + kKinematicLimits.kMaxDriveAcceleration = 10.0; // m/s^2 + kKinematicLimits.kMaxSteeringVelocity = Math.toRadians(1500.0); // rad/s + }; + protected final static double kDt = 0.01; // s + protected final static double kMaxSteeringVelocityError = Math.toRadians(2.0); // rad/s + protected final static double kMaxAccelerationError = 0.1; // m/s^2 + + public void SatisfiesConstraints(SwerveSetpoint prev, SwerveSetpoint next) { + for (int i = 0; i < prev.mModuleStates.length; ++i) { + final var prevModule = prev.mModuleStates[i]; + final var nextModule = next.mModuleStates[i]; + Rotation2d254 diffRotation = prevModule.angle.inverse().rotateBy(nextModule.angle); + assertTrue( + Math.abs(diffRotation.getRadians()) + < kKinematicLimits.kMaxSteeringVelocity + kMaxSteeringVelocityError + ); + assertTrue(Math.abs(nextModule.speedMetersPerSecond) <= kKinematicLimits.kMaxDriveVelocity); + assertTrue( + Math.abs(nextModule.speedMetersPerSecond - prevModule.speedMetersPerSecond) / kDt + <= kKinematicLimits.kMaxDriveAcceleration + kMaxAccelerationError + ); + } + } + + public SwerveSetpoint driveToGoal( + SwerveSetpoint prevSetpoint, ChassisSpeeds goal, SwerveSetpointGenerator generator + ) { + System.out.println("Driving to goal state " + goal); + System.out.println("Initial state: " + prevSetpoint); + while (!prevSetpoint.mChassisSpeeds.toTwist2d().epsilonEquals(goal.toTwist2d(), Util.kEpsilon) + ) { + var newsetpoint = generator.generateSetpoint(kKinematicLimits, prevSetpoint, goal, kDt); + System.out.println(newsetpoint); + SatisfiesConstraints(prevSetpoint, newsetpoint); + prevSetpoint = newsetpoint; + } + return prevSetpoint; + } + + @Test + public void testGenerateSetpoint() { + SwerveModuleState[] initialStates = { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState()}; + SwerveSetpoint setpoint = new SwerveSetpoint(new ChassisSpeeds(), initialStates); + + var generator = new SwerveSetpointGenerator(kKinematics); + + var goalSpeeds = new ChassisSpeeds(0.0, 0.0, 1.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(0.0, 0.0, -1.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(1.0, 0.0, 0.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(0.0, 1.0, 0.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(0.1, -1.0, 0.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(1.0, -0.5, 0.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + + goalSpeeds = new ChassisSpeeds(1.0, 0.4, 0.0); + setpoint = driveToGoal(setpoint, goalSpeeds, generator); + } +} diff --git a/src/test/java/com/team254/lib/trajectory/DistanceViewTest.java b/src/test/java/com/team254/lib/trajectory/DistanceViewTest.java new file mode 100755 index 0000000..21a1318 --- /dev/null +++ b/src/test/java/com/team254/lib/trajectory/DistanceViewTest.java @@ -0,0 +1,57 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.Util; +import java.util.Arrays; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class DistanceViewTest { + public static final double kTestEpsilon = Util.kEpsilon; + + @Test + public void test() { + // Specify desired waypoints. + List waypoints = Arrays.asList( + new Translation2d(0.0, 0.0), + new Translation2d(24.0, 0.0), + new Translation2d(36.0, 0.0), + new Translation2d(36.0, 24.0), + new Translation2d(60.0, 24.0) + ); + List headings = Arrays.asList( + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(30), + Rotation2d254.fromDegrees(60), + Rotation2d254.fromDegrees(90), + Rotation2d254.fromDegrees(180) + ); + + // Create the reference trajectory (straight line motion between waypoints). + Trajectory trajectory = new Trajectory<>(waypoints, headings); + final DistanceView distance_view = new DistanceView<>(trajectory); + + Assert.assertEquals(0.0, distance_view.first_interpolant(), kTestEpsilon); + Assert.assertEquals(84.0, distance_view.last_interpolant(), kTestEpsilon); + + Assert.assertEquals(waypoints.get(0), distance_view.sample(0.0).state()); + Assert.assertEquals( + waypoints.get(0).interpolate(waypoints.get(1), 0.5), distance_view.sample(12.0).state() + ); + Assert.assertEquals( + waypoints.get(3).interpolate(waypoints.get(4), 0.5), distance_view.sample(72.0).state() + ); + Assert.assertEquals(headings.get(0), distance_view.sample(0.0).heading()); + Assert.assertEquals( + headings.get(0).interpolate(headings.get(1), 0.5), distance_view.sample(12).heading() + ); + Assert.assertEquals( + headings.get(3).interpolate(headings.get(4), 0.5), distance_view.sample(72.0).heading() + ); + } +} diff --git a/src/test/java/com/team254/lib/trajectory/PurePursuitControllerTest.java b/src/test/java/com/team254/lib/trajectory/PurePursuitControllerTest.java new file mode 100755 index 0000000..428e0a8 --- /dev/null +++ b/src/test/java/com/team254/lib/trajectory/PurePursuitControllerTest.java @@ -0,0 +1,55 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.geometry.Twist2d; +import java.util.Arrays; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class PurePursuitControllerTest { + @Test + public void test() { + List waypoints = Arrays.asList( + new Translation2d(0.0, 0.0), + new Translation2d(24.0, 0.0), + new Translation2d(36.0, 12.0), + new Translation2d(60.0, 12.0) + ); + List headings = Arrays.asList( + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(30), + Rotation2d254.fromDegrees(60), + Rotation2d254.fromDegrees(120) + ); + + // Create the reference trajectory (straight line motion between waypoints). + Trajectory reference_trajectory = + new Trajectory<>(waypoints, headings); + DistanceView arc_length_parameterized_trajectory = + new DistanceView<>(reference_trajectory); + PurePursuitController controller = + new PurePursuitController<>(arc_length_parameterized_trajectory, 1.0, 6.0, 0.1); + + Pose2d robot_pose = new Pose2d(waypoints.get(0), Rotation2d254.identity()); + final int kMaxIter = 100; + int i = 0; + for (; i < kMaxIter; ++i) { + if (controller.isDone()) + break; + Twist2d steering_command = controller.steer(robot_pose); + steering_command = steering_command.scaled(1.0 / Math.max(1.0, steering_command.norm())); + System.out.println( + "Iter: " + i + ", Pose: " + robot_pose + ", Steering Command: " + steering_command + ); + robot_pose = robot_pose.transformBy(Pose2d.exp(steering_command)); + } + System.out.println("i = " + i); + Assert.assertTrue(i <= kMaxIter); + } +} diff --git a/src/test/java/com/team254/lib/trajectory/TrajectoryIteratorTest.java b/src/test/java/com/team254/lib/trajectory/TrajectoryIteratorTest.java new file mode 100755 index 0000000..284ee47 --- /dev/null +++ b/src/test/java/com/team254/lib/trajectory/TrajectoryIteratorTest.java @@ -0,0 +1,93 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.Util; +import java.util.Arrays; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class TrajectoryIteratorTest { + public static final double kTestEpsilon = Util.kEpsilon; + + public static final List kWaypoints = Arrays.asList( + new Translation2d(0.0, 0.0), + new Translation2d(24.0, 0.0), + new Translation2d(36.0, 12.0), + new Translation2d(60.0, 12.0) + ); + + List kHeadings = Arrays.asList( + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(30), + Rotation2d254.fromDegrees(60), + Rotation2d254.fromDegrees(90), + Rotation2d254.fromDegrees(180) + ); + + @Test + public void test() { + Trajectory traj = new Trajectory<>(kWaypoints, kHeadings); + TrajectoryIterator iterator = + new TrajectoryIterator<>(traj.getIndexView()); + + // Initial conditions. + Assert.assertEquals(0.0, iterator.getProgress(), kTestEpsilon); + Assert.assertEquals(3.0, iterator.getRemainingProgress(), kTestEpsilon); + Assert.assertEquals(kWaypoints.get(0), iterator.getState()); + Assert.assertEquals(kHeadings.get(0), iterator.getHeading()); + Assert.assertFalse(iterator.isDone()); + + // Advance forward. + Assert.assertEquals( + kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.5), iterator.preview(0.5).state() + ); + Assert.assertEquals( + kHeadings.get(0).interpolate(kHeadings.get(1), 0.5), iterator.preview(0.5).heading() + ); + TrajectorySamplePoint newPoint = iterator.advance(0.5); + Assert.assertEquals(kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.5), newPoint.state()); + Assert.assertEquals(kHeadings.get(0).interpolate(kHeadings.get(1), 0.5), newPoint.heading()); + Assert.assertEquals(0.5, iterator.getProgress(), kTestEpsilon); + Assert.assertEquals(2.5, iterator.getRemainingProgress(), kTestEpsilon); + Assert.assertFalse(iterator.isDone()); + + // Advance backwards. + Assert.assertEquals( + kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.25), iterator.preview(-0.25).state() + ); + Assert.assertEquals( + kHeadings.get(0).interpolate(kHeadings.get(1), 0.25), iterator.preview(-0.25).heading() + ); + newPoint = iterator.advance(-0.25); + Assert.assertEquals(kWaypoints.get(0).interpolate(kWaypoints.get(1), 0.25), newPoint.state()); + Assert.assertEquals(kHeadings.get(0).interpolate(kHeadings.get(1), 0.25), newPoint.heading()); + Assert.assertEquals(0.25, iterator.getProgress(), kTestEpsilon); + Assert.assertEquals(2.75, iterator.getRemainingProgress(), kTestEpsilon); + Assert.assertFalse(iterator.isDone()); + + // Advance past end. + Assert.assertEquals(kWaypoints.get(3), iterator.preview(5.0).state()); + Assert.assertEquals(kHeadings.get(3), iterator.preview(5.0).heading()); + newPoint = iterator.advance(5.0); + Assert.assertEquals(kWaypoints.get(3), newPoint.state()); + Assert.assertEquals(kHeadings.get(3), newPoint.heading()); + Assert.assertEquals(3.0, iterator.getProgress(), kTestEpsilon); + Assert.assertEquals(0.0, iterator.getRemainingProgress(), kTestEpsilon); + Assert.assertTrue(iterator.isDone()); + + // Advance past beginning. + Assert.assertEquals(kWaypoints.get(0), iterator.preview(-5.0).state()); + Assert.assertEquals(kHeadings.get(0), iterator.preview(-5.0).heading()); + newPoint = iterator.advance(-5.0); + Assert.assertEquals(kWaypoints.get(0), newPoint.state()); + Assert.assertEquals(kHeadings.get(0), newPoint.heading()); + Assert.assertEquals(0.0, iterator.getProgress(), kTestEpsilon); + Assert.assertEquals(3.0, iterator.getRemainingProgress(), kTestEpsilon); + Assert.assertFalse(iterator.isDone()); + } +} diff --git a/src/test/java/com/team254/lib/trajectory/TrajectoryTest.java b/src/test/java/com/team254/lib/trajectory/TrajectoryTest.java new file mode 100755 index 0000000..f8f8c0c --- /dev/null +++ b/src/test/java/com/team254/lib/trajectory/TrajectoryTest.java @@ -0,0 +1,141 @@ +package com.team254.lib.trajectory; + +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.Util; +import java.util.Arrays; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class TrajectoryTest { + public static final double kTestEpsilon = Util.kEpsilon; + + public static final List kWaypoints = Arrays.asList( + new Translation2d(0.0, 0.0), + new Translation2d(24.0, 0.0), + new Translation2d(36.0, 12.0), + new Translation2d(60.0, 12.0) + ); + + List kHeadings = Arrays.asList( + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(30), + Rotation2d254.fromDegrees(60), + Rotation2d254.fromDegrees(90), + Rotation2d254.fromDegrees(180) + ); + + @Test + public void testConstruction() { + // Empty constructor. + Trajectory traj = new Trajectory<>(); + Assert.assertTrue(traj.isEmpty()); + Assert.assertEquals(0.0, traj.getIndexView().first_interpolant(), kTestEpsilon); + Assert.assertEquals(0.0, traj.getIndexView().last_interpolant(), kTestEpsilon); + Assert.assertEquals(0, traj.length()); + + // Set states at construction time. + traj = new Trajectory<>(kWaypoints, kHeadings); + Assert.assertFalse(traj.isEmpty()); + Assert.assertEquals(0.0, traj.getIndexView().first_interpolant(), kTestEpsilon); + Assert.assertEquals(3.0, traj.getIndexView().last_interpolant(), kTestEpsilon); + Assert.assertEquals(4, traj.length()); + } + + @Test + public void testStateAccessors() { + Trajectory traj = new Trajectory<>(kWaypoints, kHeadings); + + Assert.assertEquals(kWaypoints.get(0), traj.getPoint(0).state()); + Assert.assertEquals(kWaypoints.get(1), traj.getPoint(1).state()); + Assert.assertEquals(kWaypoints.get(2), traj.getPoint(2).state()); + Assert.assertEquals(kWaypoints.get(3), traj.getPoint(3).state()); + + Assert.assertEquals(kHeadings.get(0), traj.getPoint(0).heading()); + Assert.assertEquals(kHeadings.get(1), traj.getPoint(1).heading()); + Assert.assertEquals(kHeadings.get(2), traj.getPoint(2).heading()); + Assert.assertEquals(kHeadings.get(3), traj.getPoint(3).heading()); + + Assert.assertEquals(kWaypoints.get(0), traj.getInterpolated(0.0).state()); + Assert.assertEquals(traj.getInterpolated(0.0).index_floor(), 0); + Assert.assertEquals(traj.getInterpolated(0.0).index_ceil(), 0); + Assert.assertEquals(kWaypoints.get(1), traj.getInterpolated(1.0).state()); + Assert.assertEquals(traj.getInterpolated(1.0).index_floor(), 1); + Assert.assertEquals(traj.getInterpolated(1.0).index_ceil(), 1); + Assert.assertEquals(kWaypoints.get(2), traj.getInterpolated(2.0).state()); + Assert.assertEquals(traj.getInterpolated(2.0).index_floor(), 2); + Assert.assertEquals(traj.getInterpolated(2.0).index_ceil(), 2); + Assert.assertEquals(kWaypoints.get(3), traj.getInterpolated(3.0).state()); + Assert.assertEquals(traj.getInterpolated(3.0).index_floor(), 3); + Assert.assertEquals(traj.getInterpolated(3.0).index_ceil(), 3); + + Assert.assertEquals(kHeadings.get(0), traj.getInterpolated(0.0).heading()); + Assert.assertEquals(traj.getInterpolated(0.0).index_floor(), 0); + Assert.assertEquals(traj.getInterpolated(0.0).index_ceil(), 0); + Assert.assertEquals(kHeadings.get(1), traj.getInterpolated(1.0).heading()); + Assert.assertEquals(traj.getInterpolated(1.0).index_floor(), 1); + Assert.assertEquals(traj.getInterpolated(1.0).index_ceil(), 1); + Assert.assertEquals(kHeadings.get(2), traj.getInterpolated(2.0).heading()); + Assert.assertEquals(traj.getInterpolated(2.0).index_floor(), 2); + Assert.assertEquals(traj.getInterpolated(2.0).index_ceil(), 2); + Assert.assertEquals(kHeadings.get(3), traj.getInterpolated(3.0).heading()); + Assert.assertEquals(traj.getInterpolated(3.0).index_floor(), 3); + Assert.assertEquals(traj.getInterpolated(3.0).index_ceil(), 3); + + Assert.assertEquals( + kWaypoints.get(0).interpolate(kWaypoints.get(1), .25), traj.getInterpolated(0.25).state() + ); + Assert.assertEquals(traj.getInterpolated(0.25).index_floor(), 0); + Assert.assertEquals(traj.getInterpolated(0.25).index_ceil(), 1); + Assert.assertEquals( + kWaypoints.get(1).interpolate(kWaypoints.get(2), .5), traj.getInterpolated(1.5).state() + ); + Assert.assertEquals(traj.getInterpolated(1.5).index_floor(), 1); + Assert.assertEquals(traj.getInterpolated(1.5).index_ceil(), 2); + Assert.assertEquals( + kWaypoints.get(2).interpolate(kWaypoints.get(3), .75), traj.getInterpolated(2.75).state() + ); + Assert.assertEquals(traj.getInterpolated(2.75).index_floor(), 2); + Assert.assertEquals(traj.getInterpolated(2.75).index_ceil(), 3); + + Assert.assertEquals( + kHeadings.get(0).interpolate(kHeadings.get(1), .25), traj.getInterpolated(0.25).heading() + ); + Assert.assertEquals(traj.getInterpolated(0.25).index_floor(), 0); + Assert.assertEquals(traj.getInterpolated(0.25).index_ceil(), 1); + Assert.assertEquals( + kHeadings.get(1).interpolate(kHeadings.get(2), .5), traj.getInterpolated(1.5).heading() + ); + Assert.assertEquals(traj.getInterpolated(1.5).index_floor(), 1); + Assert.assertEquals(traj.getInterpolated(1.5).index_ceil(), 2); + Assert.assertEquals( + kHeadings.get(2).interpolate(kHeadings.get(3), .75), traj.getInterpolated(2.75).heading() + ); + Assert.assertEquals(traj.getInterpolated(2.75).index_floor(), 2); + Assert.assertEquals(traj.getInterpolated(2.75).index_ceil(), 3); + + Trajectory.IndexView index_view = traj.getIndexView(); + Assert.assertEquals( + kWaypoints.get(0).interpolate(kWaypoints.get(1), .25), index_view.sample(0.25).state() + ); + Assert.assertEquals( + kWaypoints.get(1).interpolate(kWaypoints.get(2), .5), index_view.sample(1.5).state() + ); + Assert.assertEquals( + kWaypoints.get(2).interpolate(kWaypoints.get(3), .75), index_view.sample(2.75).state() + ); + Assert.assertEquals( + kHeadings.get(0).interpolate(kHeadings.get(1), .25), index_view.sample(0.25).heading() + ); + Assert.assertEquals( + kHeadings.get(1).interpolate(kHeadings.get(2), .5), index_view.sample(1.5).heading() + ); + Assert.assertEquals( + kHeadings.get(2).interpolate(kHeadings.get(3), .75), index_view.sample(2.75).heading() + ); + } +} diff --git a/src/test/java/com/team254/lib/trajectory/timing/TimedStateTest.java b/src/test/java/com/team254/lib/trajectory/timing/TimedStateTest.java new file mode 100755 index 0000000..79d6330 --- /dev/null +++ b/src/test/java/com/team254/lib/trajectory/timing/TimedStateTest.java @@ -0,0 +1,39 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.Pose2d; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.util.Util; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class TimedStateTest { + public static final double kTestEpsilon = Util.kEpsilon; + + @Test + public void test() { + // At (0,0,0), t=0, v=0, acceleration=1 + final TimedState start_state = + new TimedState<>(Pose2d.fromTranslation(new Translation2d(0.0, 0.0)), 0.0, 0.0, 1.0); + + // At (.5,0,0), t=1, v=1, acceleration=0 + final TimedState end_state = + new TimedState<>(Pose2d.fromTranslation(new Translation2d(0.5, 0.0)), 1.0, 1.0, 0.0); + + Assert.assertEquals(start_state, start_state.interpolate(end_state, 0.0)); + Assert.assertEquals(end_state, start_state.interpolate(end_state, 1.0)); + Assert.assertEquals(end_state, end_state.interpolate(start_state, 0.0)); + System.out.println(end_state.interpolate(start_state, 1.0)); + Assert.assertEquals(start_state, end_state.interpolate(start_state, 1.0)); + + final TimedState intermediate_state = start_state.interpolate(end_state, 0.5); + Assert.assertEquals(0.5, intermediate_state.t(), kTestEpsilon); + Assert.assertEquals( + start_state.acceleration(), intermediate_state.acceleration(), kTestEpsilon + ); + Assert.assertEquals(0.5, intermediate_state.velocity(), kTestEpsilon); + Assert.assertEquals(0.125, intermediate_state.state().getTranslation().x(), kTestEpsilon); + } +} diff --git a/src/test/java/com/team254/lib/trajectory/timing/TimingUtilTest.java b/src/test/java/com/team254/lib/trajectory/timing/TimingUtilTest.java new file mode 100755 index 0000000..79a4996 --- /dev/null +++ b/src/test/java/com/team254/lib/trajectory/timing/TimingUtilTest.java @@ -0,0 +1,184 @@ +package com.team254.lib.trajectory.timing; + +import com.team254.lib.geometry.ITranslation2d; +import com.team254.lib.geometry.Rotation2d254; +import com.team254.lib.geometry.State; +import com.team254.lib.geometry.Translation2d; +import com.team254.lib.trajectory.DistanceView; +import com.team254.lib.trajectory.Trajectory; +import com.team254.lib.trajectory.timing.TimingConstraint.MinMaxAcceleration; +import com.team254.lib.util.Util; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class TimingUtilTest { + public static final double kTestEpsilon = Util.kEpsilon; + + public static final List kWaypoints = Arrays.asList( + new Translation2d(0.0, 0.0), + new Translation2d(24.0, 0.0), + new Translation2d(36.0, 12.0), + new Translation2d(60.0, 12.0) + ); + + public static final List kHeadings = List.of( + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(0), + Rotation2d254.fromDegrees(0) + ); + + public , T extends State> Trajectory, TimedState> + buildAndCheckTrajectory( + final DistanceView dist_view, + double step_size, + List> constraints, + double start_vel, + double end_vel, + double max_vel, + double max_acc + ) { + Trajectory, TimedState> timed_traj = TimingUtil.timeParameterizeTrajectory( + false, dist_view, step_size, constraints, start_vel, end_vel, max_vel, max_acc + ); + checkTrajectory(timed_traj, constraints, start_vel, end_vel, max_vel, max_acc); + return timed_traj; + } + + public , T extends State> void checkTrajectory( + final Trajectory, TimedState> traj, + List> constraints, + double start_vel, + double end_vel, + double max_vel, + double max_acc + ) { + Assert.assertFalse(traj.isEmpty()); + Assert.assertEquals(traj.getPoint(0).state().velocity(), start_vel, kTestEpsilon); + Assert.assertEquals(traj.getPoint(traj.length() - 1).state().velocity(), end_vel, kTestEpsilon); + + // Go state by state, verifying all constraints are satisfied and integration is correct. + for (int i = 0; i < traj.length(); ++i) { + final TimedState state = traj.getPoint(i).state(); + for (final TimingConstraint constraint : constraints) { + Assert.assertTrue( + state.velocity() - kTestEpsilon <= constraint.getMaxVelocity(state.state()) + ); + final MinMaxAcceleration accel_limits = + constraint.getMinMaxAcceleration(state.state(), state.velocity()); + Assert.assertTrue(state.acceleration() - kTestEpsilon <= accel_limits.max_acceleration()); + Assert.assertTrue(state.acceleration() + kTestEpsilon >= accel_limits.min_acceleration()); + } + if (i > 0) { + final TimedState prev_state = traj.getPoint(i - 1).state(); + Assert.assertEquals( + state.velocity(), + prev_state.velocity() + (state.t() - prev_state.t()) * prev_state.acceleration(), + kTestEpsilon + ); + } + } + } + + @Test + public void testNoConstraints() { + Trajectory traj = new Trajectory<>(kWaypoints, kHeadings); + DistanceView dist_view = new DistanceView<>(traj); + + // Triangle profile. + Trajectory, TimedState> timed_traj = + buildAndCheckTrajectory( + dist_view, 1.0, new ArrayList>(), 0.0, 0.0, 20.0, 5.0 + ); + System.out.println(timed_traj.toCSV()); + + // Trapezoidal profile. + timed_traj = buildAndCheckTrajectory( + dist_view, 1.0, new ArrayList>(), 0.0, 0.0, 10.0, 5.0 + ); + System.out.println(timed_traj.toCSV()); + + // Trapezoidal profile with start and end velocities. + timed_traj = buildAndCheckTrajectory( + dist_view, 1.0, new ArrayList>(), 5.0, 2.0, 10.0, 5.0 + ); + System.out.println(timed_traj.toCSV()); + } + + @Test + public void testConditionalVelocityConstraint() { + Trajectory traj = new Trajectory<>(kWaypoints, kHeadings); + DistanceView dist_view = new DistanceView<>(traj); + + class ConditionalTimingConstraint> implements TimingConstraint { + @Override + public double getMaxVelocity(S state) { + if (state.getTranslation().x() >= 24.0) { + return 5.0; + } else { + return Double.POSITIVE_INFINITY; + } + } + + @Override + public TimingConstraint.MinMaxAcceleration getMinMaxAcceleration(S state, double velocity) { + return new TimingConstraint.MinMaxAcceleration( + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY + ); + } + } + + // Trapezoidal profile. + Trajectory, TimedState> timed_traj = + buildAndCheckTrajectory( + dist_view, 1.0, Arrays.asList(new ConditionalTimingConstraint<>()), 0.0, 0.0, 10.0, 5.0 + ); + System.out.println(timed_traj.toCSV()); + } + + @Test + public void testConditionalAccelerationConstraint() { + Trajectory traj = new Trajectory<>(kWaypoints, kHeadings); + DistanceView dist_view = new DistanceView<>(traj); + + class ConditionalTimingConstraint> implements TimingConstraint { + @Override + public double getMaxVelocity(S state) { + return Double.POSITIVE_INFINITY; + } + + @Override + public TimingConstraint.MinMaxAcceleration getMinMaxAcceleration(S state, double velocity) { + return new TimingConstraint.MinMaxAcceleration(-10.0, 10.0 / velocity); + } + } + + // Trapezoidal profile. + Trajectory, TimedState> timed_traj = + buildAndCheckTrajectory( + dist_view, 1.0, Arrays.asList(new ConditionalTimingConstraint<>()), 0.0, 0.0, 10.0, 5.0 + ); + System.out.println(timed_traj.toCSV()); + } + + @Test + public void testVelocityLimitRegionConstraint() { + Trajectory traj = new Trajectory<>(kWaypoints, kHeadings); + DistanceView dist_view = new DistanceView<>(traj); + + VelocityLimitRegionConstraint constraint = new VelocityLimitRegionConstraint<>( + new Translation2d(6.0, -6.0), new Translation2d(18.0, 6.0), 3.0 + ); + + // Trapezoidal profile. + Trajectory, TimedState> timed_traj = + buildAndCheckTrajectory(dist_view, 1.0, Arrays.asList(constraint), 0.0, 0.0, 10.0, 5.0); + System.out.println(timed_traj.toCSV()); + } +} diff --git a/src/test/java/com/team254/lib/util/DeadbandTest.java b/src/test/java/com/team254/lib/util/DeadbandTest.java new file mode 100644 index 0000000..059b77e --- /dev/null +++ b/src/test/java/com/team254/lib/util/DeadbandTest.java @@ -0,0 +1,19 @@ +package com.team254.lib.util; + +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class DeadbandTest { + private final double kEpsilon = 1e-5; + + @Test + public void testInDeadband() { + Assert.assertEquals(Util.handleDeadband(1, .5), 1, kEpsilon); + Assert.assertEquals(Util.handleDeadband(.5, .1), 0.4444444444, kEpsilon); + Assert.assertEquals(Util.handleDeadband(.1, .2), 0, kEpsilon); + Assert.assertEquals(Util.handleDeadband(.8, .2), 0.75, kEpsilon); + } +} diff --git a/src/test/java/com/team254/lib/util/PolynomialRegressionTest.java b/src/test/java/com/team254/lib/util/PolynomialRegressionTest.java new file mode 100644 index 0000000..e5ce600 --- /dev/null +++ b/src/test/java/com/team254/lib/util/PolynomialRegressionTest.java @@ -0,0 +1,31 @@ +package com.team254.lib.util; + +import static org.junit.Assert.*; + +import com.team254.lib.Constants; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +@RunWith(JUnit4.class) +public class PolynomialRegressionTest { + public static final double kTestEpsilon = 1E-9; + + @Test + public void testPolynomialRegression() { + double[] x = {0, 1, 2, 3, 4, 5}; + double[] y = {0, 2, 4, 6, 8, 10}; + PolynomialRegression regression = new PolynomialRegression(x, y, 1); + + assertEquals(regression.degree(), 1); + assertEquals(regression.beta(1), 2.0, kTestEpsilon); + assertEquals(regression.beta(0), 0.0, kTestEpsilon); + assertEquals(regression.predict(2.5), 5.0, kTestEpsilon); + + regression = Constants.kHoodRegression; + System.out.println("Hood: " + regression); + + regression = Constants.kRPMRegression; + System.out.println("RPM: " + regression); + } +} diff --git a/src/test/java/com/team254/lib/util/StickyBooleanTest.java b/src/test/java/com/team254/lib/util/StickyBooleanTest.java new file mode 100644 index 0000000..44cee0f --- /dev/null +++ b/src/test/java/com/team254/lib/util/StickyBooleanTest.java @@ -0,0 +1,24 @@ +package com.team254.lib.util; + +import org.junit.Assert; +import org.junit.Test; + +public class StickyBooleanTest { + @Test + public void testLatches() { + StickyBoolean b = new StickyBoolean(); + Assert.assertFalse(b.update(false)); + Assert.assertTrue(b.update(true)); + Assert.assertTrue(b.update(false)); + Assert.assertTrue(b.get()); + } + + @Test + public void testReset() { + StickyBoolean b = new StickyBoolean(); + Assert.assertTrue(b.update(true)); + b.reset(); + Assert.assertFalse(b.get()); + Assert.assertFalse(b.update(false)); + } +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..8e61586 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,35 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2023.4.4", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2023.4.4" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2023.4.4", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PhoenixProAnd5.json b/vendordeps/PhoenixProAnd5.json new file mode 100644 index 0000000..21206b0 --- /dev/null +++ b/vendordeps/PhoenixProAnd5.json @@ -0,0 +1,458 @@ +{ + "fileName": "PhoenixProAnd5.json", + "name": "CTRE-Phoenix (Pro And v5)", + "version": "23.0.12", + "frcYear": 2023, + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenixpro/PhoenixProAnd5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.4" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.4" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "wpiapi-java", + "version": "23.0.12" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.12", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.4", + "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.30.4", + "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.30.4", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.12", + "libName": "CTRE_PhoenixTools", + "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.30.4", + "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.30.4", + "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.30.4", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.12", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.12", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.12", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.12", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.12", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.12", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.12", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.12", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.12", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "wpiapi-cpp", + "version": "23.0.12", + "libName": "CTRE_PhoenixPro_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "23.0.12", + "libName": "CTRE_PhoenixPro_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..bd535bf --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "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 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "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": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file