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 5a1b8673..375771e7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,11 +29,6 @@ jobs: - name: Install Roborio Toolchain run: ./gradlew installRoborioToolchain - # Execute check for editor config - - name: Editor Config Check - run: ./gradlew editorconfigCheck - # 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..cba3cc1b --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,28 @@ +name: Lint and Format + +on: + pull_request: + push: + +jobs: + wpiformat: + 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 master origin/master; + - 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 diff --git a/.styleguide b/.styleguide new file mode 100644 index 00000000..be156d3c --- /dev/null +++ b/.styleguide @@ -0,0 +1,18 @@ + +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..6de02839 --- /dev/null +++ b/.styleguide-license @@ -0,0 +1,3 @@ +// Copyright (c) 2023-2024 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 da5af00b..8efe01d7 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 are on our programming team it is recomended that you also read the contributing guide for Wombat. This guide assumes you have already read the README. +# 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 are on our programming team it is recomended that you also read the contributing guide for Wombat. This guide assumes you have already read the README. ## 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,13 +14,13 @@ 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. This will include formatting, tests and build checks. Your pull request will automatically request a review from relevant teams. ## 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. ## Code Formatting -We have strict formatting rules to keep code consistent. We use the [WPILib styleguide](https://github.com/wpilibsuite/styleguide). In particular this means we use wpiformat. You can see [here](https://github.com/wpilibsuite/styleguide/blob/main/wpiformat/README.rst) how to install it. Before opening a pull request make sure to run wpiformat on the codebase. \ No newline at end of file +We have strict formatting rules to keep code consistent. We use the [WPILib styleguide](https://github.com/wpilibsuite/styleguide). In particular this means we use wpiformat. You can see [here](https://github.com/wpilibsuite/styleguide/blob/main/wpiformat/README.rst) how to install it. Before opening a pull request make sure to run wpiformat on the codebase. diff --git a/LICENSE.md b/LICENSE.md index 4e8604fe..a2382ada 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -1,6 +1,6 @@ # MIT License -Copyright (c) 2024 CurtinFRC +Copyright (c) 2023-2024 CurtinFRC Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -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/build.gradle b/build.gradle index 1cb70113..40a79417 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) @@ -122,4 +113,4 @@ subprojects { wrapper { gradleVersion = '8.5' distributionType = Wrapper.DistributionType.BIN -} +} \ No newline at end of file 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/init.sh b/init.sh index 775068ee..6a932ed6 100755 --- a/init.sh +++ b/init.sh @@ -3,4 +3,3 @@ sudo apt update chmod +x gradlew ./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 33e1ec61..e27c9c50 100644 --- a/src/main/cpp/Main.cpp +++ b/src/main/cpp/Main.cpp @@ -1,10 +1,13 @@ +// Copyright (c) 2023-2024 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 */ -#include "Wombat.h" #include "Robot.h" +#include "Wombat.h" WOMBAT_ROBOT_MAIN(Robot); - diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 8a402a6a..244353d4 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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..a564a3b7 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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..6598c5b6 100644 --- a/src/test/cpp/main.cpp +++ b/src/test/cpp/main.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 4bd44248..f13d4d5d 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -420,4 +420,4 @@ "simMode": "swsim" } ] -} \ No newline at end of file +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 989b8e64..3a88ead2 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -70,4 +70,4 @@ ] } ] -} \ No newline at end of file +} 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/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..0acd378e 100644 --- a/wombat/src/main/cpp/behaviour/Behaviour.cpp +++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp @@ -1,12 +1,19 @@ +// Copyright (c) 2023-2024 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..bba7681d 100644 --- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp +++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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..964522c6 100644 --- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp +++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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 40bfb694..b13cdc02 100644 --- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp +++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp @@ -1,26 +1,37 @@ +// Copyright (c) 2023-2024 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 frc; using namespace units; -wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig *config, XboxController &driver): _config(config), _driver(driver) {} +wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, + XboxController& driver) + : _config(config), _driver(driver) {} wom::drivetrain::Drivetrain::~Drivetrain() {} -wom::drivetrain::DrivetrainConfig *wom::drivetrain::Drivetrain::GetConfig() { return _config; } -wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() { return _state; } +wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() { + return _config; +} +wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() { + return _state; +} -void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) { _state = state; } +void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) { + _state = state; +} void wom::drivetrain::Drivetrain::OnStart() { std::cout << "Starting Tank" << std::endl; } void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) { - switch(_state) { + switch (_state) { case DrivetrainState::kIdle: break; - case DrivetrainState::kTank: - { + case DrivetrainState::kTank: { double rightSpeed = wom::utils::deadzone(_driver.GetRightY()); double leftSpeed = wom::utils::deadzone(_driver.GetLeftY()); _config->left1.transmission->SetVoltage(leftSpeed * maxVolts); @@ -30,7 +41,7 @@ void wom::drivetrain::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 896baa98..ee0bab61 100644 --- a/wombat/src/main/cpp/subsystems/Arm.cpp +++ b/wombat/src/main/cpp/subsystems/Arm.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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,56 +9,64 @@ using namespace frc; using namespace wom; -//creates network table instatnce on shuffleboard -void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr table) { +// creates network table instatnce on shuffleboard +void wom::subsystems::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 wom::subsystems::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 wom::subsystems::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; @@ -63,10 +75,13 @@ void wom::subsystems::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); @@ -76,7 +91,7 @@ void wom::subsystems::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")); } @@ -85,7 +100,8 @@ void wom::subsystems::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 wom::subsystems::Arm::SetIdle() { _state = ArmState::kIdle; @@ -106,7 +122,7 @@ void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) { _velocityPID.SetSetpoint(velocity); } -wom::subsystems::ArmConfig &wom::subsystems::Arm::GetConfig() { +wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() { return _config; } @@ -126,7 +142,6 @@ bool wom::subsystems::Arm::IsStable() const { return _pid.IsStable(5_deg); } - /* SIMULATION */ // #include @@ -134,9 +149,12 @@ bool wom::subsystems::Arm::IsStable() const { // : 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 { @@ -144,9 +162,12 @@ bool wom::subsystems::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; @@ -164,7 +185,8 @@ bool wom::subsystems::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 2da63b0c..b3765646 100644 --- a/wombat/src/main/cpp/subsystems/Elevator.cpp +++ b/wombat/src/main/cpp/subsystems/Elevator.cpp @@ -1,20 +1,27 @@ +// Copyright (c) 2023-2024 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 -void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr table) { +void wom::subsystems::ElevatorConfig::WriteNT( + std::shared_ptr table) { table->GetEntry("radius").SetDouble(radius.value()); table->GetEntry("mass").SetDouble(mass.value()); table->GetEntry("maxHeight").SetDouble(maxHeight.value()); } wom::subsystems::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 wom::subsystems::Elevator::OnUpdate(units::second_t dt) { @@ -22,44 +29,47 @@ void wom::subsystems::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; // } // } @@ -67,7 +77,8 @@ void wom::subsystems::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; // } // } @@ -94,7 +105,8 @@ void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) { speedLimit = limit; } -void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) { +void wom::subsystems::Elevator::SetVelocity( + units::meters_per_second_t velocity) { _velocityPID.SetSetpoint(velocity); _state = ElevatorState::kVelocity; } @@ -103,7 +115,7 @@ void wom::subsystems::Elevator::SetIdle() { _state = ElevatorState::kIdle; } -wom::subsystems::ElevatorConfig &wom::subsystems::Elevator::GetConfig() { +wom::subsystems::ElevatorConfig& wom::subsystems::Elevator::GetConfig() { return _config; } @@ -116,19 +128,25 @@ wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const { } double wom::subsystems::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 wom::subsystems::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 wom::subsystems::Elevator::GetElevatorVelocity() const { - return _config.elevatorEncoder.GetVelocity() / 60_s * 14/60 * 2 * 3.1415 * 0.02225 * 1_m; +units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() + const { + return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * + 0.02225 * 1_m; } units::meters_per_second_t wom::subsystems::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 e829cc9a..97fbfa18 100644 --- a/wombat/src/main/cpp/subsystems/Shooter.cpp +++ b/wombat/src/main/cpp/subsystems/Shooter.cpp @@ -1,48 +1,56 @@ +// Copyright (c) 2023-2024 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 wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params) - : _params(params), _state(ShooterState::kIdle), - _pid{path + "/pid", params.pid}, - _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {} + : _params(params), + _state(ShooterState::kIdle), + _pid{path + "/pid", params.pid}, + _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {} void wom::subsystems::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 wom::subsystems::Shooter::SetManual(units::volt_t voltage) { _state = ShooterState::kManual; _setpointManual = voltage; - } void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) { @@ -58,12 +66,13 @@ bool wom::subsystems::Shooter::IsStable() const { return _pid.IsStable(); } -//Shooter Manual Set +// Shooter Manual Set -wom::subsystems::ShooterConstant::ShooterConstant(Shooter *s, units::volt_t setpoint) - : _shooter(s), _setpoint(setpoint) { - Controls(_shooter); - } +wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, + units::volt_t setpoint) + : _shooter(s), _setpoint(setpoint) { + Controls(_shooter); +} void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) { _shooter->SetManual(_setpoint); @@ -71,8 +80,10 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) { // ShooterSpinup -wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter *s, units::radians_per_second_t speed, bool hold) - : _shooter(s), _speed(speed), _hold(hold) { +wom::subsystems::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 63711841..9403c5ff 100644 --- a/wombat/src/main/cpp/utils/Encoder.cpp +++ b/wombat/src/main/cpp/utils/Encoder.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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" double wom::utils::Encoder::GetEncoderTicks() const { @@ -49,8 +53,10 @@ double wom::utils::Encoder::GetEncoderDistance() { } units::radians_per_second_t wom::utils::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; } @@ -62,8 +68,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const { return _nativeEncoder.GetRate(); } -wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax *controller, double reduction) - : wom::utils::Encoder(42, reduction, 2), _encoder(controller->GetEncoder()) {} +wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, + double reduction) + : wom::utils::Encoder(42, reduction, 2), + _encoder(controller->GetEncoder()) {} double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const { return _encoder.GetPosition() * _reduction; @@ -73,10 +81,12 @@ double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const { return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60; } -wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix::motorcontrol::can::TalonFX *controller, double reduction) - : wom::utils::Encoder(2048, reduction, 0), _controller(controller) { - controller->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::TalonFXFeedbackDevice::IntegratedSensor); - } +wom::utils::TalonFXEncoder::TalonFXEncoder( + ctre::phoenix::motorcontrol::can::TalonFX* controller, double reduction) + : wom::utils::Encoder(2048, reduction, 0), _controller(controller) { + controller->ConfigSelectedFeedbackSensor( + ctre::phoenix::motorcontrol::TalonFXFeedbackDevice::IntegratedSensor); +} double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const { return _controller->GetSelectedSensorPosition(); @@ -86,11 +96,14 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const { return _controller->GetSelectedSensorVelocity() * 10; } - -wom::utils::TalonSRXEncoder::TalonSRXEncoder(ctre::phoenix::motorcontrol::can::TalonSRX *controller, double ticksPerRotation, double reduction) - : wom::utils::Encoder(ticksPerRotation, reduction, 0), _controller(controller) { - controller->ConfigSelectedFeedbackSensor(ctre::phoenix::motorcontrol::TalonSRXFeedbackDevice::QuadEncoder); - } +wom::utils::TalonSRXEncoder::TalonSRXEncoder( + ctre::phoenix::motorcontrol::can::TalonSRX* controller, + double ticksPerRotation, double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, 0), + _controller(controller) { + controller->ConfigSelectedFeedbackSensor( + ctre::phoenix::motorcontrol::TalonSRXFeedbackDevice::QuadEncoder); +} double wom::utils::TalonSRXEncoder::GetEncoderRawTicks() const { return _controller->GetSelectedSensorPosition(); @@ -100,8 +113,11 @@ double wom::utils::TalonSRXEncoder::GetEncoderTickVelocity() const { return _controller->GetSelectedSensorVelocity() * 10; } -wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, double ticksPerRotation, double reduction) - : wom::utils::Encoder(ticksPerRotation, reduction, 0), _dutyCycleEncoder(channel) {} +wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, + double ticksPerRotation, + double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, 0), + _dutyCycleEncoder(channel) {} double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const { return _dutyCycleEncoder.Get().value(); @@ -111,10 +127,11 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const { return 0; } -wom::utils::CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation, double reduction) - : wom::utils::Encoder(ticksPerRotation, reduction, 1) { - _canEncoder = new CANCoder(deviceNumber, "Drivebase"); - } +wom::utils::CanEncoder::CanEncoder(int deviceNumber, double ticksPerRotation, + double reduction) + : wom::utils::Encoder(ticksPerRotation, reduction, 1) { + _canEncoder = new CANCoder(deviceNumber, "Drivebase"); +} double wom::utils::CanEncoder::GetEncoderRawTicks() const { return _canEncoder->GetAbsolutePosition(); @@ -123,4 +140,3 @@ double wom::utils::CanEncoder::GetEncoderRawTicks() const { double wom::utils::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 e8253f47..533d98ca 100644 --- a/wombat/src/main/cpp/utils/RobotStartup.cpp +++ b/wombat/src/main/cpp/utils/RobotStartup.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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::utils::RobotStartup::Start(std::function robotFunc) { diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp index d69cec7b..3d3e6e66 100644 --- a/wombat/src/main/cpp/utils/Util.cpp +++ b/wombat/src/main/cpp/utils/Util.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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::utils::now() { @@ -5,25 +9,27 @@ units::second_t wom::utils::now() { return static_cast(now) / 1000000 * 1_s; } -void wom::utils::WritePose2NT(std::shared_ptr table, frc::Pose2d pose) { +void wom::utils::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::utils::WritePose3NT(std::shared_ptr table, frc::Pose3d pose) { +void wom::utils::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::utils::deadzone(double val, double deadzone) { - return std::fabs(val) > deadzone ? val : 0; + return std::fabs(val) > deadzone ? val : 0; } double wom::utils::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 710971ed..adac944d 100644 --- a/wombat/src/main/cpp/utils/VoltageController.cpp +++ b/wombat/src/main/cpp/utils/VoltageController.cpp @@ -1,5 +1,8 @@ +// Copyright (c) 2023-2024 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 @@ -8,8 +11,9 @@ units::volt_t wom::utils::VoltageController::GetEstimatedRealVoltage() const { return units::math::min(units::math::max(-vb, GetVoltage()), vb); } -wom::utils::VoltageController::VoltageController(frc::MotorController *MotorController) : _MotorController(MotorController) -{} +wom::utils::VoltageController::VoltageController( + frc::MotorController* MotorController) + : _MotorController(MotorController) {} void wom::utils::VoltageController::SetVoltage(units::volt_t voltage) { _MotorController->Set(voltage / GetBusVoltage()); diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h index 563b784b..2f419b1a 100644 --- a/wombat/src/main/include/Wombat.h +++ b/wombat/src/main/include/Wombat.h @@ -1,26 +1,27 @@ +// Copyright (c) 2023-2024 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 "behaviour/Behaviour.h" -#include "behaviour/HasBehaviour.h" #include "behaviour/BehaviourScheduler.h" - -#include "utils/PID.h" -#include "utils/Util.h" +#include "behaviour/HasBehaviour.h" +#include "drivetrain/Drivetrain.h" +#include "subsystems/Arm.h" +#include "subsystems/Elevator.h" +#include "subsystems/Shooter.h" #include "utils/Encoder.h" #include "utils/Gearbox.h" +#include "utils/PID.h" #include "utils/RobotStartup.h" +#include "utils/Util.h" #include "utils/VoltageController.h" -#include "subsystems/Arm.h" -#include "subsystems/Shooter.h" -#include "subsystems/Elevator.h" - -#include "drivetrain/Drivetrain.h" - namespace wom { - using namespace wom; - using namespace wom::utils; - using namespace wom::subsystems; - using namespace wom::drivetrain; - using namespace behaviour; -} +using namespace wom; +using namespace wom::utils; +using namespace wom::subsystems; +using namespace wom::drivetrain; +using namespace behaviour; +} // namespace wom diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h index c467171b..0cca45d4 100644 --- a/wombat/src/main/include/behaviour/Behaviour.h +++ b/wombat/src/main/include/behaviour/Behaviour.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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..d3468919 100644 --- a/wombat/src/main/include/behaviour/BehaviourScheduler.h +++ b/wombat/src/main/include/behaviour/BehaviourScheduler.h @@ -1,6 +1,11 @@ +// Copyright (c) 2023-2024 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..c9b35b2e 100644 --- a/wombat/src/main/include/behaviour/HasBehaviour.h +++ b/wombat/src/main/include/behaviour/HasBehaviour.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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 1e8036e1..f803f7ef 100644 --- a/wombat/src/main/include/drivetrain/Drivetrain.h +++ b/wombat/src/main/include/drivetrain/Drivetrain.h @@ -1,55 +1,58 @@ +// Copyright (c) 2023-2024 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 { namespace drivetrain { - // TODO PID - struct DrivetrainConfig { - std::string path; - - wom::utils::Gearbox left1; - wom::utils::Gearbox left2; - wom::utils::Gearbox left3; - wom::utils::Gearbox right1; - wom::utils::Gearbox right2; - wom::utils::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::utils::Gearbox left1; + wom::utils::Gearbox left2; + wom::utils::Gearbox left3; + wom::utils::Gearbox right1; + wom::utils::Gearbox right2; + wom::utils::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 drivetrain +} // 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 d79fb0cc..7adc23dc 100644 --- a/wombat/src/main/include/subsystems/Arm.h +++ b/wombat/src/main/include/subsystems/Arm.h @@ -1,77 +1,81 @@ -#pragma once +// Copyright (c) 2023-2024 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 { namespace subsystems { - struct ArmConfig { - std::string path; - - wom::utils::Gearbox leftGearbox; - wom::utils::Gearbox rightGearbox; - rev::SparkMaxRelativeEncoder armEncoder; - wom::utils::PIDConfig pidConfig; - wom::utils::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::utils::PIDController _pid; - wom::utils::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::utils::Gearbox leftGearbox; + wom::utils::Gearbox rightGearbox; + rev::SparkMaxRelativeEncoder armEncoder; + wom::utils::PIDConfig pidConfig; + wom::utils::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::utils::PIDController _pid; + wom::utils::PIDController + _velocityPID; + + std::shared_ptr _table; + + double armLimit = 0.4; + units::radians_per_second_t lastVelocity; + + units::volt_t _voltage{0}; }; +} // namespace subsystems +} // namespace wom diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h index 90a2b86d..f7d01fc6 100644 --- a/wombat/src/main/include/subsystems/Elevator.h +++ b/wombat/src/main/include/subsystems/Elevator.h @@ -1,86 +1,86 @@ -#pragma once +// Copyright (c) 2023-2024 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 { namespace subsystems { - enum class ElevatorState { - kIdle, - kPID, - kManual, - kVelocity - }; - - struct ElevatorConfig { - std::string path; - wom::utils::Gearbox leftGearbox; - wom::utils::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; - wom::utils::PIDConfig pid; - wom::utils::PIDConfig velocityPID; - - void WriteNT(std::shared_ptr table); - }; +enum class ElevatorState { kIdle, kPID, kManual, kVelocity }; + +struct ElevatorConfig { + std::string path; + wom::utils::Gearbox leftGearbox; + wom::utils::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; + wom::utils::PIDConfig pid; + wom::utils::PIDConfig velocityPID; + + void WriteNT(std::shared_ptr table); +}; - class Elevator : public behaviour::HasBehaviour { - public: - Elevator(ElevatorConfig params); +class Elevator : public behaviour::HasBehaviour { + public: + explicit Elevator(ElevatorConfig params); - void OnUpdate(units::second_t dt); + void OnUpdate(units::second_t dt); - void SetManual(units::volt_t voltage); - void SetPID(units::meter_t height); - void SetIdle(); - void SetRaw(); + void SetManual(units::volt_t voltage); + void SetPID(units::meter_t height); + void SetIdle(); + void SetRaw(); - void SetVelocity(units::meters_per_second_t velocity); + void SetVelocity(units::meters_per_second_t velocity); - units::volt_t GetRaw(); + units::volt_t GetRaw(); - double GetElevatorEncoderPos(); - void SetElevatorSpeedLimit(double limit); + double GetElevatorEncoderPos(); + void SetElevatorSpeedLimit(double limit); - ElevatorConfig &GetConfig(); + ElevatorConfig& GetConfig(); - bool IsStable() const; - ElevatorState GetState() const; + 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; + 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}; + private: + units::volt_t _setpointManual{0}; - ElevatorConfig _config; - ElevatorState _state; - double speedLimit = 0.5; + ElevatorConfig _config; + ElevatorState _state; + double speedLimit = 0.5; - units::meters_per_second_t _velocity; + units::meters_per_second_t _velocity; - wom::utils::PIDController _pid; - wom::utils::PIDController _velocityPID; + wom::utils::PIDController _pid; + wom::utils::PIDController _velocityPID; - std::shared_ptr _table; - }; -} + std::shared_ptr _table; }; +} // namespace subsystems +} // namespace wom diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h index 9dc50b55..16526798 100644 --- a/wombat/src/main/include/subsystems/Shooter.h +++ b/wombat/src/main/include/subsystems/Shooter.h @@ -1,72 +1,76 @@ -#pragma once +// Copyright (c) 2023-2024 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 { namespace subsystems { - enum class ShooterState { - kPID, - kManual, - kIdle - }; - - struct ShooterParams { - wom::utils::Gearbox gearbox; - wom::utils::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}; - - wom::utils::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 { + wom::utils::Gearbox gearbox; + wom::utils::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}; + + wom::utils::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 subsystems +} // namespace wom diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h index fb771a34..0fea8e65 100644 --- a/wombat/src/main/include/utils/Encoder.h +++ b/wombat/src/main/include/utils/Encoder.h @@ -1,114 +1,128 @@ +// Copyright (c) 2023-2024 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 { namespace utils { - class Encoder { - public: - Encoder(double encoderTicksPerRotation, double reduction, int type) : _reduction(reduction), _encoderTicksPerRotation(encoderTicksPerRotation), _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; - 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) + : _reduction(reduction), + _encoderTicksPerRotation(encoderTicksPerRotation), + _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; + + 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 utils } // namespace wom diff --git a/wombat/src/main/include/utils/Gearbox.h b/wombat/src/main/include/utils/Gearbox.h index 5a677776..59f6ec28 100644 --- a/wombat/src/main/include/utils/Gearbox.h +++ b/wombat/src/main/include/utils/Gearbox.h @@ -1,12 +1,17 @@ +// Copyright (c) 2023-2024 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 { namespace utils { - using DCMotor = frc::DCMotor; +using DCMotor = frc::DCMotor; /** * Struct for motor and encoder pairs. * @@ -14,21 +19,20 @@ namespace utils { * 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 utils +} // namespace wom diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h index 0acecec0..48537221 100644 --- a/wombat/src/main/include/utils/PID.h +++ b/wombat/src/main/include/utils/PID.h @@ -1,173 +1,206 @@ -#pragma once - -#include "utils/Util.h" +// Copyright (c) 2023-2024 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 +#include "utils/Util.h" + namespace wom { namespace utils { - 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>>; - - 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(); +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; + in_t GetSetpoint() const { return _setpoint; } - kp_t kp; - ki_t ki{0}; - kd_t kd{0}; + in_t GetError() const { return _last_error; } - error_t stableThresh{-1}; - deriv_t stableDerivThresh{-1}; + void SetWrap(std::optional range) { _wrap_range = range; } - in_t izone{-1}; + void Reset() { _integralSum = sum_t{0}; } - 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; - } - - in_t GetSetpoint() const { - return _setpoint; - } - - in_t GetError() const { - return _last_error; - } - - void SetWrap(std::optional range) { - _wrap_range = range; - } - - 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 utils +} // namespace wom diff --git a/wombat/src/main/include/utils/RobotStartup.h b/wombat/src/main/include/utils/RobotStartup.h index 6b398f42..fb6829c3 100644 --- a/wombat/src/main/include/utils/RobotStartup.h +++ b/wombat/src/main/include/utils/RobotStartup.h @@ -1,8 +1,13 @@ +// Copyright (c) 2023-2024 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 { namespace utils { @@ -18,9 +23,12 @@ int StartRobot() { } #ifndef RUNNING_FRC_TESTS -#define WOMBAT_ROBOT_MAIN(RobotClz) int main() { wom::StartRobot(); } +#define WOMBAT_ROBOT_MAIN(RobotClz) \ + int main() { \ + wom::StartRobot(); \ + } #else #define WOMBAT_ROBOT_MAIN(RobotClz) #endif -} -} // ns wom +} // namespace utils +} // namespace wom diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h index 65e0d6b7..51fb8d04 100644 --- a/wombat/src/main/include/utils/Util.h +++ b/wombat/src/main/include/utils/Util.h @@ -1,79 +1,100 @@ +// Copyright (c) 2023-2024 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 { namespace utils { - 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) { +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); })); - } + } - NTBound(const NTBound &other) - : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) { + 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); + })); + } - _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); } - ~NTBound() { - _table->RemoveListener(_listener); - } - protected: - NT_Listener _listener; - std::shared_ptr _table; - nt::NetworkTableEntry _entry; - std::function _onUpdate; - std::string _name; - }; + 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(); }) {} - }; +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(); }) {} +}; - 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() }; }) {} - }; +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); +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); -} -} +double deadzone(double val, double deadzone = 0.05); +double spow2(double val); +} // namespace utils +} // namespace wom diff --git a/wombat/src/main/include/utils/VoltageController.h b/wombat/src/main/include/utils/VoltageController.h index 196ed022..60ea7202 100644 --- a/wombat/src/main/include/utils/VoltageController.h +++ b/wombat/src/main/include/utils/VoltageController.h @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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 @@ -8,55 +12,57 @@ namespace wom { namespace utils { +/** + * A VoltageController is analagous to a MotorController, but in terms of + * voltage instead of speed. + */ +class VoltageController { + public: + explicit VoltageController(frc::MotorController* MotorController); + /** + * Set the voltage of the output. + */ + void SetVoltage(units::volt_t voltage); /** - * A VoltageController is analagous to a MotorController, but in terms of voltage instead - * of speed. + * Get the voltage of the output. */ - class VoltageController { - public: - VoltageController(frc::MotorController *MotorController); - /** - * Set the voltage of the output. - */ - void SetVoltage(units::volt_t voltage); - /** - * Get the voltage of the output. - */ - units::volt_t GetVoltage() const; - - /** - * Set the output as inverted. - */ - void SetInverted(bool invert); - /** - * Get whether the output is inverted - */ - bool GetInverted() const; - - /** - * Get the estimated real voltage of the output, based on the controller voltage. - */ - units::volt_t GetEstimatedRealVoltage() const; - - units::volt_t GetBusVoltage() const; + units::volt_t GetVoltage() const; /** - * Create a MotorVoltageController with a given frc::MotorController - * subclass. Please note that this creates an unsafe pointer (will never dealloc) - */ - template - static VoltageController Of(Args& ...args) { - T *t = new T(args...); // Be warned, does not deallocate! + * Set the output as inverted. + */ + void SetInverted(bool invert); + /** + * Get whether the output is inverted + */ + bool GetInverted() const; + + /** + * Get the estimated real voltage of the output, based on the controller + * voltage. + */ + units::volt_t GetEstimatedRealVoltage() 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 VoltageController Of(Args&... args) { + T* t = new T(args...); // Be warned, does not deallocate! return VoltageController{t}; } - template - static VoltageController Group(Args& ...args) { - return Of(args...); + template + static VoltageController Group(Args&... args) { + return Of(args...); } - private: - frc::MotorController *_MotorController; - }; -} + private: + frc::MotorController* _MotorController; +}; +} // namespace utils } // namespace wom diff --git a/wombat/src/test/cpp/main.cpp b/wombat/src/test/cpp/main.cpp index b8b23d23..6598c5b6 100644 --- a/wombat/src/test/cpp/main.cpp +++ b/wombat/src/test/cpp/main.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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..1834631a 100644 --- a/wombat/src/test/cpp/test_behaviours.cpp +++ b/wombat/src/test/cpp/test_behaviours.cpp @@ -1,3 +1,7 @@ +// Copyright (c) 2023-2024 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 +}