From 12a0e0df010efd6d0d85158db56f46c6991d0081 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Tue, 26 Dec 2023 16:17:14 +0000 Subject: [PATCH] [ci, formatting] add wpiformat Also temporarily deletes swerve drive files as when commented out they generate warnings --- .clang-format | 256 ++++++++++++++ .editorconfig | 1 - .gitattributes | 7 + .github/ISSUE_TEMPLATE/bug_report.md | 1 - .github/ISSUE_TEMPLATE/feature_request.md | 1 - .github/PULL_REQUEST_TEMPLATE.md | 5 +- .github/workflows/ci.yml | 1 - .github/workflows/format.yml | 42 +++ .github/workflows/release.yml | 1 - .styleguide | 17 + .styleguide-license | 3 + .wpilib/wpilib_preferences.json | 2 +- CODE_OF_CONDUCT.md | 1 - CONTRIBUTING.md | 14 +- LICENSE.md | 2 +- README.md | 3 +- build.gradle | 10 - init | 1 - init.ps1 | 1 - settings.gradle | 1 - src/main/cpp/Main.cpp | 5 +- src/main/cpp/Robot.cpp | 4 + src/main/deploy/example.txt | 2 +- src/main/include/Robot.h | 5 +- src/test/cpp/main.cpp | 4 + wombat/.gitignore | 1 - wombat/build.gradle | 4 +- wombat/example/cpp/Example.cpp | 59 ++-- .../cpp/behaviour/ExampleBehaviours.cpp | 8 +- wombat/example/include/example/BEHAVIOURS.md | 30 +- .../example/include/example/BestPractices.md | 1 - wombat/example/include/example/Example.h | 99 +++--- .../example/behaviour/ExampleBehaviours.h | 13 +- wombat/settings.gradle | 1 + wombat/src/main/cpp/behaviour/Behaviour.cpp | 83 +++-- .../main/cpp/behaviour/BehaviourScheduler.cpp | 27 +- .../src/main/cpp/behaviour/HasBehaviour.cpp | 6 +- wombat/src/main/cpp/drivetrain/Drivetrain.cpp | 26 +- .../src/main/cpp/drivetrain/SwerveDrive.cpp | 37 -- wombat/src/main/cpp/subsystems/Arm.cpp | 127 ++++--- wombat/src/main/cpp/subsystems/Elevator.cpp | 100 +++--- wombat/src/main/cpp/subsystems/Shooter.cpp | 59 ++-- wombat/src/main/cpp/utils/Encoder.cpp | 55 +-- wombat/src/main/cpp/utils/RobotStartup.cpp | 6 +- wombat/src/main/cpp/utils/Util.cpp | 18 +- .../src/main/cpp/utils/VoltageController.cpp | 9 +- wombat/src/main/include/behaviour/Behaviour.h | 88 ++--- .../include/behaviour/BehaviourScheduler.h | 17 +- .../src/main/include/behaviour/Behaviours.md | 29 +- .../src/main/include/behaviour/HasBehaviour.h | 11 +- .../src/main/include/drivetrain/Drivetrain.h | 93 ++--- .../src/main/include/drivetrain/SwerveDrive.h | 38 -- wombat/src/main/include/subsystems/Arm.h | 135 ++++---- wombat/src/main/include/subsystems/Elevator.h | 146 ++++---- wombat/src/main/include/subsystems/Shooter.h | 126 +++---- wombat/src/main/include/utils/Encoder.h | 218 ++++++------ wombat/src/main/include/utils/Gearbox.h | 20 +- wombat/src/main/include/utils/PID.h | 327 ++++++++++-------- wombat/src/main/include/utils/RobotStartup.h | 21 +- wombat/src/main/include/utils/Util.h | 139 ++++---- .../main/include/utils/VoltageController.h | 110 +++--- wombat/src/test/cpp/main.cpp | 4 + wombat/src/test/cpp/test_behaviours.cpp | 60 ++-- wombat/vendordeps/Phoenix.json | 2 +- wombat/vendordeps/REVLib.json | 2 +- 65 files changed, 1634 insertions(+), 1111 deletions(-) create mode 100644 .clang-format create mode 100644 .github/workflows/format.yml create mode 100644 .styleguide create mode 100644 .styleguide-license delete mode 100644 wombat/src/main/cpp/drivetrain/SwerveDrive.cpp delete mode 100644 wombat/src/main/include/drivetrain/SwerveDrive.h diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..f69bef0c --- /dev/null +++ b/.clang-format @@ -0,0 +1,256 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + PadOperators: false +AlignEscapedNewlines: Left +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterExternBlock: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAfterAttributes: Always +BreakAfterJavaFieldAnnotations: false +BreakArrays: true +BreakBeforeBinaryOperators: None +BreakBeforeConceptDeclarations: Always +BreakBeforeBraces: Attach +BreakBeforeInlineASMColon: OnlyMultiline +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakInheritanceList: BeforeColon +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +ExperimentalAutoDetectBinPacking: false +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 +IndentCaseBlocks: false +IndentCaseLabels: true +IndentExternBlock: AfterExternBlock +IndentGotoLabels: true +IndentPPDirectives: None +IndentRequiresClause: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PackConstructorInitializers: NextLine +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +PPIndentWidth: -1 +QualifierAlignment: Leave +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 +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SortIncludes: false +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default +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 +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: c++20 +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseTab: Never +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE +... diff --git a/.editorconfig b/.editorconfig index 8d762fbc..0020fc03 100644 --- a/.editorconfig +++ b/.editorconfig @@ -3,4 +3,3 @@ root = true [*] indent_style = space indent_size = 2 - diff --git a/.gitattributes b/.gitattributes index e69de29b..416557ac 100644 --- a/.gitattributes +++ b/.gitattributes @@ -0,0 +1,7 @@ +* text=auto +*.sh text eol=lf +*.gradle text eol=lf +*.java text eol=lf +*.json text eol=lf +*.md text eol=lf +*.xml text eol=lf diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 3238c6e6..74514987 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -22,4 +22,3 @@ A clear and concise description of what you expected to happen. **Additional context** Add any other context about the problem here. - diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md index f3a9fe72..bbcbbe7d 100644 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -18,4 +18,3 @@ A clear and concise description of any alternative solutions or features you've **Additional context** Add any other context or screenshots about the feature request here. - diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 50f3ba76..eb2cc424 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,8 +1,7 @@ -**What this PR does** +**What this PR does** -**What does it fix/add?** +**What does it fix/add?** _Reference issues with "Fixes #10" or "Closes #10", where #10 is the number assigned to the issue_ **What does this depend on?** _References pull requests or branches/forks that this pull request depends on_ - diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5ab2f38d..833cae46 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -36,4 +36,3 @@ jobs: # Runs a single command using the runners shell - name: Compile and run tests on robot code run: ./gradlew build - diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 00000000..dbdccf6d --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,42 @@ +name: Lint and Format + +on: + pull_request: + push: + +jobs: + name: "wpiformat" + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + - name: Fetch all history and metadata + run: | + git checkout -b pr + git branch -f main origin/main + - name: Set up Python 3.8 + uses: actions/setup-python@v4 + with: + python-version: 3.8 + - name: Install wpiformat + run: pip3 install wpiformat==2023.36 + - name: Run + run: wpiformat + - name: Check output + run: git --no-pager diff --exit-code HEAD + - name: Generate diff + run: git diff HEAD > wpiformat-fixes.patch + if: ${{ failure() }} + - uses: actions/upload-artifact@v4 + with: + name: wpiformat fixes + path: wpiformat-fixes.patch + if: ${{ failure() }} + - name: Write to job summary + run: | + echo '```diff' >> $GITHUB_STEP_SUMMARY + cat wpiformat-fixes.patch >> $GITHUB_STEP_SUMMARY + echo '' >> $GITHUB_STEP_SUMMARY + echo '```' >> $GITHUB_STEP_SUMMARY + if: ${{ failure() }} diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 289b91d5..94a87fc5 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -25,4 +25,3 @@ jobs: The latest release of our 2024 build season code! draft: false prerelease: false - diff --git a/.styleguide b/.styleguide new file mode 100644 index 00000000..82efdeab --- /dev/null +++ b/.styleguide @@ -0,0 +1,17 @@ +cppHeaderFileInclude { + \.h$ + \.hpp$ + \.inc$ + \.inl$ +} + +cppSrcFileInclude { + \.cpp$ +} + +modifiableFileExclude { + gradle/ + compile_commands.json + gradlew + gradlew.bat +} diff --git a/.styleguide-license b/.styleguide-license new file mode 100644 index 00000000..7f0ba363 --- /dev/null +++ b/.styleguide-license @@ -0,0 +1,3 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index c58fdd5b..2f66de4c 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -3,4 +3,4 @@ "currentLanguage": "cpp", "projectYear": "2023", "teamNumber": 4788 -} \ No newline at end of file +} diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md index 84135e69..6bf83810 100644 --- a/CODE_OF_CONDUCT.md +++ b/CODE_OF_CONDUCT.md @@ -111,4 +111,3 @@ individual, or aggression toward or disparagement of classes of individuals. **Consequence**: A permanent ban from any sort of public interaction within the community. - diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 026752f0..d86f98f1 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,11 +1,11 @@ -# Contributing to our 2024 Codebase. -Our 2024 Codebase is split up into two main projects, our robot code and our library Wombat. This will only focus on our robot code but if you our on programming team it is recomended that you also read the contributing guide for Wombat. This guide assumes you have already read the README.md. +# Contributing to our 2024 Codebase. +Our 2024 Codebase is split up into two main projects, our robot code and our library Wombat. This will only focus on our robot code but if you our on programming team it is recomended that you also read the contributing guide for Wombat. This guide assumes you have already read the README.md. ## Git -Whenever you start on something new create a new branch from your master branch. The branch name should describe what it aims to achieve. When you are done you should open a pull request to CurtinFRC master. +Whenever you start on something new create a new branch from your master branch. The branch name should describe what it aims to achieve. When you are done you should open a pull request to CurtinFRC master. ## Git History -Try and keep git history clean. Commit messages should be *descriptive*. +Try and keep git history clean. Commit messages should be *descriptive*. An example of good Git usage would be as follows : ```bash git add . @@ -14,10 +14,10 @@ git push _remotename_ _branchname_ eg origin intake-fix ``` ## Creating Issues -Issues should be created to report bugs or request features. Use the appropriate template for your issue and make sure to apply the appropriate labels. If you are working on an issue assign yourself to the issue. +Issues should be created to report bugs or request features. Use the appropriate template for your issue and make sure to apply the appropriate labels. If you are working on an issue assign yourself to the issue. ## Creating Pull Requests -Pull requests should be created to resolve issues. Reference the issue you are resolving in your pull request. Pull requests are required to pass continuous integration checks that run automatically, follow formatting rules as per Wombat documentation. Request a review from programming captain. +Pull requests should be created to resolve issues. Reference the issue you are resolving in your pull request. Pull requests are required to pass continuous integration checks that run automatically, follow formatting rules as per Wombat documentation. Request a review from programming captain. ## Wombat Contributions -Wombat contributions are expected to follow the guidelines set out in the contributing guide for Wombat. All Wombat changes during build season will be periodically reviewed and merged into a seperate branch of Wombat during build season. Outside of build season it will be less periodically merged in but will be directly merged to master. As Wombat is a 4788 published library there are stricter standards. +Wombat contributions are expected to follow the guidelines set out in the contributing guide for Wombat. All Wombat changes during build season will be periodically reviewed and merged into a seperate branch of Wombat during build season. Outside of build season it will be less periodically merged in but will be directly merged to master. As Wombat is a 4788 published library there are stricter standards. diff --git a/LICENSE.md b/LICENSE.md index 4e8604fe..1e3eb41e 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -18,4 +18,4 @@ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. \ No newline at end of file +SOFTWARE. diff --git a/README.md b/README.md index 2216272f..c5d24653 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,7 @@ These commands can be used in a variety of combinations, feel free to experiment ## Build `./gradlew build` -Build will compile and get the code ready without deploying it. It will also run all automated tests, which is great for testing your code before it ever gets run on a robot (which also means you can build whenever). +Build will compile and get the code ready without deploying it. It will also run all automated tests, which is great for testing your code before it ever gets run on a robot (which also means you can build whenever). `./gradlew :Wombat:build` Will compile and build the Wombat library. Also runs all of Wombat's inbuilt tests. @@ -41,4 +41,3 @@ Deploying will build your code (as above), and deploy it to the robot. You have ## Clean `./gradlew clean` Cleaning removes caches of your compiled code. If you do not understand an error it can often help to clean before getting help. Clean building is slower so you should not generally use it. - diff --git a/build.gradle b/build.gradle index 98cbab30..349a9371 100644 --- a/build.gradle +++ b/build.gradle @@ -2,15 +2,6 @@ plugins { id "cpp" id "google-test-test-suite" id "edu.wpi.first.GradleRIO" version "2023.4.3" - id 'org.ec4j.editorconfig' version '0.0.3' -} - -editorconfig { - - includes = ['src/**', 'build.gradle', 'settings.gradle'] - - excludes = ['gradlew', 'LICENSE'] - } // Define my targets (RoboRIO) and artifacts (deployable files) @@ -118,4 +109,3 @@ model { subprojects { project.buildDir = rootProject.buildDir } - diff --git a/init b/init index 319a1947..74dc5a0f 100755 --- a/init +++ b/init @@ -4,4 +4,3 @@ sudo apt install -y openjdk-11-jre-headless chmod +x gradlew ./gradlew installRoborioToolchain ./gradlew build - diff --git a/init.ps1 b/init.ps1 index 8a05f31f..fa83b9a6 100644 --- a/init.ps1 +++ b/init.ps1 @@ -1,3 +1,2 @@ ./gradlew installRoborioToolchain ./gradlew build - diff --git a/settings.gradle b/settings.gradle index e0509975..57804a08 100644 --- a/settings.gradle +++ b/settings.gradle @@ -28,4 +28,3 @@ pluginManagement { include 'wombat' rootProject.name = '2024-Crescendo' - diff --git a/src/main/cpp/Main.cpp b/src/main/cpp/Main.cpp index c6df2c7f..3c3bfe12 100644 --- a/src/main/cpp/Main.cpp +++ b/src/main/cpp/Main.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + /* Dont edit this file Go to Robot.cpp @@ -7,4 +11,3 @@ #include "utils/RobotStartup.h" WOMBAT_ROBOT_MAIN(Robot); - diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 8a402a6a..4773a817 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "Robot.h" void Robot::RobotInit() {} diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt index 68395391..15bc5c1e 100644 --- a/src/main/deploy/example.txt +++ b/src/main/deploy/example.txt @@ -1,4 +1,4 @@ Files placed in this directory will be deployed to the RoboRIO into the 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy - directory. \ No newline at end of file + directory. diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 0e40ed87..35e5de35 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include @@ -16,5 +20,4 @@ class Robot : public frc::TimedRobot { void TestPeriodic() override; private: - }; diff --git a/src/test/cpp/main.cpp b/src/test/cpp/main.cpp index b8b23d23..c7052d44 100644 --- a/src/test/cpp/main.cpp +++ b/src/test/cpp/main.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include #include "gtest/gtest.h" diff --git a/wombat/.gitignore b/wombat/.gitignore index d92702af..5fbba068 100644 --- a/wombat/.gitignore +++ b/wombat/.gitignore @@ -3,4 +3,3 @@ .gradle *.exe *.out - diff --git a/wombat/build.gradle b/wombat/build.gradle index 97e79785..a9b57267 100644 --- a/wombat/build.gradle +++ b/wombat/build.gradle @@ -8,7 +8,7 @@ model { components { Wombat(NativeLibrarySpec) { targetPlatform NativePlatforms.desktop - targetPlatform NativePlatforms.roborio + targetPlatform NativePlatforms.roborio sources { sources.cpp { @@ -20,7 +20,7 @@ model { exportedHeaders { srcDir 'src/main/include' } - } + } } binaries.all { diff --git a/wombat/example/cpp/Example.cpp b/wombat/example/cpp/Example.cpp index 9acc0e2a..ebdd7f80 100644 --- a/wombat/example/cpp/Example.cpp +++ b/wombat/example/cpp/Example.cpp @@ -1,44 +1,59 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + // This should be the only include in your subsystem files #include "example/Example.h" // here is the declaration for the example class's constructor // first we have our function declaration again as done before // then we have the initializer which is entered using a colon -// the initializer takes a variable which should then have brackets added to the end of it as if it was a function and then assigns that variable -// the value or the parameter placed inside the brackets -// we see this done here with the most common usage assigning _config the subsystem's configuration -// you can also add more logic as you would inside a normal function but that is not normally necessary - -Example::Example(ExampleConfig *_config, frc::XboxController &_driver): _config(_config), _driver(_driver) {} - -// here we have our set state function. inside of the logic all we need to do for a setter is take the variable we want and assign it the -// parameters value so they are usually one line functions - -void Example::SetState(ExampleState state) { _state = state; } +// the initializer takes a variable which should then have brackets added to the +// end of it as if it was a function and then assigns that variable the value or +// the parameter placed inside the brackets we see this done here with the most +// common usage assigning _config the subsystem's configuration you can also add +// more logic as you would inside a normal function but that is not normally +// necessary + +Example::Example(ExampleConfig* _config, frc::XboxController& _driver) + : _config(_config), _driver(_driver) {} + +// here we have our set state function. inside of the logic all we need to do +// for a setter is take the variable we want and assign it the parameters value +// so they are usually one line functions + +void Example::SetState(ExampleState state) { + _state = state; +} -// here we have our get config. inside of the logic all we need to do is return our config variable making getters also quite simple and usually -// one liners +// here we have our get config. inside of the logic all we need to do is return +// our config variable making getters also quite simple and usually one liners -ExampleConfig *Example::GetConfig() { return _config; } +ExampleConfig* Example::GetConfig() { + return _config; +} -// here we have our on start function. all this on start function does is print starting to the console when the robot starts but you will often -// also have to add zeroing and other features +// here we have our on start function. all this on start function does is print +// starting to the console when the robot starts but you will often also have to +// add zeroing and other features void Example::OnStart() { std::cout << "starting" << std::endl; } -// here we have our on update function. this function is relatively simple in this example but in most subsystems will be much more complicated. -// all we are doing here switching between states based on _state and performing certain logic based on what state we are in. in kIdle we do nothing -// the point of the kIdle state. in kRunning we use a ternary operator to change the speed of our motor based on the y value of the left joystick -// on our controller +// here we have our on update function. this function is relatively simple in +// this example but in most subsystems will be much more complicated. all we are +// doing here switching between states based on _state and performing certain +// logic based on what state we are in. in kIdle we do nothing the point of the +// kIdle state. in kRunning we use a ternary operator to change the speed of our +// motor based on the y value of the left joystick on our controller void Example::OnUpdate(units::second_t dt) { - switch(_state) { + switch (_state) { case ExampleState::kIdle: break; case ExampleState::kRunning: - double speed = (fabs(_driver.GetLeftY()) > 0.15) ? speed : 0; + double speed = (std::fabs(_driver.GetLeftY()) > 0.15) ? speed : 0; _config->leftGearbox.transmission->SetVoltage(speed * 1_V); break; } diff --git a/wombat/example/cpp/behaviour/ExampleBehaviours.cpp b/wombat/example/cpp/behaviour/ExampleBehaviours.cpp index 46efc211..bd2b201e 100644 --- a/wombat/example/cpp/behaviour/ExampleBehaviours.cpp +++ b/wombat/example/cpp/behaviour/ExampleBehaviours.cpp @@ -1,7 +1,11 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + // this should be your only include in your behaviour cpp files #include "example/behaviour/ExampleBehaviours.h" -ExampleBehaviour::ExampleBehaviour(Example *_example): _example(_example) { +ExampleBehaviour::ExampleBehaviour(Example* _example) : _example(_example) { Controls(_example); } @@ -13,4 +17,4 @@ void ExampleBehaviour::OnStart() { void ExampleBehaviour::OnTick(units::second_t dt) { _example->SetState(ExampleState::kRunning); // _example->OnUpdate(dt); -} \ No newline at end of file +} diff --git a/wombat/example/include/example/BEHAVIOURS.md b/wombat/example/include/example/BEHAVIOURS.md index 58f173a4..a7a557fd 100644 --- a/wombat/example/include/example/BEHAVIOURS.md +++ b/wombat/example/include/example/BEHAVIOURS.md @@ -70,7 +70,7 @@ BehaviourScheduler::GetInstance()->Schedule(behaviour); ``` ### Controlling systems -Let's look at how we might control a system with Behaviours. For this example, we're going to create a Behaviour that tells a flywheel shooter to spin up to a certain speed. +Let's look at how we might control a system with Behaviours. For this example, we're going to create a Behaviour that tells a flywheel shooter to spin up to a certain speed. First, we need to make our shooter system able to have a Behaviour. To do this, we make it implement from `HasBehaviour` - that's it, no methods to override or anything else. @@ -128,8 +128,8 @@ ShooterSpinup::ShooterSpinup(Shooter &s, units::rad_per_s speed, bool hold) : _s void ShooterSpinup::OnTick(units::time::second_t dt) { // Get the current speed from the shooter's encoder units::rad_per_s current_speed = _shooter.gearbox.encoder.GetAngularVelocity(); - // Calculate the feedforward voltage - the voltage required to spin the - // motor assuming there is no torque (load) applied. This is simple if + // Calculate the feedforward voltage - the voltage required to spin the + // motor assuming there is no torque (load) applied. This is simple if // we use WPILib's DcMotor class from wpimath (#include ). units::volt_t feed_forward = _shooter.gearbox.motor.Voltage(0, _speed); @@ -139,7 +139,7 @@ void ShooterSpinup::OnTick(units::time::second_t dt) { // If the PID says we're done, stop this behaviour and move on! // Note "hold", which will keep the behaviour running even after we meet our speed - // We can use this in conjunction with ->Until(behaviour), which will keep our + // We can use this in conjunction with ->Until(behaviour), which will keep our // behaviour running until another behaviour is finished. if (_pid.IsDone() && !_hold) SetDone(); @@ -163,10 +163,10 @@ spinup->SetPeriod(10_ms); // 100Hz ``` ## Using Behaviours Together -As we mentioned, Behaviours are small, compartmentalised units of work that we can use together to make complex routines. In order to achieve this, Wombat provides some ways to combine behaviours together into larger sequences. +As we mentioned, Behaviours are small, compartmentalised units of work that we can use together to make complex routines. In order to achieve this, Wombat provides some ways to combine behaviours together into larger sequences. ### Sequential Execution -Behaviours can be executed in sequence by using the `<<` operator. +Behaviours can be executed in sequence by using the `<<` operator. ```cpp auto combined = make() @@ -192,7 +192,7 @@ auto wait_until = make() ``` ### Waiting -Wombat provides `WaitTime` and `WaitFor` to produce simple waits in the behaviour chain. +Wombat provides `WaitTime` and `WaitFor` to produce simple waits in the behaviour chain. ```cpp auto wait_2s = make(2_s); @@ -213,7 +213,7 @@ auto if_bhvr = make([&vision]() { return vision.ready(); }) ->Else(make()); ``` -Switch is similar to a switch-case statement, allowing you to choose from one of many options. +Switch is similar to a switch-case statement, allowing you to choose from one of many options. ```cpp auto switch_bhvr = make(my_int) @@ -224,7 +224,7 @@ auto switch_bhvr = make(my_int) ->When([](auto my_int) { return my_int > 6; }, make()) ->Otherwise(make()); -// If no When matches, and Otherwise is not provided, the behaviour will +// If no When matches, and Otherwise is not provided, the behaviour will // keep running until one of the options matches. You can also provide // Otherwise without an argument to exit if none match. @@ -251,7 +251,7 @@ Let's say we come up with a plan to do a really awesome (but really complicated) - Wait 2 seconds - Move forward 1.5m - Intake a ball -- While driving backwards 3m: +- While driving backwards 3m: - Spinup the shooter - Wait until vision is ready - Shoot a ball @@ -260,13 +260,13 @@ Let's say we come up with a plan to do a really awesome (but really complicated) Complex, right? Let's look at how we break it down. First of all, notice that it's already in a sequence of steps for us - small chunks of work that we can harness to complete our goals. Also notice that there's some common behaviour across this routine - there's multiple times where we move forward, intake a ball, etc. We can use this to our advantage by reducing the amount of code we need to write. -We can use the steps we've already outlined to build our overall behaviour sequence that describes the autonomous routine. +We can use the steps we've already outlined to build our overall behaviour sequence that describes the autonomous routine. Let's go ahead and mock up what we think our autonomous routine above will look like in code: ```cpp Behaviour::ptr MyAutoRoutine() { return ( - make(shooter, 500_rpm, true) + make(shooter, 500_rpm, true) ->Until( make(drivetrain, 2_m) << make(intake) @@ -277,11 +277,11 @@ Behaviour::ptr MyAutoRoutine() { << make(drivetrain, 1.5_m) << make(intake) << ( - // Drive Straight backwards, and + // Drive Straight backwards, and make(drivetrain, -3_m) & ( // Spinup the shooter until vision is ready, then fire. - make(shooter, 500_rpm, true) + make(shooter, 500_rpm, true) ->Until(make([vision]() { return vision.ready(); })) << make(shooter) ) @@ -297,4 +297,4 @@ See how we can just flow on from the individual steps of our big, complex exampl - `ShooterFire` - `DriveStraight` - `DriveTurn` -- `IntakeOne` \ No newline at end of file +- `IntakeOne` diff --git a/wombat/example/include/example/BestPractices.md b/wombat/example/include/example/BestPractices.md index 84261325..874ca7f3 100644 --- a/wombat/example/include/example/BestPractices.md +++ b/wombat/example/include/example/BestPractices.md @@ -1,2 +1 @@ # Best Practices - diff --git a/wombat/example/include/example/Example.h b/wombat/example/include/example/Example.h index c3af1bc5..42c860d9 100644 --- a/wombat/example/include/example/Example.h +++ b/wombat/example/include/example/Example.h @@ -1,14 +1,20 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + // files should be named in upper camel case -// all .h or .hpp files should start with this as it causes the file to only compile its contents once at compile time +// all .h or .hpp files should start with this as it causes the file to only +// compile its contents once at compile time #pragma once -// your includes should be structured as shown below in your .h and .hpp files to increase readability -// Local includes, this means includes from the project you are working on eg 2023-ChargedUp +// your includes should be structured as shown below in your .h and .hpp files +// to increase readability Local includes, this means includes from the project +// you are working on eg 2023-ChargedUp // Wombat includes -#include "behaviour/HasBehaviour.h" #include "Gearbox.h" +#include "behaviour/HasBehaviour.h" // WPILIB includes @@ -29,9 +35,10 @@ #include // structs should use upper camel case in naming -// the first step in creating a subsystem is creating a configuration struct for it -// this struct should have all of the relevant motors, sensors, encoders and other required options for the code to work -// in this example the only config is a gearbox but configs can get much larger and more complicted +// the first step in creating a subsystem is creating a configuration struct for +// it this struct should have all of the relevant motors, sensors, encoders and +// other required options for the code to work in this example the only config +// is a gearbox but configs can get much larger and more complicted struct ExampleConfig { // all variables should be named using lower camel case @@ -41,10 +48,12 @@ struct ExampleConfig { // enums should use upper camel case naming // all of the states for your subsystem should be encapsulated using an enum -// all subsystems should have a kIdle state for when the subsystem is not running +// all subsystems should have a kIdle state for when the subsystem is not +// running enum class ExampleState { - // states should be named using a lowercase k then the state name in upper camel case + // states should be named using a lowercase k then the state name in upper + // camel case kIdle, kRunning @@ -53,63 +62,73 @@ enum class ExampleState { // classes should be named in upper camel case // all subsystems need a class which inherits from the has behaviour class -class Example: public behaviour::HasBehaviour { - // the public field has data which can be accessed from any instance of the class +class Example : public behaviour::HasBehaviour { + // the public field has data which can be accessed from any instance of the + // class public: - // all classes require a constructor which tells you what you need to provide to create an instance of the class - // constructors are declared as functions with no return type with the class name as the function name - // constructors take in parameters, can assign those parameters to variables and also perform any logic a normal function can - // all constructors should have a configuration variable passed in using a pointer as shown below and initialize that configuration to your - // _config variable as done is Example.cpp + // all classes require a constructor which tells you what you need to provide + // to create an instance of the class constructors are declared as functions + // with no return type with the class name as the function name constructors + // take in parameters, can assign those parameters to variables and also + // perform any logic a normal function can all constructors should have a + // configuration variable passed in using a pointer as shown below and + // initialize that configuration to your _config variable as done is + // Example.cpp - Example(ExampleConfig *_config, frc::XboxController &_driver); + Example(ExampleConfig* _config, frc::XboxController& _driver); // all classes require a deconstructor which in most cases can just be empty - // deconstructors are declared the same way as constructors but with a ~ at the front - // the deconstructor tells the class what to do when the instance is deleted + // deconstructors are declared the same way as constructors but with a ~ at + // the front the deconstructor tells the class what to do when the instance is + // deleted ~Example(); - // all functions are named in upper camel case and should be declared in the .h or .hpp file but given logic in the .cpp file - // all subsystems require a function which will instruct it what to do when it starts named OnStart - // in this function things like zeroing, logging or other tasks that are done on subsystem startup happen + // all functions are named in upper camel case and should be declared in the + // .h or .hpp file but given logic in the .cpp file all subsystems require a + // function which will instruct it what to do when it starts named OnStart in + // this function things like zeroing, logging or other tasks that are done on + // subsystem startup happen void OnStart(); - // all subsystems require a function which will instruct it what to do every tick (usually at 50 hertz but with a parameter to change frequency) - // in this function you usually will have a switch statement for your states which will dictate what your states do along with other things that - // happen during runtime eg logging + // all subsystems require a function which will instruct it what to do every + // tick (usually at 50 hertz but with a parameter to change frequency) in this + // function you usually will have a switch statement for your states which + // will dictate what your states do along with other things that happen during + // runtime eg logging void OnUpdate(units::second_t dt); - // getters are functions which get a piece of data allowing it to be accessed with an instance of the class - // all getters names should start with the word get - // a common getter all subsystems should have is GetConfig which allows you to access the config with instances of the class + // getters are functions which get a piece of data allowing it to be accessed + // with an instance of the class all getters names should start with the word + // get a common getter all subsystems should have is GetConfig which allows + // you to access the config with instances of the class - ExampleConfig *GetConfig(); + ExampleConfig* GetConfig(); // setters are functions which set a variables value based on input // all setters should start with the word set - // a common example all subsystems should have is SetState which allows you to set the state of a subsystem + // a common example all subsystems should have is SetState which allows you to + // set the state of a subsystem void SetState(ExampleState state); - // the protected field has data which can be accessed from within the class or within classes which inherit from it - // it is usually not used but occasionally used for things like PID configs + // the protected field has data which can be accessed from within the class or + // within classes which inherit from it it is usually not used but + // occasionally used for things like PID configs protected: - - // the private field has data which can be accessed only from within the class + // the private field has data which can be accessed only from within the class private: - - // all variables in the private field should start with an underscore + // all variables in the private field should start with an underscore // most subsystem private variables are config and state - // these variables will store the configuration and state of the subsystem and are vital to its functioning + // these variables will store the configuration and state of the subsystem and + // are vital to its functioning - ExampleConfig *_config; + ExampleConfig* _config; ExampleState _state; - frc::XboxController &_driver; - + frc::XboxController& _driver; }; diff --git a/wombat/example/include/example/behaviour/ExampleBehaviours.h b/wombat/example/include/example/behaviour/ExampleBehaviours.h index 48c5c943..8cf447be 100644 --- a/wombat/example/include/example/behaviour/ExampleBehaviours.h +++ b/wombat/example/include/example/behaviour/ExampleBehaviours.h @@ -1,12 +1,15 @@ -#pragma once +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project -#include "example/Example.h" +#pragma once #include "behaviour/Behaviour.h" +#include "example/Example.h" class ExampleBehaviour : public behaviour::Behaviour { public: - ExampleBehaviour(Example *_example); + explicit ExampleBehaviour(Example* _example); ~ExampleBehaviour(); // See Example.h for more information about OnStart functions @@ -15,5 +18,5 @@ class ExampleBehaviour : public behaviour::Behaviour { void OnTick(units::second_t dt); private: - Example *_example; -}; \ No newline at end of file + Example* _example; +}; diff --git a/wombat/settings.gradle b/wombat/settings.gradle index e69de29b..8b137891 100644 --- a/wombat/settings.gradle +++ b/wombat/settings.gradle @@ -0,0 +1 @@ + diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp index a3872ab6..2bbb679f 100644 --- a/wombat/src/main/cpp/behaviour/Behaviour.cpp +++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp @@ -1,12 +1,19 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "behaviour/Behaviour.h" using namespace behaviour; // Behaviour Behaviour::Behaviour(std::string name, units::time::second_t period) - : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {} + : _bhvr_name(name), + _bhvr_period(period), + _bhvr_state(BehaviourState::INITIALISED) {} Behaviour::~Behaviour() { - if (!IsFinished()) Interrupt(); + if (!IsFinished()) + Interrupt(); } std::string Behaviour::GetName() const { @@ -25,12 +32,14 @@ units::time::second_t Behaviour::GetRunTime() const { return _bhvr_timer; } -void Behaviour::Controls(HasBehaviour *sys) { - if (sys != nullptr) _bhvr_controls.insert(sys); +void Behaviour::Controls(HasBehaviour* sys) { + if (sys != nullptr) + _bhvr_controls.insert(sys); } -void Behaviour::Inherit(Behaviour &bhvr) { - for (auto c : bhvr.GetControlled()) Controls(c); +void Behaviour::Inherit(Behaviour& bhvr) { + for (auto c : bhvr.GetControlled()) + Controls(c); } Behaviour::ptr Behaviour::WithTimeout(units::time::second_t timeout) { @@ -38,7 +47,7 @@ Behaviour::ptr Behaviour::WithTimeout(units::time::second_t timeout) { return shared_from_this(); } -wpi::SmallPtrSetImpl &Behaviour::GetControlled() { +wpi::SmallPtrSetImpl& Behaviour::GetControlled() { return _bhvr_controls; } @@ -56,7 +65,7 @@ void Behaviour::SetDone() { bool Behaviour::Tick() { if (_bhvr_state == BehaviourState::INITIALISED) { - _bhvr_time = frc::RobotController::GetFPGATime(); + _bhvr_time = frc::RobotController::GetFPGATime(); _bhvr_state = BehaviourState::RUNNING; _bhvr_timer = 0_s; @@ -65,13 +74,14 @@ bool Behaviour::Tick() { if (_bhvr_state == BehaviourState::RUNNING) { uint64_t now = frc::RobotController::GetFPGATime(); - auto dt = static_cast(now - _bhvr_time) / 1000000 * 1_s; - _bhvr_time = now; + auto dt = static_cast(now - _bhvr_time) / 1000000 * 1_s; + _bhvr_time = now; _bhvr_timer += dt; if (dt > 2 * _bhvr_period) { std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value() - << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl; + << " Dt(deadline)=" << (2 * _bhvr_period).value() + << ". Bhvr: " << GetName() << std::endl; } if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout) @@ -88,11 +98,13 @@ bool Behaviour::IsRunning() const { } bool Behaviour::IsFinished() const { - return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING; + return _bhvr_state != BehaviourState::INITIALISED && + _bhvr_state != BehaviourState::RUNNING; } void Behaviour::Stop(BehaviourState new_state) { - if (_bhvr_state.exchange(new_state) == BehaviourState::RUNNING) OnStop(); + if (_bhvr_state.exchange(new_state) == BehaviourState::RUNNING) + OnStop(); } Behaviour::ptr Behaviour::Until(Behaviour::ptr other) { @@ -144,10 +156,11 @@ ConcurrentBehaviour::ConcurrentBehaviour(ConcurrentBehaviourReducer reducer) void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) { for (auto c : behaviour->GetControlled()) { - auto &controls = GetControlled(); + auto& controls = GetControlled(); if (controls.find(c) != controls.end()) { throw DuplicateControlException( - "Cannot run behaviours with the same controlled system concurrently (duplicate in: " + + "Cannot run behaviours with the same controlled system concurrently " + "(duplicate in: " + behaviour->GetName() + ")"); } Controls(c); @@ -158,8 +171,10 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) { } std::string ConcurrentBehaviour::GetName() const { - std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); - for (auto b : _children) msg += b->GetName() + ", "; + std::string msg = + (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); + for (auto b : _children) + msg += b->GetName() + ", "; msg += "}"; return msg; } @@ -173,10 +188,12 @@ void ConcurrentBehaviour::OnStart() { using namespace std::chrono_literals; b->Tick(); - std::this_thread::sleep_for(std::chrono::milliseconds((int64_t)(b->GetPeriod().value() * 1000))); + std::this_thread::sleep_for(std::chrono::milliseconds( + static_cast(b->GetPeriod().value() * 1000))); } - if (IsFinished() && !b->IsFinished()) b->Interrupt(); + if (IsFinished() && !b->IsFinished()) + b->Interrupt(); { std::lock_guard lk(_children_finished_mtx); @@ -205,11 +222,12 @@ void ConcurrentBehaviour::OnTick(units::time::second_t dt) { } } - if (ok) SetDone(); + if (ok) + SetDone(); } void ConcurrentBehaviour::OnStop() { - for (auto &t : _threads) { + for (auto& t : _threads) { t.join(); } } @@ -236,28 +254,35 @@ void If::OnStart() { void If::OnTick(units::time::second_t dt) { Behaviour::ptr _active = _value ? _then : _else; - if (_active) _active->Tick(); - if (!_active || _active->IsFinished()) SetDone(); + if (_active) + _active->Tick(); + if (!_active || _active->IsFinished()) + SetDone(); - if (IsFinished() && _active && !_active->IsFinished()) _active->Interrupt(); + if (IsFinished() && _active && !_active->IsFinished()) + _active->Interrupt(); } // WaitFor WaitFor::WaitFor(std::function predicate) : _predicate(predicate) {} void WaitFor::OnTick(units::time::second_t dt) { - if (_predicate()) SetDone(); + if (_predicate()) + SetDone(); } // WaitTime -WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {} -WaitTime::WaitTime(std::function time_fn) : _time_fn(time_fn) {} +WaitTime::WaitTime(units::time::second_t time) + : WaitTime([time]() { return time; }) {} +WaitTime::WaitTime(std::function time_fn) + : _time_fn(time_fn) {} void WaitTime::OnStart() { _time = _time_fn(); } void WaitTime::OnTick(units::time::second_t dt) { - if (GetRunTime() > _time) SetDone(); + if (GetRunTime() > _time) + SetDone(); } // Print @@ -265,4 +290,4 @@ Print::Print(std::string message) : _message(message) {} void Print::OnTick(units::time::second_t dt) { std::cout << _message << std::endl; SetDone(); -} \ No newline at end of file +} diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp index 886b2863..0c5f203b 100644 --- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp +++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "behaviour/BehaviourScheduler.h" using namespace behaviour; @@ -5,24 +9,25 @@ using namespace behaviour; BehaviourScheduler::BehaviourScheduler() {} BehaviourScheduler::~BehaviourScheduler() { - for (HasBehaviour *sys : _systems) { - if (sys->_active_behaviour) sys->_active_behaviour->Interrupt(); + for (HasBehaviour* sys : _systems) { + if (sys->_active_behaviour) + sys->_active_behaviour->Interrupt(); } - for (auto &t : _threads) { + for (auto& t : _threads) { t.join(); } } -BehaviourScheduler *_scheduler_instance; +BehaviourScheduler* _scheduler_instance; -BehaviourScheduler *BehaviourScheduler::GetInstance() { +BehaviourScheduler* BehaviourScheduler::GetInstance() { if (_scheduler_instance == nullptr) _scheduler_instance = new BehaviourScheduler(); return _scheduler_instance; } -void BehaviourScheduler::Register(HasBehaviour *system) { +void BehaviourScheduler::Register(HasBehaviour* system) { _systems.push_back(system); } @@ -33,7 +38,7 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) { std::lock_guard lk(_active_mtx); - for (HasBehaviour *sys : behaviour->GetControlled()) { + for (HasBehaviour* sys : behaviour->GetControlled()) { if (sys->_active_behaviour != nullptr) sys->_active_behaviour->Interrupt(); sys->_active_behaviour = behaviour; @@ -48,14 +53,14 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) { behaviour->Tick(); } std::this_thread::sleep_for(std::chrono::milliseconds( - (int64_t)(behaviour->GetPeriod().value() * 1000))); + static_cast(behaviour->GetPeriod().value() * 1000))); } }); } void BehaviourScheduler::Tick() { std::lock_guard lk(_active_mtx); - for (HasBehaviour *sys : _systems) { + for (HasBehaviour* sys : _systems) { if (sys->_active_behaviour != nullptr) { if (sys->_active_behaviour->IsFinished()) { if (sys->_default_behaviour_producer == nullptr) { @@ -72,8 +77,8 @@ void BehaviourScheduler::Tick() { void BehaviourScheduler::InterruptAll() { std::lock_guard lk(_active_mtx); - for (HasBehaviour *sys : _systems) { + for (HasBehaviour* sys : _systems) { if (sys->_active_behaviour) sys->_active_behaviour->Interrupt(); } -} \ No newline at end of file +} diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp index b2792901..582be028 100644 --- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp +++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "behaviour/HasBehaviour.h" #include "behaviour/Behaviour.h" @@ -11,4 +15,4 @@ void HasBehaviour::SetDefaultBehaviour( std::shared_ptr HasBehaviour::GetActiveBehaviour() { return _active_behaviour; -} \ No newline at end of file +} diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp index df7baa6e..82c54bed 100644 --- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp +++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp @@ -1,27 +1,37 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "drivetrain/Drivetrain.h" using namespace wom; using namespace frc; using namespace units; -Drivetrain::Drivetrain(DrivetrainConfig *config, XboxController &driver): _config(config), _driver(driver) {} +Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver) + : _config(config), _driver(driver) {} Drivetrain::~Drivetrain() {} -DrivetrainConfig *Drivetrain::GetConfig() { return _config; } -DrivetrainState Drivetrain::GetState() { return _state; } +DrivetrainConfig* Drivetrain::GetConfig() { + return _config; +} +DrivetrainState Drivetrain::GetState() { + return _state; +} -void Drivetrain::SetState(DrivetrainState state) { _state = state; } +void Drivetrain::SetState(DrivetrainState state) { + _state = state; +} void Drivetrain::OnStart() { std::cout << "Starting Tank" << std::endl; } void Drivetrain::OnUpdate(second_t dt) { - switch(_state) { + switch (_state) { case DrivetrainState::kIdle: break; - case DrivetrainState::kTank: - { + case DrivetrainState::kTank: { double rightSpeed = deadzone(_driver.GetRightY()); double leftSpeed = deadzone(_driver.GetLeftY()); _config->left1.transmission->SetVoltage(leftSpeed * maxVolts); @@ -31,7 +41,7 @@ void Drivetrain::OnUpdate(second_t dt) { _config->right2.transmission->SetVoltage(rightSpeed * maxVolts); _config->right3.transmission->SetVoltage(rightSpeed * maxVolts); break; - } + } case DrivetrainState::kAuto: break; } diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp deleted file mode 100644 index 5fed1b19..00000000 --- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* #include "drivetrain/SwerveDrive.h" */ -/**/ -/* // do not touch sub_system unless you know what your doing */ -/* wom::SwerveModule::SwerveModule(wom::Subsystem sub_system, wom::ModuleName name, wom::SwerveModuleConfig config, wom::SwerveModuleState state) : _name(name), _config(config), _state(state) { */ -/**/ -/* } */ -/**/ -/* wom::SwerveModule::~SwerveModule() {} */ -/**/ -/* void wom::SwerveModule::OnUpdate(units::second_t dt) { */ -/* switch(_state) { */ -/* case wom::SwerveModuleState::kIdle: */ -/* break; */ -/* case wom::SwerveModuleState::kPID: */ -/* break; */ -/* case wom::SwerveModuleState::kCalibration: */ -/* break; */ -/* } */ -/* } */ -/**/ -/* void wom::SwerveModule::OnStart() { */ -/* switch(_name) { */ -/* case wom::ModuleName::FrontLeft: */ -/* std::cout << "Starting Swerve Module Front Left" << std::endl; */ -/* break; */ -/* case wom::ModuleName::FrontRight: */ -/* std::cout << "Starting Swerve Module Front Right" << std::endl; */ -/* break; */ -/* case wom::ModuleName::BackLeft: */ -/* std::cout << "Starting Swerve Module Back Left" << std::endl; */ -/* break; */ -/* case wom::ModuleName::BackRight: */ -/* std::cout << "Starting Swerve Module Back Right" << std::endl; */ -/* break; */ -/* } */ -/* } */ - diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp index 82fb2113..aae3c928 100644 --- a/wombat/src/main/cpp/subsystems/Arm.cpp +++ b/wombat/src/main/cpp/subsystems/Arm.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "subsystems/Arm.h" #include @@ -5,64 +9,73 @@ using namespace frc; using namespace wom; -//creates network table instatnce on shuffleboard +// creates network table instatnce on shuffleboard void ArmConfig::WriteNT(std::shared_ptr table) { table->GetEntry("armMass").SetDouble(armMass.value()); table->GetEntry("loadMass").SetDouble(loadMass.value()); table->GetEntry("armLength").SetDouble(armLength.value()); - table->GetEntry("minAngle").SetDouble(minAngle.convert().value()); - table->GetEntry("maxAngle").SetDouble(maxAngle.convert().value()); - table->GetEntry("initialAngle").SetDouble(initialAngle.convert().value()); - table->GetEntry("angleOffset").SetDouble(initialAngle.convert().value()); + table->GetEntry("minAngle") + .SetDouble(minAngle.convert().value()); + table->GetEntry("maxAngle") + .SetDouble(maxAngle.convert().value()); + table->GetEntry("initialAngle") + .SetDouble(initialAngle.convert().value()); + table->GetEntry("angleOffset") + .SetDouble(initialAngle.convert().value()); } -//arm config is used +// arm config is used Arm::Arm(ArmConfig config) - : _config(config), - _pid(config.path + "/pid", config.pidConfig), - _velocityPID(config.path + "/velocityPID", config.velocityConfig), - _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) -{ -} + : _config(config), + _pid(config.path + "/pid", config.pidConfig), + _velocityPID(config.path + "/velocityPID", config.velocityConfig), + _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {} -//the loop that allows the information to be used +// the loop that allows the information to be used void Arm::OnUpdate(units::second_t dt) { - //sets the voltage and gets the current angle + // sets the voltage and gets the current angle units::volt_t voltage = 0_V; auto angle = GetAngle(); - //sets usable infromation for each state + // sets usable infromation for each state switch (_state) { case ArmState::kIdle: break; - case ArmState::kVelocity: - { - units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength * units::math::cos(angle + _config.angleOffset) * (0.5 * _config.armMass + _config.loadMass); - // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad/1_s); - units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint()); - // feedforward = 3.5_V; - // std::cout << "feedforward" << feedforward.value() << std::endl; - voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward); - // std::cout << "arm velocity voltage is: " << voltage.value() << std::endl; - // voltage = 0_V; - } - break; - case ArmState::kAngle: - { - units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength * units::math::cos(angle + _config.angleOffset) * (0.5 * _config.armMass + _config.loadMass); - units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad/ 1_s); - // std::cout << "feedforward" << feedforward.value() << std::endl; - voltage = _pid.Calculate(angle, dt, feedforward); - } - break; + case ArmState::kVelocity: { + units::newton_meter_t torque = + 9.81_m / 1_s / 1_s * _config.armLength * + units::math::cos(angle + _config.angleOffset) * + (0.5 * _config.armMass + _config.loadMass); + // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, + // 0_rad/1_s); + units::volt_t feedforward = + _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint()); + // feedforward = 3.5_V; + // std::cout << "feedforward" << feedforward.value() << std::endl; + voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward); + // std::cout << "arm velocity voltage is: " << voltage.value() << + // std::endl; voltage = 0_V; + } break; + case ArmState::kAngle: { + units::newton_meter_t torque = + 9.81_m / 1_s / 1_s * _config.armLength * + units::math::cos(angle + _config.angleOffset) * + (0.5 * _config.armMass + _config.loadMass); + units::volt_t feedforward = + _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s); + // std::cout << "feedforward" << feedforward.value() << std::endl; + voltage = _pid.Calculate(angle, dt, feedforward); + } break; case ArmState::kRaw: voltage = _voltage; break; } // if ( - // (((_config.minAngle + _config.angleOffset) < 75_deg && units::math::abs(_pid.GetSetpoint() - _config.minAngle) <= 1_deg) - // || ((_config.maxAngle + _config.angleOffset) > 105_deg && units::math::abs(_pid.GetSetpoint() - _config.maxAngle) <= 1_deg)) && + // (((_config.minAngle + _config.angleOffset) < 75_deg && + // units::math::abs(_pid.GetSetpoint() - _config.minAngle) <= 1_deg) + // || ((_config.maxAngle + _config.angleOffset) > 105_deg && + // units::math::abs(_pid.GetSetpoint() - _config.maxAngle) <= 1_deg)) && // units::math::abs(_pid.GetError()) <= 1_deg // ) { // voltage = 0_V; @@ -71,10 +84,13 @@ void Arm::OnUpdate(units::second_t dt) { voltage *= armLimit; // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq; - // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit, _config.leftGearbox.encoder->GetEncoderAngularVelocity()); - // units::volt_t voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit, _config.leftGearbox.encoder->GetEncoderAngularVelocity()); + // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit, + // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); units::volt_t + // voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit, + // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); - // voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin); + // voltage = units::math::max(units::math::min(voltage, voltageMax), + // voltageMin); units::volt_t voltageMin = -5.5_V; units::volt_t voltageMax = 5.5_V; voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin); @@ -84,7 +100,7 @@ void Arm::OnUpdate(units::second_t dt) { _config.leftGearbox.transmission->SetVoltage(voltage); _config.rightGearbox.transmission->SetVoltage(voltage); - //creates network table instances for the angle and config of the arm + // creates network table instances for the angle and config of the arm _table->GetEntry("angle").SetDouble(angle.convert().value()); _config.WriteNT(_table->GetSubTable("config")); } @@ -93,7 +109,8 @@ void Arm::SetArmSpeedLimit(double limit) { armLimit = limit; } -//defines information needed for the functions and connects the states to their respective function +// defines information needed for the functions and connects the states to their +// respective function void Arm::SetIdle() { _state = ArmState::kIdle; @@ -114,7 +131,7 @@ void Arm::SetVelocity(units::radians_per_second_t velocity) { _velocityPID.SetSetpoint(velocity); } -ArmConfig &Arm::GetConfig() { +ArmConfig& Arm::GetConfig() { return _config; } @@ -134,17 +151,19 @@ bool Arm::IsStable() const { return _pid.IsStable(5_deg); } - /* SIMULATION */ // #include -// ::wom::sim::ArmSim::ArmSim(ArmConfig config) +// ::wom::sim::ArmSim::ArmSim(ArmConfig config) // : config(config), // angle(config.initialAngle), // encoder(config.gearbox.encoder->MakeSimEncoder()), -// lowerLimit(config.lowerLimitSwitch ? new frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr), -// upperLimit(config.upperLimitSwitch ? new frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr), -// table(nt::NetworkTableInstance::GetDefault().GetTable(config.path + "/sim")) +// lowerLimit(config.lowerLimitSwitch ? new +// frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr), +// upperLimit(config.upperLimitSwitch ? new +// frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr), +// table(nt::NetworkTableInstance::GetDefault().GetTable(config.path + +// "/sim")) // {} // units::ampere_t wom::sim::ArmSim::GetCurrent() const { @@ -152,9 +171,12 @@ bool Arm::IsStable() const { // } // void ::wom::sim::ArmSim::Update(units::second_t dt) { -// torque = (config.loadMass * config.armLength + config.armMass * config.armLength / 2.0) * 9.81_m / 1_s / 1_s * units::math::cos(config.angleOffset + angle) + additionalTorque; -// velocity = config.gearbox.motor.Speed(torque, config.gearbox.transmission->GetEstimatedRealVoltage()); -// angle += velocity * dt; +// torque = (config.loadMass * config.armLength + config.armMass * +// config.armLength / 2.0) * 9.81_m / 1_s / 1_s * +// units::math::cos(config.angleOffset + angle) + additionalTorque; velocity = +// config.gearbox.motor.Speed(torque, +// config.gearbox.transmission->GetEstimatedRealVoltage()); angle += velocity +// * dt; // if (angle <= config.minAngle) { // angle = config.minAngle; @@ -172,7 +194,8 @@ bool Arm::IsStable() const { // if (upperLimit) upperLimit->SetValue(false); // } -// current = config.gearbox.motor.Current(velocity, config.gearbox.transmission->GetEstimatedRealVoltage()); +// current = config.gearbox.motor.Current(velocity, +// config.gearbox.transmission->GetEstimatedRealVoltage()); // if (encoder) encoder->SetEncoderTurns(angle - config.initialAngle); diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp index 74406b00..2d2e325f 100644 --- a/wombat/src/main/cpp/subsystems/Elevator.cpp +++ b/wombat/src/main/cpp/subsystems/Elevator.cpp @@ -1,9 +1,14 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "subsystems/Elevator.h" -#include -#include +#include #include +#include + using namespace wom; void ElevatorConfig::WriteNT(std::shared_ptr table) { @@ -13,11 +18,13 @@ void ElevatorConfig::WriteNT(std::shared_ptr table) { } Elevator::Elevator(ElevatorConfig config) - : _config(config), _state(ElevatorState::kIdle), - _pid{config.path + "/pid", config.pid}, - _velocityPID{config.path + "/velocityPID", config.velocityPID}, - _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) { - // _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad); + : _config(config), + _state(ElevatorState::kIdle), + _pid{config.path + "/pid", config.pid}, + _velocityPID{config.path + "/velocityPID", config.velocityPID}, + _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) { + // _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / + // _config.radius * 1_rad); } void Elevator::OnUpdate(units::second_t dt) { @@ -25,44 +32,47 @@ void Elevator::OnUpdate(units::second_t dt) { units::meter_t height = GetElevatorEncoderPos() * 1_m; - switch(_state) { + switch (_state) { case ElevatorState::kIdle: voltage = 0_V; - break; + break; case ElevatorState::kManual: voltage = _setpointManual; - break; - case ElevatorState::kVelocity: - { - units::volt_t feedforward = _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad); - // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm, _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad); - feedforward = 1.2_V; - voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward); - if (voltage > 6_V) { - voltage = 6_V; - } - std::cout << "elevator feedforward: " << feedforward.value() << std::endl; - // voltage = 0_V; - } break; - case ElevatorState::kPID: - { - units::volt_t feedforward = _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s); - // std::cout << "feed forward" << feedforward.value() << std::endl; - feedforward = 1.2_V; - // voltage = _pid.Calculate(height, dt, feedforward); - voltage = _pid.Calculate(height, dt, feedforward); - if (voltage > 6_V) { - voltage = 6_V; - } + case ElevatorState::kVelocity: { + units::volt_t feedforward = _config.rightGearbox.motor.Voltage( + (_config.mass * 9.81_mps_sq) * _config.radius, + _velocityPID.GetSetpoint() / + (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad); + // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm, + // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) + // * 1_rad); + feedforward = 1.2_V; + voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward); + if (voltage > 6_V) { + voltage = 6_V; + } + std::cout << "elevator feedforward: " << feedforward.value() << std::endl; + // voltage = 0_V; + } break; + case ElevatorState::kPID: { + units::volt_t feedforward = _config.rightGearbox.motor.Voltage( + (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s); + // std::cout << "feed forward" << feedforward.value() << std::endl; + feedforward = 1.2_V; + // voltage = _pid.Calculate(height, dt, feedforward); + voltage = _pid.Calculate(height, dt, feedforward); + if (voltage > 6_V) { + voltage = 6_V; } - break; + } break; } // Top Sensor Detector // if(_config.topSensor != nullptr) { // if(_config.topSensor->Get()) { - // _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight / _config.radius * 1_rad); + // _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight / + // _config.radius * 1_rad); // //voltage = 0_V; // } // } @@ -70,7 +80,8 @@ void Elevator::OnUpdate(units::second_t dt) { // //Bottom Sensor Detection // if (_config.bottomSensor != nullptr) { // if (_config.bottomSensor->Get()) { - // _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight / _config.radius * 1_rad); + // _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight / + // _config.radius * 1_rad); // //voltage = 0_V; // } // } @@ -106,7 +117,7 @@ void Elevator::SetIdle() { _state = ElevatorState::kIdle; } -ElevatorConfig &Elevator::GetConfig() { +ElevatorConfig& Elevator::GetConfig() { return _config; } @@ -119,19 +130,24 @@ ElevatorState Elevator::GetState() const { } double Elevator::GetElevatorEncoderPos() { - return _config.elevatorEncoder.GetPosition() * 14/60 * 2 * 3.1415 * 0.02225; + return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225; } units::meter_t Elevator::GetHeight() const { - // std::cout << "elevator position"<< _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; - // return _config.rightGearbox.encoder->GetEncoderDistance() * 1_m; - return _config.elevatorEncoder.GetPosition() * 14/60 * 2 * 3.1415 * 0.02225 * 1_m; + // std::cout << "elevator position"<< + // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return + // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m; + return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * + 0.02225 * 1_m; } units::meters_per_second_t Elevator::GetElevatorVelocity() const { - return _config.elevatorEncoder.GetVelocity() / 60_s * 14/60 * 2 * 3.1415 * 0.02225 * 1_m; + return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * + 0.02225 * 1_m; } units::meters_per_second_t Elevator::MaxSpeed() const { - return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad * _config.radius; + return _config.leftGearbox.motor.Speed( + (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / + 1_rad * _config.radius; } diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp index 2e04d917..3f5cea21 100644 --- a/wombat/src/main/cpp/subsystems/Shooter.cpp +++ b/wombat/src/main/cpp/subsystems/Shooter.cpp @@ -1,50 +1,58 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "subsystems/Shooter.h" #include using namespace wom; -Shooter::Shooter(std::string path, ShooterParams params) - : _params(params), _state(ShooterState::kIdle), - _pid{path + "/pid", params.pid}, - _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {} +Shooter::Shooter(std::string path, ShooterParams params) + : _params(params), + _state(ShooterState::kIdle), + _pid{path + "/pid", params.pid}, + _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {} void Shooter::OnUpdate(units::second_t dt) { units::volt_t voltage{0}; - units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity(); + units::revolutions_per_minute_t currentSpeed = + _params.gearbox.encoder->GetEncoderAngularVelocity(); - switch(_state) { + switch (_state) { case ShooterState::kManual: voltage = _setpointManual; break; - case ShooterState::kPID: - { - auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint()); - voltage = _pid.Calculate(currentSpeed, dt, feedforward); - } - break; + case ShooterState::kPID: { + auto feedforward = + _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint()); + voltage = _pid.Calculate(currentSpeed, dt, feedforward); + } break; case ShooterState::kIdle: voltage = 0_V; break; } - units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit); - units::volt_t max_voltage_for_current_limit = _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed); + units::newton_meter_t max_torque_at_current_limit = + _params.gearbox.motor.Torque(_params.currentLimit); + units::volt_t max_voltage_for_current_limit = + _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed); - voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value()); + voltage = + 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value()); _params.gearbox.transmission->SetVoltage(voltage); _table->GetEntry("output_volts").SetDouble(voltage.value()); _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value()); - _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value()); + _table->GetEntry("setpoint_rpm") + .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value()); _table->GetEntry("stable").SetBoolean(_pid.IsStable()); } void Shooter::SetManual(units::volt_t voltage) { _state = ShooterState::kManual; _setpointManual = voltage; - } void Shooter::SetPID(units::radians_per_second_t goal) { @@ -60,21 +68,22 @@ bool Shooter::IsStable() const { return _pid.IsStable(); } -//Shooter Manual Set +// Shooter Manual Set + +ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint) + : _shooter(s), _setpoint(setpoint) { + Controls(_shooter); +} -ShooterConstant::ShooterConstant(Shooter *s, units::volt_t setpoint) - : _shooter(s), _setpoint(setpoint) { - Controls(_shooter); - } - void ShooterConstant::OnTick(units::second_t dt) { _shooter->SetManual(_setpoint); } // ShooterSpinup -ShooterSpinup::ShooterSpinup(Shooter *s, units::radians_per_second_t speed, bool hold) - : _shooter(s), _speed(speed), _hold(hold) { +ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, + bool hold) + : _shooter(s), _speed(speed), _hold(hold) { Controls(_shooter); } diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp index b40b6c5b..e0796f2e 100644 --- a/wombat/src/main/cpp/utils/Encoder.cpp +++ b/wombat/src/main/cpp/utils/Encoder.cpp @@ -1,4 +1,9 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "utils/Encoder.h" + #include using namespace wom; @@ -52,8 +57,10 @@ double Encoder::GetEncoderDistance() { } units::radians_per_second_t Encoder::GetEncoderAngularVelocity() { - // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2 * 3.1415926; - units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()}; + // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2 + // * 3.1415926; + units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / + GetEncoderTicksPerRotation()}; return n_turns_per_s; } @@ -66,8 +73,9 @@ double DigitalEncoder::GetEncoderTickVelocity() const { return _nativeEncoder.GetRate(); } -CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax *controller, double reduction) - : Encoder(42, reduction, 2), _encoder(controller->GetEncoder()) {} +CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, + double reduction) + : Encoder(42, reduction, 2), _encoder(controller->GetEncoder()) {} double CANSparkMaxEncoder::GetEncoderRawTicks() const { return _encoder.GetPosition() * _reduction; @@ -77,10 +85,12 @@ double CANSparkMaxEncoder::GetEncoderTickVelocity() const { return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60; } -TalonFXEncoder::TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX *controller, double reduction) - : Encoder(2048, reduction, 0), _controller(controller) { - controller->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::TalonFXFeedbackDevice::IntegratedSensor); - } +TalonFXEncoder::TalonFXEncoder( + ctre::phoenix::motorcontrol::can::TalonFX* controller, double reduction) + : Encoder(2048, reduction, 0), _controller(controller) { + controller->ConfigSelectedFeedbackSensor( + ctre::phoenix::motorcontrol::TalonFXFeedbackDevice::IntegratedSensor); +} double TalonFXEncoder::GetEncoderRawTicks() const { return _controller->GetSelectedSensorPosition(); @@ -90,11 +100,13 @@ double TalonFXEncoder::GetEncoderTickVelocity() const { return _controller->GetSelectedSensorVelocity() * 10; } - -TalonSRXEncoder::TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX *controller, double ticksPerRotation, double reduction) - : Encoder(ticksPerRotation, reduction, 0), _controller(controller) { - controller->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::TalonSRXFeedbackDevice::QuadEncoder); - } +TalonSRXEncoder::TalonSRXEncoder( + ctre::phoenix::motorcontrol::can::TalonSRX* controller, + double ticksPerRotation, double reduction) + : Encoder(ticksPerRotation, reduction, 0), _controller(controller) { + controller->ConfigSelectedFeedbackSensor( + ctre::phoenix::motorcontrol::TalonSRXFeedbackDevice::QuadEncoder); +} double TalonSRXEncoder::GetEncoderRawTicks() const { return _controller->GetSelectedSensorPosition(); @@ -104,10 +116,9 @@ double TalonSRXEncoder::GetEncoderTickVelocity() const { return _controller->GetSelectedSensorVelocity() * 10; } - - -DutyCycleEncoder::DutyCycleEncoder(int channel, double ticksPerRotation, double reduction) - : Encoder(ticksPerRotation, reduction, 0), _dutyCycleEncoder(channel) {} +DutyCycleEncoder::DutyCycleEncoder(int channel, double ticksPerRotation, + double reduction) + : Encoder(ticksPerRotation, reduction, 0), _dutyCycleEncoder(channel) {} double DutyCycleEncoder::GetEncoderRawTicks() const { return _dutyCycleEncoder.Get().value(); @@ -117,10 +128,11 @@ double DutyCycleEncoder::GetEncoderTickVelocity() const { return 0; } -CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation, double reduction) - : Encoder(ticksPerRotation, reduction, 1) { - _canEncoder = new CANCoder(deviceNumber, "Drivebase"); - } +CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation, + double reduction) + : Encoder(ticksPerRotation, reduction, 1) { + _canEncoder = new CANCoder(deviceNumber, "Drivebase"); +} double CanEncoder::GetEncoderRawTicks() const { return _canEncoder->GetAbsolutePosition(); @@ -129,4 +141,3 @@ double CanEncoder::GetEncoderRawTicks() const { double CanEncoder::GetEncoderTickVelocity() const { return _canEncoder->GetVelocity(); } - diff --git a/wombat/src/main/cpp/utils/RobotStartup.cpp b/wombat/src/main/cpp/utils/RobotStartup.cpp index e58e3bff..80f81dac 100644 --- a/wombat/src/main/cpp/utils/RobotStartup.cpp +++ b/wombat/src/main/cpp/utils/RobotStartup.cpp @@ -1,5 +1,9 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "utils/RobotStartup.h" void wom::RobotStartup::Start(std::function robotFunc) { robotFunc(); -} \ No newline at end of file +} diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp index b59563c1..f26aeb4f 100644 --- a/wombat/src/main/cpp/utils/Util.cpp +++ b/wombat/src/main/cpp/utils/Util.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "utils/Util.h" units::second_t wom::now() { @@ -5,25 +9,27 @@ units::second_t wom::now() { return static_cast(now) / 1000000 * 1_s; } -void wom::WritePose2NT(std::shared_ptr table, frc::Pose2d pose) { +void wom::WritePose2NT(std::shared_ptr table, + frc::Pose2d pose) { table->GetEntry("x").SetDouble(pose.X().value()); table->GetEntry("y").SetDouble(pose.Y().value()); table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value()); } -void wom::WritePose3NT(std::shared_ptr table, frc::Pose3d pose) { +void wom::WritePose3NT(std::shared_ptr table, + frc::Pose3d pose) { table->GetEntry("x").SetDouble(pose.X().value()); table->GetEntry("y").SetDouble(pose.Y().value()); table->GetEntry("z").SetDouble(pose.Z().value()); - table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert().value()); + table->GetEntry("angle").SetDouble( + pose.Rotation().Z().convert().value()); } double wom::deadzone(double val, double deadzone) { - return std::fabs(val) > deadzone ? val : 0; + return std::fabs(val) > deadzone ? val : 0; } double wom::spow2(double val) { - return val*val*(val > 0 ? 1 : -1); + return val * val * (val > 0 ? 1 : -1); } - diff --git a/wombat/src/main/cpp/utils/VoltageController.cpp b/wombat/src/main/cpp/utils/VoltageController.cpp index 43bac3a9..692edd76 100644 --- a/wombat/src/main/cpp/utils/VoltageController.cpp +++ b/wombat/src/main/cpp/utils/VoltageController.cpp @@ -1,5 +1,8 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include "utils/VoltageController.h" -#include #include @@ -10,7 +13,9 @@ units::volt_t VoltageController::GetEstimatedRealVoltage() const { return units::math::min(units::math::max(-vb, GetVoltage()), vb); } -MotorVoltageController::MotorVoltageController(frc::MotorController *MotorController) : _MotorController(MotorController) {} +MotorVoltageController::MotorVoltageController( + frc::MotorController* MotorController) + : _MotorController(MotorController) {} void MotorVoltageController::SetVoltage(units::volt_t voltage) { _MotorController->Set(voltage / GetBusVoltage()); diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h index c467171b..479adb92 100644 --- a/wombat/src/main/include/behaviour/Behaviour.h +++ b/wombat/src/main/include/behaviour/Behaviour.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include @@ -13,7 +17,9 @@ #include #include #include +#include #include +#include #include "HasBehaviour.h" @@ -42,7 +48,7 @@ class Behaviour : public std::enable_shared_from_this { public: using ptr = std::shared_ptr; - Behaviour(std::string name = "", + Behaviour(std::string name = "", units::time::second_t period = 20_ms); ~Behaviour(); @@ -54,7 +60,7 @@ class Behaviour : public std::enable_shared_from_this { /** * Called when the Behaviour first starts */ - virtual void OnStart(){}; + virtual void OnStart() {} /** * Called periodically as the Behaviour runs @@ -66,7 +72,7 @@ class Behaviour : public std::enable_shared_from_this { /** * Called when the Behaviour stops running */ - virtual void OnStop(){}; + virtual void OnStop() {} /** * Set the period of the Behaviour. Note this only affects the Behaviour @@ -91,12 +97,12 @@ class Behaviour : public std::enable_shared_from_this { * output, a demand, or some other controlling method. When Behaviours run, * only one Behaviour at a time may have control over a system. */ - void Controls(HasBehaviour *sys); + void Controls(HasBehaviour* sys); /** * Inherit controlled systems from another Behaviour. */ - void Inherit(Behaviour &bhvr); + void Inherit(Behaviour& bhvr); /** * Set a timeout on this Behaviour. If the Behaviour runs longer than the @@ -108,7 +114,7 @@ class Behaviour : public std::enable_shared_from_this { * @return wpi::SmallPtrSetImpl& The systems controlled by * this behaviour. */ - wpi::SmallPtrSetImpl &GetControlled(); + wpi::SmallPtrSetImpl& GetControlled(); /** * @return BehaviourState The current state of the behaviour @@ -149,18 +155,17 @@ class Behaviour : public std::enable_shared_from_this { */ Behaviour::ptr Until(Behaviour::ptr other); - private: void Stop(BehaviourState new_state); - std::string _bhvr_name; - units::time::second_t _bhvr_period = 20_ms; + std::string _bhvr_name; + units::time::second_t _bhvr_period = 20_ms; std::atomic _bhvr_state; - wpi::SmallSet _bhvr_controls; + wpi::SmallSet _bhvr_controls; - double _bhvr_time = 0; - units::time::second_t _bhvr_timer = 0_s; + double _bhvr_time = 0; + units::time::second_t _bhvr_timer = 0_s; units::time::second_t _bhvr_timeout = -1_s; }; @@ -168,7 +173,7 @@ class Behaviour : public std::enable_shared_from_this { * Shorthand function to create a shared_ptr for a Behaviour. */ template -std::shared_ptr make(Args &&...args) { +std::shared_ptr make(Args&&... args) { return std::make_shared(std::forward(args)...); } @@ -206,8 +211,8 @@ inline std::shared_ptr operator<<( class DuplicateControlException : public std::exception { public: - DuplicateControlException(const std::string &msg) : _msg(msg) {} - const char *what() const noexcept override { return _msg.c_str(); } + explicit DuplicateControlException(const std::string& msg) : _msg(msg) {} + const char* what() const noexcept override { return _msg.c_str(); } private: std::string _msg; @@ -222,7 +227,7 @@ enum class ConcurrentBehaviourReducer { ALL, ANY, FIRST }; */ class ConcurrentBehaviour : public Behaviour { public: - ConcurrentBehaviour(ConcurrentBehaviourReducer reducer); + explicit ConcurrentBehaviour(ConcurrentBehaviourReducer reducer); void Add(Behaviour::ptr behaviour); @@ -233,11 +238,11 @@ class ConcurrentBehaviour : public Behaviour { void OnStop() override; private: - ConcurrentBehaviourReducer _reducer; + ConcurrentBehaviourReducer _reducer; std::vector> _children; - std::mutex _children_finished_mtx; - std::vector _children_finished; - std::vector _threads; + std::mutex _children_finished_mtx; + std::vector _children_finished; + std::vector _threads; }; /** @@ -277,12 +282,12 @@ struct If : public Behaviour { * @param condition The condition to check, called when the behaviour is * scheduled. */ - If(std::function condition); + explicit If(std::function condition); /** * Create a new If decision behaviour * @param v The condition to check */ - If(bool v); + explicit If(bool v); /** * Set the behaviour to be called if the condition is true @@ -299,8 +304,8 @@ struct If : public Behaviour { private: std::function _condition; - bool _value; - Behaviour::ptr _then, _else; + bool _value; + Behaviour::ptr _then, _else; }; /** @@ -316,12 +321,12 @@ struct Switch : public Behaviour { * Create a new Switch behaviour, with a given parameter * @param fn The function yielding the parameter, called in OnTick */ - Switch(std::function fn) : _fn(fn) {} + explicit Switch(std::function fn) : _fn(fn) {} /** * Create a new Switch behaviour, with a given parameter * @param v The parameter on which decisions are made */ - Switch(T v) : Switch([v]() { return v; }) {} + explicit Switch(T v) : Switch([v]() { return v; }) {} /** * Add a new option to the Switch chain. @@ -329,8 +334,8 @@ struct Switch : public Behaviour { * @param condition The function yielding true if this is the correct option * @param b The behaviour to call if this option is provided. */ - std::shared_ptr When(std::function condition, - Behaviour::ptr b) { + std::shared_ptr When(std::function condition, + Behaviour::ptr b) { _options.push_back(std::pair(condition, b)); Inherit(*b); return std::reinterpret_pointer_cast>(shared_from_this()); @@ -343,7 +348,7 @@ struct Switch : public Behaviour { * @param b The behaviour to call if this option is provided. */ std::shared_ptr When(T value, Behaviour::ptr b) { - return When([value](T &v) { return value == v; }, b); + return When([value](T& v) { return value == v; }, b); } /** @@ -351,14 +356,14 @@ struct Switch : public Behaviour { * @param b The behaviour to call if no other When's match. */ std::shared_ptr Otherwise(Behaviour::ptr b = nullptr) { - return When([](T &v) { return true; }, b); + return When([](T& v) { return true; }, b); } void OnTick(units::time::second_t dt) override { T val = _fn(); if (!_locked) { - for (auto &opt : _options) { + for (auto& opt : _options) { if (opt.first(val)) { _locked = opt.second; @@ -378,7 +383,7 @@ struct Switch : public Behaviour { void OnStop() override { if (GetBehaviourState() != BehaviourState::DONE) { - for (auto &opt : _options) { + for (auto& opt : _options) { opt.second->Interrupt(); } } @@ -386,8 +391,8 @@ struct Switch : public Behaviour { private: std::function _fn; - wpi::SmallVector, Behaviour::ptr>, 4> - _options; + wpi::SmallVector, Behaviour::ptr>, 4> + _options; Behaviour::ptr _locked = nullptr; }; @@ -396,7 +401,7 @@ struct Switch : public Behaviour { * provided and is instead based purely on predicates. */ struct Decide : public Switch { - Decide() : Switch(std::monostate{}){}; + Decide() : Switch(std::monostate{}) {} /** * Add a new option to the Switch chain. @@ -405,7 +410,7 @@ struct Decide : public Switch { * @param b The behaviour to call if this option is provided. */ std::shared_ptr When(std::function condition, - Behaviour::ptr b) { + Behaviour::ptr b) { return std::reinterpret_pointer_cast( Switch::When([condition](auto) { return condition(); }, b)); } @@ -420,7 +425,7 @@ struct WaitFor : public Behaviour { * Create a new WaitFor behaviour * @param predicate The condition predicate */ - WaitFor(std::function predicate); + explicit WaitFor(std::function predicate); void OnTick(units::time::second_t dt) override; @@ -437,27 +442,28 @@ struct WaitTime : public Behaviour { * Create a new WaitTime behaviour * @param time The time period to wait */ - WaitTime(units::time::second_t time); + explicit WaitTime(units::time::second_t time); /** * Create a new WaitTime behaviour * @param time_fn The time period to wait, evaluated at OnStart */ - WaitTime(std::function time_fn); + explicit WaitTime(std::function time_fn); void OnStart() override; void OnTick(units::time::second_t dt) override; private: std::function _time_fn; - units::time::second_t _time; + units::time::second_t _time; }; struct Print : public Behaviour { public: - Print(std::string message); + explicit Print(std::string message); void OnTick(units::time::second_t dt) override; + private: std::string _message; }; diff --git a/wombat/src/main/include/behaviour/BehaviourScheduler.h b/wombat/src/main/include/behaviour/BehaviourScheduler.h index 8ee23992..47e96ebf 100644 --- a/wombat/src/main/include/behaviour/BehaviourScheduler.h +++ b/wombat/src/main/include/behaviour/BehaviourScheduler.h @@ -1,6 +1,11 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include +#include #include "Behaviour.h" #include "HasBehaviour.h" @@ -23,13 +28,13 @@ class BehaviourScheduler { /** * @return BehaviourScheduler* The global instance of the BehaviourScheduler */ - static BehaviourScheduler *GetInstance(); + static BehaviourScheduler* GetInstance(); /** * Register a system with the behaviour scheduler. A system must be registered * for it to be controlled by behaviours. */ - void Register(HasBehaviour *system); + void Register(HasBehaviour* system); /** * Schedule a behaviour, interrupting all behaviours currently running that @@ -49,8 +54,8 @@ class BehaviourScheduler { void InterruptAll(); private: - std::vector _systems; - std::recursive_mutex _active_mtx; - std::vector _threads; + std::vector _systems; + std::recursive_mutex _active_mtx; + std::vector _threads; }; -} // namespace behaviour \ No newline at end of file +} // namespace behaviour diff --git a/wombat/src/main/include/behaviour/Behaviours.md b/wombat/src/main/include/behaviour/Behaviours.md index 651c93c8..eb43d12d 100644 --- a/wombat/src/main/include/behaviour/Behaviours.md +++ b/wombat/src/main/include/behaviour/Behaviours.md @@ -69,7 +69,7 @@ BehaviourScheduler::GetInstance()->Schedule(behaviour); ``` ### Controlling systems -Let's look at how we might control a system with Behaviours. For this example, we're going to create a Behaviour that tells a flywheel shooter to spin up to a certain speed. +Let's look at how we might control a system with Behaviours. For this example, we're going to create a Behaviour that tells a flywheel shooter to spin up to a certain speed. First, we need to make our shooter system able to have a Behaviour. To do this, we make it implement from `HasBehaviour` - that's it, no methods to override or anything else. @@ -126,8 +126,8 @@ ShooterSpinup::ShooterSpinup(Shooter &s, units::rad_per_s speed, bool hold) : _s void ShooterSpinup::OnTick(units::time::second_t dt) { // Get the current speed from the shooter's encoder units::rad_per_s current_speed = _shooter.gearbox.encoder.GetAngularVelocity(); - // Calculate the feedforward voltage - the voltage required to spin the - // motor assuming there is no torque (load) applied. This is simple if + // Calculate the feedforward voltage - the voltage required to spin the + // motor assuming there is no torque (load) applied. This is simple if // we use WPILib's DcMotor class from wpimath (#include ). units::volt_t feed_forward = _shooter.gearbox.motor.Voltage(0, _speed); @@ -137,7 +137,7 @@ void ShooterSpinup::OnTick(units::time::second_t dt) { // If the PID says we're done, stop this behaviour and move on! // Note "hold", which will keep the behaviour running even after we meet our speed - // We can use this in conjunction with ->Until(behaviour), which will keep our + // We can use this in conjunction with ->Until(behaviour), which will keep our // behaviour running until another behaviour is finished. if (_pid.IsDone() && !_hold) SetDone(); @@ -158,10 +158,10 @@ spinup->SetPeriod(10_ms); // 100Hz ``` ## Using Behaviours Together -As we mentioned, Behaviours are small, compartmentalised units of work that we can use together to make complex routines. In order to achieve this, Wombat provides some ways to combine behaviours together into larger sequences. +As we mentioned, Behaviours are small, compartmentalised units of work that we can use together to make complex routines. In order to achieve this, Wombat provides some ways to combine behaviours together into larger sequences. ### Sequential Execution -Behaviours can be executed in sequence by using the `<<` operator. +Behaviours can be executed in sequence by using the `<<` operator. ```cpp auto combined = make() @@ -186,7 +186,7 @@ auto wait_until = make() ->Until(make()); ``` ### Waiting -Wombat provides `WaitTime` and `WaitFor` to produce simple waits in the behaviour chain. +Wombat provides `WaitTime` and `WaitFor` to produce simple waits in the behaviour chain. ```cpp auto wait_2s = make(2_s); // WaitFor will wait until a function (predicate) returns true before continuing. @@ -205,7 +205,7 @@ auto if_bhvr = make([&vision]() { return vision.ready(); }) ->Then(make()) ->Else(make()); ``` -Switch is similar to a switch-case statement, allowing you to choose from one of many options. +Switch is similar to a switch-case statement, allowing you to choose from one of many options. ```cpp auto switch_bhvr = make(my_int) // Select based on the value directly @@ -214,7 +214,7 @@ auto switch_bhvr = make(my_int) // Or, based on the value using a predicate ->When([](auto my_int) { return my_int > 6; }, make()) ->Otherwise(make()); -// If no When matches, and Otherwise is not provided, the behaviour will +// If no When matches, and Otherwise is not provided, the behaviour will // keep running until one of the options matches. You can also provide // Otherwise without an argument to exit if none match. // Like If, you can also provide a function to yield the initial value @@ -239,19 +239,19 @@ Let's say we come up with a plan to do a really awesome (but really complicated) - Wait 2 seconds - Move forward 1.5m - Intake a ball -- While driving backwards 3m: +- While driving backwards 3m: - Spinup the shooter - Wait until vision is ready - Shoot a ball - Intake another ball - Spinup & Shoot the last ball Complex, right? Let's look at how we break it down. First of all, notice that it's already in a sequence of steps for us - small chunks of work that we can harness to complete our goals. Also notice that there's some common behaviour across this routine - there's multiple times where we move forward, intake a ball, etc. We can use this to our advantage by reducing the amount of code we need to write. -We can use the steps we've already outlined to build our overall behaviour sequence that describes the autonomous routine. +We can use the steps we've already outlined to build our overall behaviour sequence that describes the autonomous routine. Let's go ahead and mock up what we think our autonomous routine above will look like in code: ```cpp Behaviour::ptr MyAutoRoutine() { return ( - make(shooter, 500_rpm, true) + make(shooter, 500_rpm, true) ->Until( make(drivetrain, 2_m) << make(intake) @@ -262,11 +262,11 @@ Behaviour::ptr MyAutoRoutine() { << make(drivetrain, 1.5_m) << make(intake) << ( - // Drive Straight backwards, and + // Drive Straight backwards, and make(drivetrain, -3_m) & ( // Spinup the shooter until vision is ready, then fire. - make(shooter, 500_rpm, true) + make(shooter, 500_rpm, true) ->Until(make([vision]() { return vision.ready(); })) << make(shooter) ) @@ -283,4 +283,3 @@ See how we can just flow on from the individual steps of our big, complex exampl - `DriveStraight` - `DriveTurn` - `IntakeOne` - diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h index 11e608ac..03cd26b1 100644 --- a/wombat/src/main/include/behaviour/HasBehaviour.h +++ b/wombat/src/main/include/behaviour/HasBehaviour.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include @@ -25,10 +29,11 @@ class HasBehaviour { std::shared_ptr GetActiveBehaviour(); protected: - std::shared_ptr _active_behaviour{nullptr}; - std::function(void)> _default_behaviour_producer{nullptr}; + std::shared_ptr _active_behaviour{nullptr}; + std::function(void)> _default_behaviour_producer{ + nullptr}; private: friend class BehaviourScheduler; }; -} // namespace behaviour \ No newline at end of file +} // namespace behaviour diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h index 14ecefdb..6491982e 100644 --- a/wombat/src/main/include/drivetrain/Drivetrain.h +++ b/wombat/src/main/include/drivetrain/Drivetrain.h @@ -1,53 +1,56 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once -#include +#include +#include +#include + #include +#include #include "behaviour/HasBehaviour.h" #include "utils/Gearbox.h" -#include -#include - -#include - namespace wom { - // TODO PID - struct DrivetrainConfig { - std::string path; - - wom::Gearbox left1; - wom::Gearbox left2; - wom::Gearbox left3; - wom::Gearbox right1; - wom::Gearbox right2; - wom::Gearbox right3; - }; - - enum DrivetrainState { - kIdle, - kTank, - kAuto, - }; - - class Drivetrain : public behaviour::HasBehaviour { - public: - Drivetrain(DrivetrainConfig *config, frc::XboxController &driver); - ~Drivetrain(); - - DrivetrainConfig *GetConfig(); DrivetrainState GetState(); - - void SetState(DrivetrainState state); - - void OnStart(); - void OnUpdate(units::second_t dt); - protected: - - private: - DrivetrainConfig *_config; - DrivetrainState _state; - frc::XboxController &_driver; - units::volt_t maxVolts = 9_V; - }; -} - +// TODO PID +struct DrivetrainConfig { + std::string path; + + wom::Gearbox left1; + wom::Gearbox left2; + wom::Gearbox left3; + wom::Gearbox right1; + wom::Gearbox right2; + wom::Gearbox right3; +}; + +enum DrivetrainState { + kIdle, + kTank, + kAuto, +}; + +class Drivetrain : public behaviour::HasBehaviour { + public: + Drivetrain(DrivetrainConfig* config, frc::XboxController& driver); + ~Drivetrain(); + + DrivetrainConfig* GetConfig(); + DrivetrainState GetState(); + + void SetState(DrivetrainState state); + + void OnStart(); + void OnUpdate(units::second_t dt); + + protected: + private: + DrivetrainConfig* _config; + DrivetrainState _state; + frc::XboxController& _driver; + units::volt_t maxVolts = 9_V; +}; +} // namespace wom diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h deleted file mode 100644 index 1beaefca..00000000 --- a/wombat/src/main/include/drivetrain/SwerveDrive.h +++ /dev/null @@ -1,38 +0,0 @@ -/* #pragma once */ -/**/ -/* #include "behaviour/Behaviour.h" */ -/* #include "subsystems/Subsystem.h" */ -/* #include "utils/PID.h" */ -/**/ -/* namespace wom { */ -/* enum ModuleName { */ -/* FrontLeft, */ -/* FrontRight, */ -/* BackLeft, */ -/* BackRight, */ -/* }; */ -/**/ -/* struct SwerveModuleConfig {}; */ -/**/ -/* enum SwerveModuleState { */ -/* kIdle, */ -/* kCalibration, */ -/* kPID, */ -/* }; */ -/**/ -/* class SwerveModule : public wom::Subsystem, public behaviour::HasBehaviour { */ -/* public: */ -/* SwerveModule(wom::Subsystem _, wom::ModuleName name, wom::SwerveModuleConfig config, wom::SwerveModuleState state); */ -/* ~SwerveModule(); */ -/**/ -/* void OnStart() override; */ -/* void OnUpdate(units::second_t dt) override; */ -/**/ -/* protected: */ -/**/ -/* private: */ -/* wom::ModuleName _name; */ -/**/ -/* }; */ -/**/ -/* }; */ diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h index 30832cd5..2beb455c 100644 --- a/wombat/src/main/include/subsystems/Arm.h +++ b/wombat/src/main/include/subsystems/Arm.h @@ -1,75 +1,78 @@ -#pragma once +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project -#include "behaviour/HasBehaviour.h" -#include "utils/Encoder.h" -#include "utils/Gearbox.h" -#include "utils/PID.h" +#pragma once #include #include +#include #include #include -#include + +#include +#include + +#include "behaviour/HasBehaviour.h" +#include "utils/Encoder.h" +#include "utils/Gearbox.h" +#include "utils/PID.h" namespace wom { - struct ArmConfig { - std::string path; - - wom::Gearbox leftGearbox; - wom::Gearbox rightGearbox; - rev::SparkMaxRelativeEncoder armEncoder; - wom::PIDConfig pidConfig; - wom::PIDConfig velocityConfig; - - units::kilogram_t armMass; - units::kilogram_t loadMass; - units::meter_t armLength; - units::radian_t minAngle = 0_deg; - units::radian_t maxAngle = 180_deg; - units::radian_t initialAngle = 0_deg; - units::radian_t angleOffset = 0_deg; - - void WriteNT(std::shared_ptr table); - }; - - enum class ArmState { - kIdle, - kAngle, - kRaw, - kVelocity - }; - - class Arm : public behaviour::HasBehaviour { - public: - Arm(ArmConfig config); - - void OnUpdate(units::second_t dt); - - void SetIdle(); - void SetAngle(units::radian_t angle); - void SetRaw(units::volt_t voltage); - void SetVelocity(units::radians_per_second_t velocity); - - void SetArmSpeedLimit(double limit); //units, what are they?? - - ArmConfig &GetConfig(); - - units::radian_t GetAngle() const; - units::radians_per_second_t MaxSpeed() const; - units::radians_per_second_t GetArmVelocity() const; - - bool IsStable() const; - private: - ArmConfig _config; - ArmState _state = ArmState::kIdle; - wom::PIDController _pid; - wom::PIDController _velocityPID; - - std::shared_ptr _table; - - double armLimit = 0.4; - units::radians_per_second_t lastVelocity; - - units::volt_t _voltage{0}; - }; +struct ArmConfig { + std::string path; + + wom::Gearbox leftGearbox; + wom::Gearbox rightGearbox; + rev::SparkMaxRelativeEncoder armEncoder; + wom::PIDConfig pidConfig; + wom::PIDConfig velocityConfig; + + units::kilogram_t armMass; + units::kilogram_t loadMass; + units::meter_t armLength; + units::radian_t minAngle = 0_deg; + units::radian_t maxAngle = 180_deg; + units::radian_t initialAngle = 0_deg; + units::radian_t angleOffset = 0_deg; + + void WriteNT(std::shared_ptr table); +}; + +enum class ArmState { kIdle, kAngle, kRaw, kVelocity }; + +class Arm : public behaviour::HasBehaviour { + public: + explicit Arm(ArmConfig config); + + void OnUpdate(units::second_t dt); + + void SetIdle(); + void SetAngle(units::radian_t angle); + void SetRaw(units::volt_t voltage); + void SetVelocity(units::radians_per_second_t velocity); + + void SetArmSpeedLimit(double limit); // units, what are they?? + + ArmConfig& GetConfig(); + + units::radian_t GetAngle() const; + units::radians_per_second_t MaxSpeed() const; + units::radians_per_second_t GetArmVelocity() const; + + bool IsStable() const; + + private: + ArmConfig _config; + ArmState _state = ArmState::kIdle; + wom::PIDController _pid; + wom::PIDController _velocityPID; + + std::shared_ptr _table; + + double armLimit = 0.4; + units::radians_per_second_t lastVelocity; + + units::volt_t _voltage{0}; }; +} // namespace wom diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h index a1e6ef55..ab57a239 100644 --- a/wombat/src/main/include/subsystems/Elevator.h +++ b/wombat/src/main/include/subsystems/Elevator.h @@ -1,84 +1,84 @@ -#pragma once +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project -#include "utils/Gearbox.h" -#include "utils/PID.h" -#include "behaviour/HasBehaviour.h" -#include "behaviour/Behaviour.h" -#include -#include +#pragma once #include #include #include #include +#include +#include #include +#include + +#include "behaviour/Behaviour.h" +#include "behaviour/HasBehaviour.h" +#include "utils/Gearbox.h" +#include "utils/PID.h" namespace wom { - enum class ElevatorState { - kIdle, - kPID, - kManual, - kVelocity - }; - - struct ElevatorConfig { - std::string path; - wom::Gearbox leftGearbox; - wom::Gearbox rightGearbox; - rev::SparkMaxRelativeEncoder elevatorEncoder; - frc::DigitalInput *topSensor; - frc::DigitalInput *bottomSensor; - units::meter_t radius; - units::kilogram_t mass; - units::meter_t maxHeight; - units::meter_t minHeight; - units::meter_t initialHeight; - PIDConfig pid; - PIDConfig velocityPID; - - void WriteNT(std::shared_ptr table); - }; - - class Elevator : public behaviour::HasBehaviour { - public: - Elevator(ElevatorConfig params); - - void OnUpdate(units::second_t dt); - - void SetManual(units::volt_t voltage); - void SetPID(units::meter_t height); - void SetIdle(); - void SetRaw(); - - void SetVelocity(units::meters_per_second_t velocity); - - units::volt_t GetRaw(); - - double GetElevatorEncoderPos(); - void SetElevatorSpeedLimit(double limit); - - ElevatorConfig &GetConfig(); - - bool IsStable() const; - ElevatorState GetState() const; - - units::meter_t GetHeight() const; - units::meters_per_second_t MaxSpeed() const; - units::meters_per_second_t GetElevatorVelocity() const; - - private: - units::volt_t _setpointManual{0}; - - ElevatorConfig _config; - ElevatorState _state; - double speedLimit = 0.5; - - units::meters_per_second_t _velocity; - - PIDController _pid; - PIDController _velocityPID; - - std::shared_ptr _table; - }; +enum class ElevatorState { kIdle, kPID, kManual, kVelocity }; + +struct ElevatorConfig { + std::string path; + wom::Gearbox leftGearbox; + wom::Gearbox rightGearbox; + rev::SparkMaxRelativeEncoder elevatorEncoder; + frc::DigitalInput* topSensor; + frc::DigitalInput* bottomSensor; + units::meter_t radius; + units::kilogram_t mass; + units::meter_t maxHeight; + units::meter_t minHeight; + units::meter_t initialHeight; + PIDConfig pid; + PIDConfig velocityPID; + + void WriteNT(std::shared_ptr table); +}; + +class Elevator : public behaviour::HasBehaviour { + public: + explicit Elevator(ElevatorConfig params); + + void OnUpdate(units::second_t dt); + + void SetManual(units::volt_t voltage); + void SetPID(units::meter_t height); + void SetIdle(); + void SetRaw(); + + void SetVelocity(units::meters_per_second_t velocity); + + units::volt_t GetRaw(); + + double GetElevatorEncoderPos(); + void SetElevatorSpeedLimit(double limit); + + ElevatorConfig& GetConfig(); + + bool IsStable() const; + ElevatorState GetState() const; + + units::meter_t GetHeight() const; + units::meters_per_second_t MaxSpeed() const; + units::meters_per_second_t GetElevatorVelocity() const; + + private: + units::volt_t _setpointManual{0}; + + ElevatorConfig _config; + ElevatorState _state; + double speedLimit = 0.5; + + units::meters_per_second_t _velocity; + + PIDController _pid; + PIDController _velocityPID; + + std::shared_ptr _table; }; +} // namespace wom diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h index fe985074..a6a1701b 100644 --- a/wombat/src/main/include/subsystems/Shooter.h +++ b/wombat/src/main/include/subsystems/Shooter.h @@ -1,70 +1,74 @@ -#pragma once +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project -#include "utils/Gearbox.h" -#include "utils/PID.h" -#include "behaviour/HasBehaviour.h" -#include "behaviour/Behaviour.h" +#pragma once #include #include #include #include +#include + +#include "behaviour/Behaviour.h" +#include "behaviour/HasBehaviour.h" +#include "utils/Gearbox.h" +#include "utils/PID.h" namespace wom { - enum class ShooterState { - kPID, - kManual, - kIdle - }; - - struct ShooterParams { - Gearbox gearbox; - PIDConfig pid; - units::ampere_t currentLimit; - }; - - class Shooter : public behaviour::HasBehaviour { - public: - Shooter(std::string path, ShooterParams params); - - void SetManual(units::volt_t voltage); - void SetPID(units::radians_per_second_t goal); - void SetIdle(); - - void OnUpdate(units::second_t dt); - - bool IsStable() const; - - private: - ShooterParams _params; - ShooterState _state; - - units::volt_t _setpointManual{0}; - - PIDController _pid; - - std::shared_ptr _table; - }; - - class ShooterConstant : public behaviour::Behaviour { - public: - ShooterConstant(Shooter *s, units::volt_t setpoint); - - void OnTick(units::second_t dt) override; - private: - Shooter *_shooter; - units::volt_t _setpoint; - }; - - class ShooterSpinup : public behaviour::Behaviour { - public: - ShooterSpinup(Shooter *s, units::radians_per_second_t speed, bool hold = false); - - void OnTick(units::second_t dt) override; - private: - Shooter *_shooter; - units::radians_per_second_t _speed; - bool _hold; - }; -} +enum class ShooterState { kPID, kManual, kIdle }; + +struct ShooterParams { + Gearbox gearbox; + PIDConfig pid; + units::ampere_t currentLimit; +}; + +class Shooter : public behaviour::HasBehaviour { + public: + Shooter(std::string path, ShooterParams params); + + void SetManual(units::volt_t voltage); + void SetPID(units::radians_per_second_t goal); + void SetIdle(); + + void OnUpdate(units::second_t dt); + + bool IsStable() const; + + private: + ShooterParams _params; + ShooterState _state; + + units::volt_t _setpointManual{0}; + + PIDController _pid; + + std::shared_ptr _table; +}; + +class ShooterConstant : public behaviour::Behaviour { + public: + ShooterConstant(Shooter* s, units::volt_t setpoint); + + void OnTick(units::second_t dt) override; + + private: + Shooter* _shooter; + units::volt_t _setpoint; +}; + +class ShooterSpinup : public behaviour::Behaviour { + public: + ShooterSpinup(Shooter* s, units::radians_per_second_t speed, + bool hold = false); + + void OnTick(units::second_t dt) override; + + private: + Shooter* _shooter; + units::radians_per_second_t _speed; + bool _hold; +}; +} // namespace wom diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index 5deae642..3c4df556 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -1,112 +1,126 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once +#include +#include #include +#include #include #include -#include -#include -#include - #include "utils/Util.h" namespace wom { - class Encoder { - public: - Encoder(double encoderTicksPerRotation, double reduction, int type) : _encoderTicksPerRotation(encoderTicksPerRotation), _reduction(reduction), _type(type) {}; - virtual double GetEncoderRawTicks() const = 0; - virtual double GetEncoderTickVelocity() const = 0; // ticks/s - virtual void ZeroEncoder(); - - void SetEncoderPosition(units::degree_t position); - void SetEncoderOffset(units::radian_t offset); - - double GetEncoderTicks() const; - double GetEncoderTicksPerRotation() const; - - void SetReduction(double reduction); - - units::radian_t GetEncoderPosition(); - double GetEncoderDistance(); - units::radians_per_second_t GetEncoderAngularVelocity(); // rad/s - - int encoderType = 0; - double _reduction = 1.0; - private: - double _encoderTicksPerRotation; - units::radian_t _offset = 0_rad; - int _type = 0; - }; - - class DigitalEncoder : public Encoder { - public: - DigitalEncoder(int channelA, int channelB, double ticksPerRotation, double reduction = 1) - : Encoder(ticksPerRotation, reduction, 0), - _nativeEncoder(channelA, channelB){}; - - double GetEncoderRawTicks() const override; - double GetEncoderTickVelocity() const override; - private: - frc::Encoder _nativeEncoder; - }; - - class SimCANSparkMaxEncoder; - class CANSparkMaxEncoder : public Encoder { - public: - CANSparkMaxEncoder(rev::CANSparkMax *controller, double reduction = 1); - - double GetEncoderRawTicks() const override; - double GetEncoderTickVelocity() const override; - - protected: - rev::SparkMaxRelativeEncoder _encoder; - friend class SimCANSparkMaxEncoder; - }; - - class TalonFXEncoder : public Encoder { - public: - TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX *controller, double reduction = 1); - - double GetEncoderRawTicks() const override; - double GetEncoderTickVelocity() const override; - - private: - ctre::phoenix::motorcontrol::can::TalonFX *_controller; - }; - - class TalonSRXEncoder : public Encoder { - public: - TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX *controller, double ticksPerRotation, double reduction = 1); - - double GetEncoderRawTicks() const override; - double GetEncoderTickVelocity() const override; - - private: - ctre::phoenix::motorcontrol::can::TalonSRX *_controller; - }; - - class DutyCycleEncoder : public Encoder { - public: - DutyCycleEncoder(int channel, double ticksPerRotation = 1, double reduction = 1); - - double GetEncoderRawTicks() const override; - double GetEncoderTickVelocity() const override; - - private: - frc::DutyCycleEncoder _dutyCycleEncoder; - }; - - class CanEncoder : public Encoder { - public: - CanEncoder(int deviceNumber, double ticksPerRotation = 4095, double reduction = 1); - - double GetEncoderRawTicks() const override; - double GetEncoderTickVelocity() const override; - double GetAbsoluteEncoderPosition(); - - const double constantValue = 0.0; - - private: - CANCoder *_canEncoder; - }; +class Encoder { + public: + Encoder(double encoderTicksPerRotation, double reduction, int type) + : _encoderTicksPerRotation(encoderTicksPerRotation), + _reduction(reduction), + _type(type) {} + virtual double GetEncoderRawTicks() const = 0; + virtual double GetEncoderTickVelocity() const = 0; // ticks/s + virtual void ZeroEncoder(); + + void SetEncoderPosition(units::degree_t position); + void SetEncoderOffset(units::radian_t offset); + + double GetEncoderTicks() const; + double GetEncoderTicksPerRotation() const; + + void SetReduction(double reduction); + + units::radian_t GetEncoderPosition(); + double GetEncoderDistance(); + units::radians_per_second_t GetEncoderAngularVelocity(); // rad/s + + int encoderType = 0; + double _reduction = 1.0; + + private: + double _encoderTicksPerRotation; + units::radian_t _offset = 0_rad; + int _type = 0; +}; + +class DigitalEncoder : public Encoder { + public: + DigitalEncoder(int channelA, int channelB, double ticksPerRotation, + double reduction = 1) + : Encoder(ticksPerRotation, reduction, 0), + _nativeEncoder(channelA, channelB) {} + + double GetEncoderRawTicks() const override; + double GetEncoderTickVelocity() const override; + + private: + frc::Encoder _nativeEncoder; +}; + +class SimCANSparkMaxEncoder; +class CANSparkMaxEncoder : public Encoder { + public: + explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, + double reduction = 1); + + double GetEncoderRawTicks() const override; + double GetEncoderTickVelocity() const override; + + protected: + rev::SparkMaxRelativeEncoder _encoder; + friend class SimCANSparkMaxEncoder; +}; + +class TalonFXEncoder : public Encoder { + public: + TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX* controller, + double reduction = 1); + + double GetEncoderRawTicks() const override; + double GetEncoderTickVelocity() const override; + + private: + ctre::phoenix::motorcontrol::can::TalonFX* _controller; +}; + +class TalonSRXEncoder : public Encoder { + public: + TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX* controller, + double ticksPerRotation, double reduction = 1); + + double GetEncoderRawTicks() const override; + double GetEncoderTickVelocity() const override; + + private: + ctre::phoenix::motorcontrol::can::TalonSRX* _controller; +}; + +class DutyCycleEncoder : public Encoder { + public: + DutyCycleEncoder(int channel, double ticksPerRotation = 1, + double reduction = 1); + + double GetEncoderRawTicks() const override; + double GetEncoderTickVelocity() const override; + + private: + frc::DutyCycleEncoder _dutyCycleEncoder; +}; + +class CanEncoder : public Encoder { + public: + CanEncoder(int deviceNumber, double ticksPerRotation = 4095, + double reduction = 1); + + double GetEncoderRawTicks() const override; + double GetEncoderTickVelocity() const override; + double GetAbsoluteEncoderPosition(); + + const double constantValue = 0.0; + + private: + CANCoder* _canEncoder; +}; } // namespace wom diff --git a/wombat/src/main/include/utils/Gearbox.h b/wombat/src/main/include/utils/Gearbox.h index 5a18e828..abd82c5e 100644 --- a/wombat/src/main/include/utils/Gearbox.h +++ b/wombat/src/main/include/utils/Gearbox.h @@ -1,11 +1,16 @@ -#pragma once +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once -#include "utils/Encoder.h" -#include "VoltageController.h" #include +#include "VoltageController.h" +#include "utils/Encoder.h" + namespace wom { - using DCMotor = frc::DCMotor; +using DCMotor = frc::DCMotor; /** * Struct for motor and encoder pairs. * @@ -13,20 +18,19 @@ namespace wom { * so that both Spark + Encoder and Talon SRX are treated the same. */ struct Gearbox { - /** * The VoltageController (Motor Controller). May not be null. */ - VoltageController *transmission; + VoltageController* transmission; /** * The Encoder. May be null, depending on the consumer of this structure. */ - Encoder *encoder = nullptr; + Encoder* encoder = nullptr; /** * The motor being used. By default, this is a dual CIM. */ frc::DCMotor motor = frc::DCMotor::CIM(2); }; -} //ns wom +} // namespace wom diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h index 6f802034..060542de 100644 --- a/wombat/src/main/include/utils/PID.h +++ b/wombat/src/main/include/utils/PID.h @@ -1,171 +1,204 @@ -#pragma once - -#include "utils/Util.h" +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project -#include -#include +#pragma once #include #include +#include +#include +#include #include +#include #include -namespace wom { - template - struct PIDConfig { - using in_t = units::unit_t; - - using kp_t = units::unit_t>>; - using ki_t = units::unit_t, units::inverse>>; - using kd_t = units::unit_t, units::second>>; - - using error_t = units::unit_t; - using deriv_t = units::unit_t>>; +#include "utils/Util.h" - PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1}) - : path(path), kp(kp), ki(ki), kd(kd), stableThresh(stableThresh), stableDerivThresh(stableDerivThresh), izone(izone) { - RegisterNT(); +namespace wom { +template +struct PIDConfig { + using in_t = units::unit_t; + + using kp_t = units::unit_t>>; + using ki_t = + units::unit_t, + units::inverse>>; + using kd_t = units::unit_t< + units::compound_unit, units::second>>; + + using error_t = units::unit_t; + using deriv_t = + units::unit_t>>; + + PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, + kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1}, + deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1}) + : path(path), + kp(kp), + ki(ki), + kd(kd), + stableThresh(stableThresh), + stableDerivThresh(stableDerivThresh), + izone(izone) { + RegisterNT(); + } + + std::string path; + + kp_t kp; + ki_t ki{0}; + kd_t kd{0}; + + error_t stableThresh{-1}; + deriv_t stableDerivThresh{-1}; + + in_t izone{-1}; + + private: + std::vector> _nt_bindings; + + public: + void RegisterNT() { + auto table = nt::NetworkTableInstance::GetDefault().GetTable(path); + _nt_bindings.emplace_back( + std::make_shared>(table, "kP", + kp)); + _nt_bindings.emplace_back( + std::make_shared>(table, "kI", + ki)); + _nt_bindings.emplace_back( + std::make_shared>(table, "kD", + kd)); + _nt_bindings.emplace_back( + std::make_shared>( + table, "stableThresh", stableThresh)); + _nt_bindings.emplace_back( + std::make_shared>( + table, "stableThreshVelocity", stableDerivThresh)); + _nt_bindings.emplace_back( + std::make_shared>(table, "izone", izone)); + } +}; + +template +class PIDController { + public: + using config_t = PIDConfig; + using in_t = units::unit_t; + using out_t = units::unit_t; + using sum_t = units::unit_t>; + + config_t config; + + PIDController(std::string path, config_t initialGains, + in_t setpoint = in_t{0}) + : config(initialGains), + _setpoint(setpoint), + _posFilter( + frc::LinearFilter::MovingAverage(20)), + _velFilter( + frc::LinearFilter::MovingAverage(20)), + _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {} + + void SetSetpoint(in_t setpoint) { + if (std::abs(setpoint.value() - _setpoint.value()) > + std::abs(0.1 * _setpoint.value())) { + _iterations = 0; } + _setpoint = setpoint; + } - std::string path; - - kp_t kp; - ki_t ki{0}; - kd_t kd{0}; - - error_t stableThresh{-1}; - deriv_t stableDerivThresh{-1}; - - in_t izone{-1}; + in_t GetSetpoint() const { return _setpoint; } - private: - std::vector> _nt_bindings; + in_t GetError() const { return _last_error; } - public: - void RegisterNT() { - auto table = nt::NetworkTableInstance::GetDefault().GetTable(path); - _nt_bindings.emplace_back(std::make_shared>(table, "kP", kp)); - _nt_bindings.emplace_back(std::make_shared>(table, "kI", ki)); - _nt_bindings.emplace_back(std::make_shared>(table, "kD", kd)); - _nt_bindings.emplace_back(std::make_shared>(table, "stableThresh", stableThresh)); - _nt_bindings.emplace_back(std::make_shared>(table, "stableThreshVelocity", stableDerivThresh)); - _nt_bindings.emplace_back(std::make_shared>(table, "izone", izone)); - } - }; - - template - class PIDController { - public: - using config_t = PIDConfig; - using in_t = units::unit_t; - using out_t = units::unit_t; - using sum_t = units::unit_t>; - - config_t config; - - PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0}) - : config(initialGains), _setpoint(setpoint), - _posFilter(frc::LinearFilter::MovingAverage(20)), - _velFilter(frc::LinearFilter::MovingAverage(20)), - _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) { } - - void SetSetpoint(in_t setpoint) { - if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) { - _iterations = 0; - } - _setpoint = setpoint; - } - - in_t GetSetpoint() const { - return _setpoint; - } - - in_t GetError() const { - return _last_error; - } + void SetWrap(std::optional range) { _wrap_range = range; } - void SetWrap(std::optional range) { - _wrap_range = range; - } + void Reset() { _integralSum = sum_t{0}; } - void Reset() { + out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) { + auto error = do_wrap(_setpoint - pv); + _integralSum += error * dt; + if (config.izone.value() > 0 && + (error > config.izone || error < -config.izone)) _integralSum = sum_t{0}; - } - - out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) { - auto error = do_wrap(_setpoint - pv); - _integralSum += error * dt; - if (config.izone.value() > 0 && (error > config.izone || error < -config.izone)) - _integralSum = sum_t{0}; - - typename config_t::deriv_t deriv{0}; - - if (_iterations > 0) - deriv = (pv - _last_pv) / dt; - - _stablePos = _posFilter.Calculate(error); - _stableVel = _velFilter.Calculate(deriv); - - auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward; - // std::cout << "Out value" << out.value() << std::endl; - - _table->GetEntry("pv").SetDouble(pv.value()); - _table->GetEntry("dt").SetDouble(dt.value()); - _table->GetEntry("setpoint").SetDouble(_setpoint.value()); - _table->GetEntry("error").SetDouble(error.value()); - _table->GetEntry("integralSum").SetDouble(_integralSum.value()); - _table->GetEntry("stable").SetBoolean(IsStable()); - _table->GetEntry("demand").SetDouble(out.value()); - - _last_pv = pv; - _last_error = error; - _iterations++; - return out; - } - - bool IsStable(std::optional stableThreshOverride = {}, std::optional velocityThreshOverride = {}) const { - auto stableThresh = config.stableThresh; - auto stableDerivThresh = config.stableDerivThresh; - if (stableThreshOverride.has_value()) stableThresh = stableThreshOverride.value(); - if (velocityThreshOverride.has_value()) stableDerivThresh = velocityThreshOverride.value(); - - return _iterations > 20 - && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) - && (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value()); - } - - private: - in_t do_wrap(in_t val) { - if (_wrap_range.has_value()) { - double wr = _wrap_range.value().value(); - double v = val.value(); - - v = std::fmod(v, wr); - if (std::abs(v) > (wr / 2.0)) { - return in_t{(v > 0) ? v - wr : v + wr}; - } else { - return in_t{v}; - } + typename config_t::deriv_t deriv{0}; + + if (_iterations > 0) + deriv = (pv - _last_pv) / dt; + + _stablePos = _posFilter.Calculate(error); + _stableVel = _velFilter.Calculate(deriv); + + auto out = config.kp * error + config.ki * _integralSum + + config.kd * deriv + feedforward; + // std::cout << "Out value" << out.value() << std::endl; + + _table->GetEntry("pv").SetDouble(pv.value()); + _table->GetEntry("dt").SetDouble(dt.value()); + _table->GetEntry("setpoint").SetDouble(_setpoint.value()); + _table->GetEntry("error").SetDouble(error.value()); + _table->GetEntry("integralSum").SetDouble(_integralSum.value()); + _table->GetEntry("stable").SetBoolean(IsStable()); + _table->GetEntry("demand").SetDouble(out.value()); + + _last_pv = pv; + _last_error = error; + _iterations++; + return out; + } + + bool IsStable( + std::optional stableThreshOverride = {}, + std::optional velocityThreshOverride = {}) + const { + auto stableThresh = config.stableThresh; + auto stableDerivThresh = config.stableDerivThresh; + + if (stableThreshOverride.has_value()) + stableThresh = stableThreshOverride.value(); + if (velocityThreshOverride.has_value()) + stableDerivThresh = velocityThreshOverride.value(); + + return _iterations > 20 && + std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) && + (stableDerivThresh.value() < 0 || + std::abs(_stableVel.value()) <= stableDerivThresh.value()); + } + + private: + in_t do_wrap(in_t val) { + if (_wrap_range.has_value()) { + double wr = _wrap_range.value().value(); + double v = val.value(); + + v = std::fmod(v, wr); + if (std::abs(v) > (wr / 2.0)) { + return in_t{(v > 0) ? v - wr : v + wr}; + } else { + return in_t{v}; } - return val; } + return val; + } + + in_t _setpoint; + sum_t _integralSum{0}; + in_t _last_pv{0}, _last_error{0}; - in_t _setpoint; - sum_t _integralSum{0}; - in_t _last_pv{0}, _last_error{0}; + std::optional _wrap_range; - std::optional _wrap_range; - - int _iterations = 0; + int _iterations = 0; - frc::LinearFilter _posFilter; - frc::LinearFilter _velFilter; + frc::LinearFilter _posFilter; + frc::LinearFilter _velFilter; - typename config_t::error_t _stablePos; - typename config_t::deriv_t _stableVel; + typename config_t::error_t _stablePos; + typename config_t::deriv_t _stableVel; - std::shared_ptr _table; - }; -} + std::shared_ptr _table; +}; +} // namespace wom diff --git a/wombat/src/main/include/utils/RobotStartup.h b/wombat/src/main/include/utils/RobotStartup.h index c29d9131..46cda67e 100644 --- a/wombat/src/main/include/utils/RobotStartup.h +++ b/wombat/src/main/include/utils/RobotStartup.h @@ -1,12 +1,18 @@ -#pragma once +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + +#pragma once #include -#include + #include +#include + namespace wom { class RobotStartup { - public: + public: static void Start(std::function func); }; @@ -17,9 +23,12 @@ int StartRobot() { } #ifndef RUNNING_FRC_TESTS -#define WOMBAT_ROBOT_MAIN(RobotClz) int main() { wom::StartRobot(); } -#else +#define WOMBAT_ROBOT_MAIN(RobotClz) \ + int main() { \ + wom::StartRobot(); \ + } +#else #define WOMBAT_ROBOT_MAIN(RobotClz) #endif -} // ns wom +} // namespace wom diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h index 248c8d5f..308d1aad 100644 --- a/wombat/src/main/include/utils/Util.h +++ b/wombat/src/main/include/utils/Util.h @@ -1,77 +1,98 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include -#include - -// #include -// #include #include +#include +#include +#include #include #include -#include - -#include +#include +#include namespace wom { - template - T&& invert(T &&system) { - system.SetInverted(true); - return system; - } +template +T&& invert(T&& system) { + system.SetInverted(true); + return system; +} + +units::second_t now(); - units::second_t now(); +class NTBound { + public: + NTBound(std::shared_ptr table, std::string name, + const nt::Value& value, + std::function onUpdateFn) + : _table(table), + _entry(table->GetEntry(name)), + _onUpdate(onUpdateFn), + _name(name) { + _entry.SetValue(value); + // _listener = table->AddListener(name, , ([=](const nt::EntryNotification + // &evt) { + // this->_onUpdate(evt.value); + // }, NT_NOTIFY_UPDATE); + _listener = table->AddListener( + name, nt::EventFlags::kValueAll, + ([this](nt::NetworkTable* table, std::string_view key, + const nt::Event& event) { + std::cout << "NT UPDATE" << std::endl; + this->_onUpdate(event.GetValueEventData()->value); + })); + } - class NTBound { - public: - NTBound(std::shared_ptr table, std::string name, const nt::Value &value, std::function onUpdateFn) - : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) { - _entry.SetValue(value); - // _listener = table->AddListener(name, , ([=](const nt::EntryNotification &evt) { - // this->_onUpdate(evt.value); - // }, NT_NOTIFY_UPDATE); - _listener = table->AddListener(name, nt::EventFlags::kValueAll, ([this](nt::NetworkTable *table, std::string_view key, const nt::Event &event) { + NTBound(const NTBound& other) + : _table(other._table), + _entry(other._entry), + _onUpdate(other._onUpdate), + _name(other._name) { + _listener = _table->AddListener( + _name, nt::EventFlags::kValueAll, + ([this](nt::NetworkTable* table, std::string_view key, + const nt::Event& event) { std::cout << "NT UPDATE" << std::endl; this->_onUpdate(event.GetValueEventData()->value); })); - } - - NTBound(const NTBound &other) - : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) { - - _listener = _table->AddListener(_name, nt::EventFlags::kValueAll, ([this](nt::NetworkTable *table, std::string_view key, const nt::Event &event) { - std::cout << "NT UPDATE" << std::endl; - this->_onUpdate(event.GetValueEventData()->value); - })); - } - - ~NTBound() { - _table->RemoveListener(_listener); - } - protected: - NT_Listener _listener; - std::shared_ptr _table; - nt::NetworkTableEntry _entry; - std::function _onUpdate; - std::string _name; - }; + } - class NTBoundDouble : public NTBound { - public: - NTBoundDouble(std::shared_ptr table, std::string name, double &val) - : NTBound(table, name, nt::Value::MakeDouble(val), [&val](const nt::Value &v) { val = v.GetDouble(); }) {} - }; + ~NTBound() { _table->RemoveListener(_listener); } - template - class NTBoundUnit : public NTBound { - public: - NTBoundUnit(std::shared_ptr table, std::string name, units::unit_t &val) - : NTBound(table, name, nt::Value::MakeDouble(val.value()), [&val](const nt::Value &v) { val = units::unit_t { v.GetDouble() }; }) {} - }; + protected: + NT_Listener _listener; + std::shared_ptr _table; + nt::NetworkTableEntry _entry; + std::function _onUpdate; + std::string _name; +}; - void WritePose2NT(std::shared_ptr table, frc::Pose2d pose); - void WritePose3NT(std::shared_ptr table, frc::Pose3d pose); +class NTBoundDouble : public NTBound { + public: + NTBoundDouble(std::shared_ptr table, std::string name, + double& val) + : NTBound(table, name, nt::Value::MakeDouble(val), + [&val](const nt::Value& v) { val = v.GetDouble(); }) {} +}; - double deadzone(double val, double deadzone = 0.05); - double spow2(double val); -} +template +class NTBoundUnit : public NTBound { + public: + NTBoundUnit(std::shared_ptr table, std::string name, + units::unit_t& val) + : NTBound(table, name, nt::Value::MakeDouble(val.value()), + [&val](const nt::Value& v) { + val = units::unit_t{v.GetDouble()}; + }) {} +}; + +void WritePose2NT(std::shared_ptr table, frc::Pose2d pose); +void WritePose3NT(std::shared_ptr table, frc::Pose3d pose); + +double deadzone(double val, double deadzone = 0.05); +double spow2(double val); +} // namespace wom diff --git a/wombat/src/main/include/utils/VoltageController.h b/wombat/src/main/include/utils/VoltageController.h index dcdef728..bb6358e7 100644 --- a/wombat/src/main/include/utils/VoltageController.h +++ b/wombat/src/main/include/utils/VoltageController.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #pragma once #include @@ -7,68 +11,70 @@ #include "utils/Util.h" namespace wom { +/** + * A VoltageController is analagous to a MotorController, but in terms of + * voltage instead of speed. + */ +class VoltageController { + public: /** - * A VoltageController is analagous to a MotorController, but in terms of voltage instead - * of speed. + * Set the voltage of the output. */ - class VoltageController { - public: - /** - * Set the voltage of the output. - */ - virtual void SetVoltage(units::volt_t voltage) = 0; - /** - * Get the voltage of the output. - */ - virtual units::volt_t GetVoltage() const = 0; - - /** - * Set the output as inverted. - */ - virtual void SetInverted(bool invert) = 0; - /** - * Get whether the output is inverted - */ - virtual bool GetInverted() const = 0; + virtual void SetVoltage(units::volt_t voltage) = 0; + /** + * Get the voltage of the output. + */ + virtual units::volt_t GetVoltage() const = 0; - /** - * Get the estimated real voltage of the output, based on the controller voltage. - */ - units::volt_t GetEstimatedRealVoltage() const; - }; + /** + * Set the output as inverted. + */ + virtual void SetInverted(bool invert) = 0; + /** + * Get whether the output is inverted + */ + virtual bool GetInverted() const = 0; /** - * The MotorVoltageController is an adapter for an frc::MotorController to - * a VoltageController. + * Get the estimated real voltage of the output, based on the controller + * voltage. */ - class MotorVoltageController : public VoltageController { - public: - MotorVoltageController(frc::MotorController *MotorController); + units::volt_t GetEstimatedRealVoltage() const; +}; + +/** + * The MotorVoltageController is an adapter for an frc::MotorController to + * a VoltageController. + */ +class MotorVoltageController : public VoltageController { + public: + explicit MotorVoltageController(frc::MotorController* MotorController); - void SetVoltage(units::volt_t voltage) override; - units::volt_t GetVoltage() const override; + void SetVoltage(units::volt_t voltage) override; + units::volt_t GetVoltage() const override; - void SetInverted(bool invert) override; - bool GetInverted() const override; + void SetInverted(bool invert) override; + bool GetInverted() const override; - units::volt_t GetBusVoltage() const; + units::volt_t GetBusVoltage() const; - /** - * Create a MotorVoltageController with a given frc::MotorController - * subclass. Please note that this creates an unsafe pointer (will never dealloc) - */ - template - static MotorVoltageController Of(Args& ...args) { - T *t = new T(args...); // Be warned, does not deallocate! - return MotorVoltageController{t}; - } + /** + * Create a MotorVoltageController with a given frc::MotorController + * subclass. Please note that this creates an unsafe pointer (will never + * dealloc) + */ + template + static MotorVoltageController Of(Args&... args) { + T* t = new T(args...); // Be warned, does not deallocate! + return MotorVoltageController{t}; + } - template - static MotorVoltageController Group(Args& ...args) { - return Of(args...); - } + template + static MotorVoltageController Group(Args&... args) { + return Of(args...); + } - private: - frc::MotorController *_MotorController; - }; + private: + frc::MotorController* _MotorController; +}; } // namespace wom diff --git a/wombat/src/test/cpp/main.cpp b/wombat/src/test/cpp/main.cpp index b8b23d23..c7052d44 100644 --- a/wombat/src/test/cpp/main.cpp +++ b/wombat/src/test/cpp/main.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include #include "gtest/gtest.h" diff --git a/wombat/src/test/cpp/test_behaviours.cpp b/wombat/src/test/cpp/test_behaviours.cpp index f0ad5371..c4a7826d 100644 --- a/wombat/src/test/cpp/test_behaviours.cpp +++ b/wombat/src/test/cpp/test_behaviours.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023 CurtinFRC +// Open Source Software, you can modify it according to the terms +// of the MIT License at the root of this project + #include #include @@ -16,7 +20,7 @@ class MockBehaviour : public Behaviour { MOCK_METHOD1(OnTick, void(units::time::second_t)); }; -TEST(Behaviour, Tick) { +TEST(BehaviourTest, Tick) { auto b = make(); { @@ -35,7 +39,7 @@ TEST(Behaviour, Tick) { EXPECT_EQ(b->GetBehaviourState(), BehaviourState::DONE); } -TEST(Behaviour, Interrupt) { +TEST(BehaviourTest, Interrupt) { auto b = make(); { @@ -52,7 +56,7 @@ TEST(Behaviour, Interrupt) { EXPECT_EQ(b->GetBehaviourState(), BehaviourState::INTERRUPTED); } -TEST(Behaviour, Timeout) { +TEST(BehaviourTest, Timeout) { auto b = make(); b->WithTimeout(10_ms); @@ -70,9 +74,9 @@ TEST(Behaviour, Timeout) { EXPECT_TRUE(b->Tick()); } -TEST(SequentialBehaviour, InheritsControls) { +TEST(SequentialBehaviourTest, InheritsControls) { HasBehaviour a, b; - auto b1 = make(), b2 = make(); + auto b1 = make(), b2 = make(); b1->Controls(&a); b2->Controls(&a); b2->Controls(&b); @@ -82,9 +86,9 @@ TEST(SequentialBehaviour, InheritsControls) { ASSERT_EQ(chain->GetControlled().count(&b), 1); } -TEST(SequentialBehaviour, Sequence) { - auto b1 = make(), b2 = make(), b3 = make(), - b4 = make(); +TEST(SequentialBehaviourTest, Sequence) { + auto b1 = make(), b2 = make(), + b3 = make(), b4 = make(); auto chain = b1 << b2 << b3 << b4; { @@ -114,9 +118,10 @@ TEST(SequentialBehaviour, Sequence) { ASSERT_EQ(b4->GetBehaviourState(), BehaviourState::INTERRUPTED); } -TEST(ConcurrentBehaviour, InheritsControls) { +TEST(ConcurrentBehaviourTest, InheritsControls) { HasBehaviour a, b; - auto b1 = make(), b2 = make(), b3 = make(); + auto b1 = make(), b2 = make(), + b3 = make(); b1->Controls(&a); b2->Controls(&b); b3->Controls(&a); @@ -128,7 +133,7 @@ TEST(ConcurrentBehaviour, InheritsControls) { EXPECT_THROW(b1 | b3, DuplicateControlException); } -TEST(ConcurrentBehaviour, Race) { +TEST(ConcurrentBehaviourTest, Race) { auto b1 = make(), b2 = make(); b1->SetPeriod(20_ms); b2->SetPeriod(10_ms); @@ -152,7 +157,7 @@ TEST(ConcurrentBehaviour, Race) { EXPECT_EQ(b2->GetBehaviourState(), BehaviourState::INTERRUPTED); } -TEST(ConcurrentBehaviour, All) { +TEST(ConcurrentBehaviourTest, All) { auto b1 = make(), b2 = make(); b1->SetPeriod(20_ms); b2->SetPeriod(10_ms); @@ -178,7 +183,7 @@ TEST(ConcurrentBehaviour, All) { EXPECT_EQ(b2->GetBehaviourState(), BehaviourState::DONE); } -TEST(ConcurrentBehaviour, Until) { +TEST(ConcurrentBehaviourTest, Until) { auto b1 = make(), b2 = make(); auto chain = b1->Until(b2); @@ -194,7 +199,7 @@ TEST(ConcurrentBehaviour, Until) { ASSERT_TRUE(b1->IsRunning()); ASSERT_TRUE(b2->IsRunning()); - // TODO: Need to add a way to have SetDone interrupt the wait on + // TODO: Need to add a way to have SetDone interrupt the wait on // the thread. E.g. SleepOrUntilDone b2->SetDone(); std::this_thread::sleep_for(std::chrono::milliseconds(30)); @@ -203,7 +208,7 @@ TEST(ConcurrentBehaviour, Until) { ASSERT_FALSE(b2->IsRunning()); } -TEST(WaitFor, Waits) { +TEST(WaitForTest, Waits) { bool v = false; auto b = make([&v]() { return v; }); @@ -214,7 +219,7 @@ TEST(WaitFor, Waits) { ASSERT_EQ(b->GetBehaviourState(), BehaviourState::DONE); } -TEST(WaitTime, Waits) { +TEST(WaitTimeTest, Waits) { auto b = make(20_ms); ASSERT_FALSE(b->Tick()); @@ -225,7 +230,7 @@ TEST(WaitTime, Waits) { ASSERT_EQ(b->GetBehaviourState(), BehaviourState::DONE); } -TEST(If, Then) { +TEST(IfTest, Then) { auto b1 = make(), b2 = make(); auto chain = make(true)->Then(b1)->Else(b2); @@ -237,7 +242,7 @@ TEST(If, Then) { chain->Tick(); } -TEST(If, Else) { +TEST(IfTest, Else) { auto b1 = make(), b2 = make(); auto chain = make(false)->Then(b1)->Else(b2); @@ -249,8 +254,9 @@ TEST(If, Else) { chain->Tick(); } -TEST(Switch, Int) { - auto b1 = make(), b2 = make(), b3 = make(); +TEST(SwitchTest, Int) { + auto b1 = make(), b2 = make(), + b3 = make(); auto chain = make>(1)->When(0, b1)->When(1, b2)->When(2, b3); @@ -261,8 +267,9 @@ TEST(Switch, Int) { chain->Tick(); } -TEST(Switch, Decide) { - auto b1 = make(), b2 = make(), b3 = make(); +TEST(SwitchTest, Decide) { + auto b1 = make(), b2 = make(), + b3 = make(); auto chain = make() ->When([]() { return true; }, b1) @@ -276,11 +283,11 @@ TEST(Switch, Decide) { chain->Tick(); } -TEST(Behaviour, FullChain) { +TEST(BehaviourTest, FullChain) { BehaviourScheduler s; - MockSystem a, b; - auto b1 = make(), b2 = make(), b3 = make(), - b4 = make(); + MockSystem a, b; + auto b1 = make(), b2 = make(), + b3 = make(), b4 = make(); b1->Controls(&a); b2->Controls(&b); @@ -364,4 +371,3 @@ TEST(Behaviour, FullChain) { ASSERT_EQ(a.GetActiveBehaviour(), nullptr); ASSERT_EQ(b.GetActiveBehaviour(), nullptr); } - diff --git a/wombat/vendordeps/Phoenix.json b/wombat/vendordeps/Phoenix.json index 4bd44248..f13d4d5d 100644 --- a/wombat/vendordeps/Phoenix.json +++ b/wombat/vendordeps/Phoenix.json @@ -420,4 +420,4 @@ "simMode": "swsim" } ] -} \ No newline at end of file +} diff --git a/wombat/vendordeps/REVLib.json b/wombat/vendordeps/REVLib.json index 989b8e64..3a88ead2 100644 --- a/wombat/vendordeps/REVLib.json +++ b/wombat/vendordeps/REVLib.json @@ -70,4 +70,4 @@ ] } ] -} \ No newline at end of file +}