diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..d424ff9 --- /dev/null +++ b/.clang-format @@ -0,0 +1,271 @@ +--- +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 +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseColons: 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 +KeepEmptyLinesAtEOF: false +LambdaBodyIndentation: Signature +LineEnding: DeriveLF +MacroBlockBegin: '' +MacroBlockEnd: '' +Macros: + - 'HAL_ENUM(name)=enum name' +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 +RemoveParentheses: Leave +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 +SpaceBeforeJsonColon: false +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 +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never +SpacesInContainerLiterals: true +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + InCStyleCasts: false + InConditionalStatements: false + InEmptyParentheses: false + Other: false +SpacesInSquareBrackets: false +Standard: c++20 +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..b11be28 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,69 @@ +Checks: + 'bugprone-assert-side-effect, + bugprone-bool-pointer-implicit-conversion, + bugprone-copy-constructor-init, + bugprone-dangling-handle, + bugprone-dynamic-static-initializers, + bugprone-forward-declaration-namespace, + bugprone-forwarding-reference-overload, + bugprone-inaccurate-erase, + bugprone-incorrect-roundings, + bugprone-integer-division, + bugprone-lambda-function-name, + bugprone-misplaced-operator-in-strlen-in-alloc, + bugprone-misplaced-widening-cast, + bugprone-move-forwarding-reference, + bugprone-multiple-statement-macro, + bugprone-parent-virtual-call, + bugprone-posix-return, + bugprone-sizeof-container, + bugprone-sizeof-expression, + bugprone-spuriously-wake-up-functions, + bugprone-string-constructor, + bugprone-string-integer-assignment, + bugprone-string-literal-with-embedded-nul, + bugprone-suspicious-enum-usage, + bugprone-suspicious-include, + bugprone-suspicious-memset-usage, + bugprone-suspicious-missing-comma, + bugprone-suspicious-semicolon, + bugprone-suspicious-string-compare, + bugprone-throw-keyword-missing, + bugprone-too-small-loop-variable, + bugprone-undefined-memory-manipulation, + bugprone-undelegated-constructor, + bugprone-unhandled-self-assignment, + bugprone-unused-raii, + bugprone-virtual-near-miss, + cert-err52-cpp, + cert-err60-cpp, + cert-mem57-cpp, + cert-oop57-cpp, + cert-oop58-cpp, + clang-diagnostic-*, + -clang-diagnostic-deprecated-declarations, + -clang-diagnostic-#warnings, + -clang-diagnostic-pedantic, + clang-analyzer-*, + cppcoreguidelines-slicing, + google-build-namespaces, + google-explicit-constructor, + google-global-names-in-headers, + google-readability-casting, + google-runtime-operator, + misc-definitions-in-headers, + misc-misplaced-const, + misc-new-delete-overloads, + misc-non-copyable-objects, + modernize-avoid-bind, + modernize-concat-nested-namespaces, + modernize-make-shared, + modernize-make-unique, + modernize-pass-by-value, + modernize-use-default-member-init, + modernize-use-noexcept, + modernize-use-nullptr, + modernize-use-override, + modernize-use-using, + readability-braces-around-statements' +FormatStyle: file \ No newline at end of file diff --git a/.styleguide b/.styleguide index b45d4e7..dac1213 100644 --- a/.styleguide +++ b/.styleguide @@ -35,4 +35,5 @@ modifiableFileExclude { src/main/include/pathplanner/ src/main/include/photon/ src/main/include/PhotonVersion.h + src/main/cpp/PhotonVersion.cpp } diff --git a/requirements.txt b/requirements.txt index 67fa2b6..c415e99 100644 Binary files a/requirements.txt and b/requirements.txt differ diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index ae81ab7..c182d80 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -109,7 +109,9 @@ void Robot::TeleopPeriodic() {} void Robot::TeleopExit() {} -void Robot::TestInit() { frc2::CommandScheduler::GetInstance().CancelAll(); } +void Robot::TestInit() { + frc2::CommandScheduler::GetInstance().CancelAll(); +} void Robot::TestPeriodic() {} @@ -128,5 +130,7 @@ void Robot::SimulationPeriodic() { } #ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } +int main() { + return frc::StartRobot(); +} #endif diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index bff2b3e..44ad046 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -8,7 +8,9 @@ #include #include -RobotContainer::RobotContainer() { ConfigureBindings(); } +RobotContainer::RobotContainer() { + ConfigureBindings(); +} void RobotContainer::ConfigureBindings() { // DEFAULT DRIVE COMMAND @@ -119,35 +121,35 @@ frc2::CommandPtr RobotContainer::IntakeNote() { .Until([this] { return feederSubsystem.HasNote(); }); } -frc2::Command *RobotContainer::GetAutonomousCommand() { +frc2::Command* RobotContainer::GetAutonomousCommand() { // return autos.GetSelectedCommand(); return nullptr; } -SwerveSubsystem &RobotContainer::GetSwerveSubsystem() { +SwerveSubsystem& RobotContainer::GetSwerveSubsystem() { return swerveSubsystem; } -ShooterSubsystem &RobotContainer::GetShooterSubsystem() { +ShooterSubsystem& RobotContainer::GetShooterSubsystem() { return shooterSubsystem; } -IntakeSubsystem &RobotContainer::GetIntakeSubsystem() { +IntakeSubsystem& RobotContainer::GetIntakeSubsystem() { return intakeSubsystem; } -FeederSubsystem &RobotContainer::GetFeederSubsystem() { +FeederSubsystem& RobotContainer::GetFeederSubsystem() { return feederSubsystem; } // str::Vision &RobotContainer::GetVision() { return vision; } -str::NoteVisualizer &RobotContainer::GetNoteVisualizer() { +str::NoteVisualizer& RobotContainer::GetNoteVisualizer() { return noteVisualizer; } -frc2::CommandPtr -RobotContainer::RumbleDriver(std::function timeToRumble) { +frc2::CommandPtr RobotContainer::RumbleDriver( + std::function timeToRumble) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([this] { driverController.SetRumble( @@ -163,8 +165,8 @@ RobotContainer::RumbleDriver(std::function timeToRumble) { }); } -frc2::CommandPtr -RobotContainer::RumbleOperator(std::function timeToRumble) { +frc2::CommandPtr RobotContainer::RumbleOperator( + std::function timeToRumble) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([this] { operatorController.SetRumble( diff --git a/src/main/cpp/str/Camera.cpp b/src/main/cpp/str/Camera.cpp index 9b778c4..403e608 100644 --- a/src/main/cpp/str/Camera.cpp +++ b/src/main/cpp/str/Camera.cpp @@ -6,7 +6,11 @@ #include +#include +#include +#include #include +#include #include #include "constants/Constants.h" @@ -26,7 +30,8 @@ Camera::Camera(std::string cameraName, frc::Transform3d robotToCamera, : simulate(simulate), posePub(nt->GetStructTopic(cameraName + "PoseEstimation") .Publish()), - singleTagDevs(singleTagDevs), multiTagDevs(multiTagDevs), + singleTagDevs(singleTagDevs), + multiTagDevs(multiTagDevs), stdDevXPosePub(nt->GetDoubleTopic(cameraName + "StdDevsX").Publish()), stdDevYPosePub(nt->GetDoubleTopic(cameraName + "StdDevsY").Publish()), stdDevRotPosePub( @@ -81,7 +86,7 @@ std::optional Camera::GetDistanceToNote() { double pixelToRad = 20; photon::PhotonPipelineResult result = camera->GetLatestResult(); if (result.HasTargets()) { - for (const auto &target : result.GetTargets()) { + for (const auto& target : result.GetTargets()) { if (target.GetFiducialId() == -1) { double width = 0; double yaw = 0; @@ -89,7 +94,7 @@ std::optional Camera::GetDistanceToNote() { double minX = corners[0].x; double maxX = corners[0].x; - for (const auto &corner : corners) { + for (const auto& corner : corners) { minX = std::min(minX, corner.x); maxX = std::max(maxX, corner.x); } @@ -119,9 +124,13 @@ std::optional Camera::GetDistanceToNote() { } } -std::optional Camera::GetAngleToNote() { return angleToNote; } +std::optional Camera::GetAngleToNote() { + return angleToNote; +} -photon::PhotonPipelineResult Camera::GetLatestResult() { return latestResult; } +photon::PhotonPipelineResult Camera::GetLatestResult() { + return latestResult; +} std::optional Camera::GetEstimatedGlobalPose() { if (!simulate) { @@ -130,7 +139,7 @@ std::optional Camera::GetEstimatedGlobalPose() { std::optional visionEst; - for (const auto &result : camera->GetAllUnreadResults()) { + for (const auto& result : camera->GetAllUnreadResults()) { visionEst = photonEstimator->Update(result); if (visionEst.has_value()) { @@ -145,13 +154,13 @@ std::optional Camera::GetEstimatedGlobalPose() { return visionEst; } -Eigen::Matrix -Camera::GetEstimationStdDevs(frc::Pose2d estimatedPose) { +Eigen::Matrix Camera::GetEstimationStdDevs( + frc::Pose2d estimatedPose) { Eigen::Matrix estStdDevs = singleTagDevs; auto targets = GetLatestResult().GetTargets(); int numTags = 0; units::meter_t avgDist = 0_m; - for (const auto &tgt : targets) { + for (const auto& tgt : targets) { auto tagPose = photonEstimator->GetFieldLayout().GetTagPose(tgt.GetFiducialId()); if (tagPose.has_value()) { @@ -191,10 +200,9 @@ void Camera::SimPeriodic(frc::Pose2d robotSimPose) { } } -std::vector -Camera::CreateTorusVertices(units::meter_t majorRadius, - units::meter_t minorRadius, int numMajorDivisions, - int numMinorDivisons) { +std::vector Camera::CreateTorusVertices( + units::meter_t majorRadius, units::meter_t minorRadius, + int numMajorDivisions, int numMinorDivisons) { std::vector vertices{}; units::radian_t majorAngleIncrement = diff --git a/src/main/cpp/str/ChoreoSwerveCommandWithForce.cpp b/src/main/cpp/str/ChoreoSwerveCommandWithForce.cpp index f22e869..0c56179 100644 --- a/src/main/cpp/str/ChoreoSwerveCommandWithForce.cpp +++ b/src/main/cpp/str/ChoreoSwerveCommandWithForce.cpp @@ -19,7 +19,8 @@ ChoreoSwerveCommandWithForce::ChoreoSwerveCommandWithForce( std::function)> outputXForce, std::function)> outputYForce, std::function mirrorTrajectory, frc2::Requirements requirements) - : m_traj(std::move(trajectory)), m_pose(std::move(poseSupplier)), + : m_traj(std::move(trajectory)), + m_pose(std::move(poseSupplier)), m_controller(std::move(controller)), m_outputChassisSpeeds(std::move(outputChassisSpeeds)), m_outputXForce(std::move(outputXForce)), @@ -29,7 +30,9 @@ ChoreoSwerveCommandWithForce::ChoreoSwerveCommandWithForce( } // Called when the command is initially scheduled. -void ChoreoSwerveCommandWithForce::Initialize() { m_timer.Restart(); } +void ChoreoSwerveCommandWithForce::Initialize() { + m_timer.Restart(); +} // Called repeatedly when this Command is scheduled to run void ChoreoSwerveCommandWithForce::Execute() { diff --git a/src/main/cpp/str/DataUtils.cpp b/src/main/cpp/str/DataUtils.cpp index 20a33f4..4a73299 100644 --- a/src/main/cpp/str/DataUtils.cpp +++ b/src/main/cpp/str/DataUtils.cpp @@ -10,6 +10,7 @@ #include #include +#include using namespace str; diff --git a/src/main/cpp/str/NoteVisualizer.cpp b/src/main/cpp/str/NoteVisualizer.cpp index 7694f3d..1b24fe5 100644 --- a/src/main/cpp/str/NoteVisualizer.cpp +++ b/src/main/cpp/str/NoteVisualizer.cpp @@ -65,7 +65,7 @@ void NoteVisualizer::Periodic() { } void NoteVisualizer::DisplayRobotNote(bool hasNote, - const frc::Pose2d &robotPosition) { + const frc::Pose2d& robotPosition) { if (hasNote) { frc::Pose3d robotPose = frc::Pose3d(robotPosition); robotNote = robotPose.TransformBy( @@ -78,7 +78,7 @@ void NoteVisualizer::DisplayRobotNote(bool hasNote, void NoteVisualizer::UpdateLaunchedNotes(units::second_t loopTime) { int i = 0; - for (auto ¬e : launchedNotes) { + for (auto& note : launchedNotes) { ProjectileMotion(note, loopTime); launchedNotePoses[i] = note.currentPose; i++; @@ -98,7 +98,7 @@ void NoteVisualizer::CleanUp() { } } -void NoteVisualizer::ProjectileMotion(FlyingNote ¬e, +void NoteVisualizer::ProjectileMotion(FlyingNote& note, units::second_t loopTime) { note.currentVelocity.zVel = note.currentVelocity.zVel - (9.81_mps_sq * loopTime); diff --git a/src/main/cpp/str/SwerveDrive.cpp b/src/main/cpp/str/SwerveDrive.cpp index bd8dbea..fa64fd5 100644 --- a/src/main/cpp/str/SwerveDrive.cpp +++ b/src/main/cpp/str/SwerveDrive.cpp @@ -13,7 +13,7 @@ using namespace str; SwerveDrive::SwerveDrive() { for (size_t i = 0; i < modules.size(); i++) { - const auto &modSigs = modules[i].GetSignals(); + const auto& modSigs = modules[i].GetSignals(); allSignals[(i * 8) + 0] = modSigs[0]; allSignals[(i * 8) + 1] = modSigs[1]; allSignals[(i * 8) + 2] = modSigs[2]; @@ -42,11 +42,11 @@ SwerveDrive::SwerveDrive() { allSignals[allSignals.size() - 2] = &imu.GetYaw(); allSignals[allSignals.size() - 1] = &imu.GetAngularVelocityZWorld(); - for (const auto &sig : allSignals) { + for (const auto& sig : allSignals) { sig->SetUpdateFrequency(1 / consts::SWERVE_ODOM_LOOP_PERIOD); } - for (auto &mod : modules) { + for (auto& mod : modules) { if (!mod.OptimizeBusSignals()) { frc::DataLogManager::Log(fmt::format( "Failed to optimize bus signals for {}\n", mod.GetName())); @@ -60,7 +60,7 @@ SwerveDrive::SwerveDrive() { lastOdomUpdateTime = frc::Timer::GetFPGATimestamp(); } -void SwerveDrive::ResetPose(const frc::Pose2d &resetPose) { +void SwerveDrive::ResetPose(const frc::Pose2d& resetPose) { odom.ResetPosition(frc::Rotation2d{GetYawFromImu()}, modulePositions, resetPose); poseEstimator.ResetPosition(frc::Rotation2d{GetYawFromImu()}, modulePositions, @@ -68,7 +68,7 @@ void SwerveDrive::ResetPose(const frc::Pose2d &resetPose) { } void SwerveDrive::DriveRobotRelative( - const frc::ChassisSpeeds &robotRelativeSpeeds) { + const frc::ChassisSpeeds& robotRelativeSpeeds) { Drive(robotRelativeSpeeds.vx, robotRelativeSpeeds.vy, robotRelativeSpeeds.omega, false); } @@ -100,7 +100,9 @@ frc::ChassisSpeeds SwerveDrive::GetRobotRelativeSpeeds() const { return consts::swerve::physical::KINEMATICS.ToChassisSpeeds(moduleStates); } -frc::Pose2d SwerveDrive::GetOdomPose() const { return odom.GetPose(); } +frc::Pose2d SwerveDrive::GetOdomPose() const { + return odom.GetPose(); +} frc::Pose2d SwerveDrive::GetPose() const { return poseEstimator.GetEstimatedPosition(); @@ -115,7 +117,9 @@ frc::Pose2d SwerveDrive::GetPredictedPose(units::second_t translationLookahead, frc::Rotation2d{currentVel.omega * rotationLookahead}}); } -units::radian_t SwerveDrive::GetYawFromImu() { return yawLatencyComped; } +units::radian_t SwerveDrive::GetYawFromImu() { + return yawLatencyComped; +} std::array SwerveDrive::GetModuleDriveOutputShaftPositions() { @@ -124,14 +128,14 @@ SwerveDrive::GetModuleDriveOutputShaftPositions() { } void SwerveDrive::SetModuleStates( - const std::array &desiredStates, bool optimize, + const std::array& desiredStates, bool optimize, bool openLoop, - const std::array &moduleTorqueCurrentsFF) { + const std::array& moduleTorqueCurrentsFF) { wpi::array finalState = desiredStates; frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( &finalState, consts::swerve::physical::DRIVE_MAX_SPEED); int i = 0; - for (auto &mod : modules) { + for (auto& mod : modules) { finalState[i] = mod.GoToState(finalState[i], optimize, openLoop, moduleTorqueCurrentsFF[i]); i++; @@ -139,9 +143,9 @@ void SwerveDrive::SetModuleStates( desiredStatesPub.Set(finalState); } -void SwerveDrive::AddVisionMeasurement(const frc::Pose2d &visionMeasurement, +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp, - const Eigen::Vector3d &stdDevs) { + const Eigen::Vector3d& stdDevs) { // outside field, so we dont want to add this measurement to estimator, // because we know its wrong // TODO: Maybe add a small a buffer around the robots frame to add poses that @@ -167,7 +171,7 @@ void SwerveDrive::UpdateSwerveOdom() { // } int i = 0; - for (auto &mod : modules) { + for (auto& mod : modules) { modulePositions[i] = mod.GetCurrentPosition(false); moduleStates[i] = mod.GetCurrentState(); i++; @@ -197,7 +201,7 @@ void SwerveDrive::UpdateNTEntries() { void SwerveDrive::SimulationPeriodic() { std::array simState; int i = 0; - for (auto &swerveModule : modules) { + for (auto& swerveModule : modules) { simState[i] = swerveModule.UpdateSimulation( consts::LOOP_PERIOD, frc::RobotController::GetBatteryVoltage()); i++; @@ -215,19 +219,19 @@ void SwerveDrive::SimulationPeriodic() { units::ampere_t SwerveDrive::GetSimulatedCurrentDraw() const { units::ampere_t totalCurrent = 0_A; - for (const auto &swerveModule : modules) { + for (const auto& swerveModule : modules) { totalCurrent += swerveModule.GetSimulatedCurrentDraw(); } return totalCurrent; } void SwerveDrive::SetXModuleForces( - const std::array &xForce) { + const std::array& xForce) { xModuleForce = xForce; } void SwerveDrive::SetYModuleForces( - const std::array &yForce) { + const std::array& yForce) { yModuleForce = yForce; } @@ -261,7 +265,7 @@ void SwerveDrive::SetCharacterizationVoltageDrive(units::volt_t volts) { modules[3].SetDriveToVoltage(volts); } -void SwerveDrive::LogMk4iSteerTorque(frc::sysid::SysIdRoutineLog *log) { +void SwerveDrive::LogMk4iSteerTorque(frc::sysid::SysIdRoutineLog* log) { log->Motor("swerve-steer-mk4i") .voltage(units::volt_t{allSignals[5]->GetValueAsDouble()}) .position(units::turn_t{allSignals[2]->GetValueAsDouble()}) @@ -269,21 +273,21 @@ void SwerveDrive::LogMk4iSteerTorque(frc::sysid::SysIdRoutineLog *log) { } // This assumes the mk4n's are in the back of the robot -void SwerveDrive::LogMk4nSteerTorque(frc::sysid::SysIdRoutineLog *log) { +void SwerveDrive::LogMk4nSteerTorque(frc::sysid::SysIdRoutineLog* log) { log->Motor("swerve-steer-mk4n") .voltage(units::volt_t{allSignals[21]->GetValueAsDouble()}) .position(units::turn_t{allSignals[18]->GetValueAsDouble()}) .velocity(units::turns_per_second_t{allSignals[19]->GetValueAsDouble()}); } -void SwerveDrive::LogDriveTorque(frc::sysid::SysIdRoutineLog *log) { +void SwerveDrive::LogDriveTorque(frc::sysid::SysIdRoutineLog* log) { log->Motor("swerve-drive") .voltage(units::volt_t{allSignals[4]->GetValueAsDouble()}) .position(units::turn_t{allSignals[0]->GetValueAsDouble()}) .velocity(units::turns_per_second_t{allSignals[1]->GetValueAsDouble()}); } -void SwerveDrive::LogMk4iSteerVoltage(frc::sysid::SysIdRoutineLog *log) { +void SwerveDrive::LogMk4iSteerVoltage(frc::sysid::SysIdRoutineLog* log) { log->Motor("swerve-steer-mk4i") .voltage(units::volt_t{allSignals[7]->GetValueAsDouble()}) .position(units::turn_t{allSignals[2]->GetValueAsDouble()}) @@ -291,14 +295,14 @@ void SwerveDrive::LogMk4iSteerVoltage(frc::sysid::SysIdRoutineLog *log) { } // This assumes the mk4n's are in the back of the robot -void SwerveDrive::LogMk4nSteerVoltage(frc::sysid::SysIdRoutineLog *log) { +void SwerveDrive::LogMk4nSteerVoltage(frc::sysid::SysIdRoutineLog* log) { log->Motor("swerve-steer-mk4n") .voltage(units::volt_t{allSignals[23]->GetValueAsDouble()}) .position(units::turn_t{allSignals[18]->GetValueAsDouble()}) .velocity(units::turns_per_second_t{allSignals[19]->GetValueAsDouble()}); } -void SwerveDrive::LogDriveVoltage(frc::sysid::SysIdRoutineLog *log) { +void SwerveDrive::LogDriveVoltage(frc::sysid::SysIdRoutineLog* log) { log->Motor("swerve-drive") .voltage(units::volt_t{allSignals[6]->GetValueAsDouble()}) .position(units::turn_t{allSignals[0]->GetValueAsDouble()}) @@ -306,8 +310,8 @@ void SwerveDrive::LogDriveVoltage(frc::sysid::SysIdRoutineLog *log) { } std::array SwerveDrive::ConvertModuleForcesToTorqueCurrent( - const std::array &xForce, - const std::array &yForce) { + const std::array& xForce, + const std::array& yForce) { std::array retVal; for (int i = 0; i < 4; i++) { frc::Translation2d moduleForceFieldRef{units::meter_t{xForce[i].value()}, @@ -336,12 +340,12 @@ bool SwerveDrive::IsSlipping() { translationComponent); auto maxIt = std::max_element( transStates.begin(), transStates.end(), - [](const frc::SwerveModuleState &a, const frc::SwerveModuleState &b) { + [](const frc::SwerveModuleState& a, const frc::SwerveModuleState& b) { return a.speed < b.speed; }); auto minIt = std::min_element( transStates.begin(), transStates.end(), - [](const frc::SwerveModuleState &a, const frc::SwerveModuleState &b) { + [](const frc::SwerveModuleState& a, const frc::SwerveModuleState& b) { return a.speed < b.speed; }); return (maxIt->speed / minIt->speed) > slipCoeff; diff --git a/src/main/cpp/str/SwerveModule.cpp b/src/main/cpp/str/SwerveModule.cpp index 44f5c30..2f9f407 100644 --- a/src/main/cpp/str/SwerveModule.cpp +++ b/src/main/cpp/str/SwerveModule.cpp @@ -7,15 +7,20 @@ #include #include +#include + using namespace str; SwerveModule::SwerveModule(SwerveModuleConstants constants, SwerveModulePhysical physicalAttrib, SwerveModuleSteerGains steerGains, SwerveModuleDriveGains driveGains) - : steerMotor(constants.steerId, "*"), driveMotor(constants.driveId, "*"), - steerEncoder(constants.encoderId, "*"), moduleName(constants.moduleName), - steerGains(steerGains), driveGains(driveGains), + : steerMotor(constants.steerId, "*"), + driveMotor(constants.driveId, "*"), + steerEncoder(constants.encoderId, "*"), + moduleName(constants.moduleName), + steerGains(steerGains), + driveGains(driveGains), couplingRatio(physicalAttrib.couplingRatio), driveGearing(physicalAttrib.driveGearing), wheelRadius(physicalAttrib.wheelRadius), @@ -40,9 +45,9 @@ SwerveModule::SwerveModule(SwerveModuleConstants constants, ConfigureControlSignals(); } -frc::SwerveModuleState -SwerveModule::GoToState(frc::SwerveModuleState desiredState, bool optimize, - bool openLoopDrive, units::ampere_t arbff) { +frc::SwerveModuleState SwerveModule::GoToState( + frc::SwerveModuleState desiredState, bool optimize, bool openLoopDrive, + units::ampere_t arbff) { frc::SwerveModuleState currentState = GetCurrentState(); if (optimize) { desiredState.Optimize(currentState.angle); @@ -141,13 +146,12 @@ units::radian_t SwerveModule::GetOutputShaftTurns() { return ConvertDriveMotorRotationsToWheelRotations(latencyCompDrivePos); } -frc::SwerveModuleState -SwerveModule::UpdateSimulation(units::second_t deltaTime, - units::volt_t supplyVoltage) { +frc::SwerveModuleState SwerveModule::UpdateSimulation( + units::second_t deltaTime, units::volt_t supplyVoltage) { return moduleSim.Update(deltaTime, supplyVoltage); } -std::array SwerveModule::GetSignals() { +std::array SwerveModule::GetSignals() { return {&drivePositionSig, &driveVelocitySig, &steerPositionSig, &steerVelocitySig, &driveTorqueCurrentSig, &steerTorqueCurrentSig, &driveVoltageSig, &steerVoltageSig}; @@ -300,7 +304,9 @@ bool SwerveModule::OptimizeBusSignals() { return optimizeDriveResult.IsOK() && optimizeSteerResult.IsOK(); } -std::string SwerveModule::GetName() const { return moduleName; } +std::string SwerveModule::GetName() const { + return moduleName; +} units::ampere_t SwerveModule::GetSimulatedCurrentDraw() const { return moduleSim.GetDriveCurrentDraw() + moduleSim.GetSteerCurrentDraw(); diff --git a/src/main/cpp/str/SwerveModuleSim.cpp b/src/main/cpp/str/SwerveModuleSim.cpp index 6788e31..7b89870 100644 --- a/src/main/cpp/str/SwerveModuleSim.cpp +++ b/src/main/cpp/str/SwerveModuleSim.cpp @@ -10,14 +10,15 @@ using namespace str; SwerveModuleSim::SwerveModuleSim( SwerveModuleConstants constants, SwerveModulePhysical physicalAttrib, - ctre::phoenix6::sim::TalonFXSimState &driveSimState, - ctre::phoenix6::sim::TalonFXSimState &steerSimState, - ctre::phoenix6::sim::CANcoderSimState &steerEncoderSimState) + ctre::phoenix6::sim::TalonFXSimState& driveSimState, + ctre::phoenix6::sim::TalonFXSimState& steerSimState, + ctre::phoenix6::sim::CANcoderSimState& steerEncoderSimState) : driveSim(physicalAttrib.driveMotor, physicalAttrib.driveGearing, 0.025_kg_sq_m), steerSim(physicalAttrib.steerMotor, physicalAttrib.steerGearing, 0.004_kg_sq_m), - driveSimState(driveSimState), steerSimState(steerSimState), + driveSimState(driveSimState), + steerSimState(steerSimState), steerEncoderSimState(steerEncoderSimState), driveInverted(constants.invertDrive), steerInverted(constants.invertSteer), @@ -70,14 +71,13 @@ frc::SwerveModuleState SwerveModuleSim::Update(units::second_t deltaTime, steerEncoderSimState.SetRawPosition(steerSim.GetAngularPosition()); steerEncoderSimState.SetVelocity(steerSim.GetAngularVelocity()); - return frc::SwerveModuleState{(driveSim.GetAngularVelocity() / 1_rad) * - wheelRadius, - frc::Rotation2d{steerSim.GetAngularPosition()}}; + return frc::SwerveModuleState{ + (driveSim.GetAngularVelocity() / 1_rad) * wheelRadius, + frc::Rotation2d{steerSim.GetAngularPosition()}}; } -units::volt_t -SwerveModuleSim::AddFrictionVoltage(units::volt_t outputVoltage, - units::volt_t frictionVoltage) { +units::volt_t SwerveModuleSim::AddFrictionVoltage( + units::volt_t outputVoltage, units::volt_t frictionVoltage) { if (units::math::abs(outputVoltage) < frictionVoltage) { outputVoltage = 0_V; } else if (outputVoltage > 0_V) { diff --git a/src/main/cpp/str/Vision.cpp b/src/main/cpp/str/Vision.cpp index 54c19b3..e121f84 100644 --- a/src/main/cpp/str/Vision.cpp +++ b/src/main/cpp/str/Vision.cpp @@ -4,15 +4,17 @@ #include "str/Vision.h" -#include "frc/geometry/Pose2d.h" +#include + +#include using namespace str; std::vector>> Vision::GetPoseStdDevs( - const std::vector> &poses) { + const std::vector>& poses) { std::vector>> allStdDevs; int i = 0; - for (auto &cam : cameras) { + for (auto& cam : cameras) { if (poses[i].has_value()) { allStdDevs.push_back( cam.GetEstimationStdDevs(poses[i].value().estimatedPose.ToPose2d())); @@ -27,14 +29,14 @@ std::vector>> Vision::GetPoseStdDevs( std::vector> Vision::GetCameraEstimatedPoses() { std::vector> allPoses; - for (auto &cam : cameras) { + for (auto& cam : cameras) { allPoses.push_back(cam.GetEstimatedGlobalPose()); } return allPoses; } void Vision::SimulationPeriodic(frc::Pose2d simRobotPose) { - for (auto &cam : cameras) { + for (auto& cam : cameras) { cam.SimPeriodic(simRobotPose); noteCamera.SimPeriodic(simRobotPose); } diff --git a/src/main/cpp/subsystems/FeederSubsystem.cpp b/src/main/cpp/subsystems/FeederSubsystem.cpp index f8e3bf0..03ad1e9 100644 --- a/src/main/cpp/subsystems/FeederSubsystem.cpp +++ b/src/main/cpp/subsystems/FeederSubsystem.cpp @@ -139,4 +139,6 @@ bool FeederSubsystem::ConfigureMotorSignals() { return optimizeFeederMotor.IsOK(); } -bool FeederSubsystem::HasNote() const { return hasNote; } +bool FeederSubsystem::HasNote() const { + return hasNote; +} diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp index 79fbd29..5670240 100644 --- a/src/main/cpp/subsystems/IntakeSubsystem.cpp +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -20,7 +20,9 @@ IntakeSubsystem::IntakeSubsystem() { frc::SmartDashboard::PutData(this); } -bool IntakeSubsystem::TouchingNote() { return isTouchingNote; } +bool IntakeSubsystem::TouchingNote() { + return isTouchingNote; +} frc2::CommandPtr IntakeSubsystem::IntakeNote() { return frc2::cmd::RunEnd( diff --git a/src/main/cpp/subsystems/ShooterSubsystem.cpp b/src/main/cpp/subsystems/ShooterSubsystem.cpp index 8204a7d..0ba0183 100644 --- a/src/main/cpp/subsystems/ShooterSubsystem.cpp +++ b/src/main/cpp/subsystems/ShooterSubsystem.cpp @@ -8,6 +8,8 @@ #include #include +#include + #include "constants/Constants.h" ShooterSubsystem::ShooterSubsystem() { @@ -32,38 +34,38 @@ frc2::CommandPtr ShooterSubsystem::RunShooter( return frc2::cmd::Run( [this, preset, distance] { switch (preset()) { - case consts::shooter::PRESET_SPEEDS::AMP: - topWheelVelocitySetpoint = - consts::shooter::AMP_SPEEDS.topSpeed; - bottomWheelVelocitySetpoint = - consts::shooter::AMP_SPEEDS.bottomSpeed; - break; - case consts::shooter::PRESET_SPEEDS::SUBWOOFER: - topWheelVelocitySetpoint = - consts::shooter::SUBWOOFER_SPEEDS.topSpeed; - bottomWheelVelocitySetpoint = - consts::shooter::SUBWOOFER_SPEEDS.bottomSpeed; - break; - case consts::shooter::PRESET_SPEEDS::PASS: - topWheelVelocitySetpoint = - consts::shooter::PASS_SPEEDS.topSpeed; - bottomWheelVelocitySetpoint = - consts::shooter::PASS_SPEEDS.bottomSpeed; - break; - case consts::shooter::PRESET_SPEEDS::SPEAKER_DIST: - topWheelVelocitySetpoint = - consts::shooter::TOP_SHOOTER_LUT[distance()]; - bottomWheelVelocitySetpoint = - consts::shooter::BOTTOM_SHOOTER_LUT[distance()]; - break; - case consts::shooter::PRESET_SPEEDS::OFF: - topWheelVelocitySetpoint = 0_rpm; - bottomWheelVelocitySetpoint = 0_rpm; - break; - default: - topWheelVelocitySetpoint = 0_rpm; - bottomWheelVelocitySetpoint = 0_rpm; - break; + case consts::shooter::PRESET_SPEEDS::AMP: + topWheelVelocitySetpoint = + consts::shooter::AMP_SPEEDS.topSpeed; + bottomWheelVelocitySetpoint = + consts::shooter::AMP_SPEEDS.bottomSpeed; + break; + case consts::shooter::PRESET_SPEEDS::SUBWOOFER: + topWheelVelocitySetpoint = + consts::shooter::SUBWOOFER_SPEEDS.topSpeed; + bottomWheelVelocitySetpoint = + consts::shooter::SUBWOOFER_SPEEDS.bottomSpeed; + break; + case consts::shooter::PRESET_SPEEDS::PASS: + topWheelVelocitySetpoint = + consts::shooter::PASS_SPEEDS.topSpeed; + bottomWheelVelocitySetpoint = + consts::shooter::PASS_SPEEDS.bottomSpeed; + break; + case consts::shooter::PRESET_SPEEDS::SPEAKER_DIST: + topWheelVelocitySetpoint = + consts::shooter::TOP_SHOOTER_LUT[distance()]; + bottomWheelVelocitySetpoint = + consts::shooter::BOTTOM_SHOOTER_LUT[distance()]; + break; + case consts::shooter::PRESET_SPEEDS::OFF: + topWheelVelocitySetpoint = 0_rpm; + bottomWheelVelocitySetpoint = 0_rpm; + break; + default: + topWheelVelocitySetpoint = 0_rpm; + bottomWheelVelocitySetpoint = 0_rpm; + break; } }, {this}) @@ -156,7 +158,6 @@ void ShooterSubsystem::UpdateNTEntries() { bool ShooterSubsystem::ConfigureShooterMotors( bool invertBottom, bool invertTop, units::scalar_t shooterGearing, units::ampere_t supplyCurrentLimit, units::ampere_t statorCurrentLimit) { - ctre::phoenix6::configs::TalonFXConfiguration shooterConfig{}; shooterConfig.MotorOutput.NeutralMode = @@ -225,15 +226,15 @@ bool ShooterSubsystem::ConfigureMotorSignals() { return optimizeTopMotor.IsOK() && optimizeBottomMotor.IsOK(); } -frc2::CommandPtr -ShooterSubsystem::TopWheelSysIdQuasistatic(frc2::sysid::Direction direction) { +frc2::CommandPtr ShooterSubsystem::TopWheelSysIdQuasistatic( + frc2::sysid::Direction direction) { return topWheelSysIdRoutine.Quasistatic(direction) .BeforeStarting([this] { runningSysid = true; }) .AndThen([this] { runningSysid = false; }); } -frc2::CommandPtr -ShooterSubsystem::TopWheelSysIdDynamic(frc2::sysid::Direction direction) { +frc2::CommandPtr ShooterSubsystem::TopWheelSysIdDynamic( + frc2::sysid::Direction direction) { return topWheelSysIdRoutine.Dynamic(direction) .BeforeStarting([this] { runningSysid = true; }) .AndThen([this] { runningSysid = false; }); @@ -246,16 +247,16 @@ frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdQuasistatic( .AndThen([this] { runningSysid = false; }); } -frc2::CommandPtr -ShooterSubsystem::BottomWheelSysIdDynamic(frc2::sysid::Direction direction) { +frc2::CommandPtr ShooterSubsystem::BottomWheelSysIdDynamic( + frc2::sysid::Direction direction) { return bottomWheelSysIdRoutine.Dynamic(direction) .BeforeStarting([this] { runningSysid = true; }) .AndThen([this] { runningSysid = false; }); } void ShooterSubsystem::SetupLUTs( - const std::map &speeds) { - for (const auto &[key, val] : speeds) { + const std::map& speeds) { + for (const auto& [key, val] : speeds) { consts::shooter::TOP_SHOOTER_LUT.insert(key, val.topSpeed); consts::shooter::BOTTOM_SHOOTER_LUT.insert(key, val.bottomSpeed); } diff --git a/src/main/cpp/subsystems/SwerveSubsystem.cpp b/src/main/cpp/subsystems/SwerveSubsystem.cpp index 756a661..eeca0f0 100644 --- a/src/main/cpp/subsystems/SwerveSubsystem.cpp +++ b/src/main/cpp/subsystems/SwerveSubsystem.cpp @@ -13,6 +13,8 @@ #include #include +#include + #include "constants/Constants.h" #include "constants/VisionConstants.h" @@ -33,22 +35,32 @@ SwerveSubsystem::SwerveSubsystem() // LoadChoreoTrajectories(); } -void SwerveSubsystem::UpdateSwerveOdom() { swerveDrive.UpdateSwerveOdom(); } +void SwerveSubsystem::UpdateSwerveOdom() { + swerveDrive.UpdateSwerveOdom(); +} -void SwerveSubsystem::AddVisionMeasurement(const frc::Pose2d &visionMeasurement, +void SwerveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp, - const Eigen::Vector3d &stdDevs) { + const Eigen::Vector3d& stdDevs) { swerveDrive.AddVisionMeasurement(visionMeasurement, timestamp, stdDevs); } // This method will be called once per scheduler run -void SwerveSubsystem::Periodic() { swerveDrive.UpdateNTEntries(); } +void SwerveSubsystem::Periodic() { + swerveDrive.UpdateNTEntries(); +} -void SwerveSubsystem::SimulationPeriodic() { swerveDrive.SimulationPeriodic(); } +void SwerveSubsystem::SimulationPeriodic() { + swerveDrive.SimulationPeriodic(); +} -frc::Pose2d SwerveSubsystem::GetOdomPose() { return swerveDrive.GetOdomPose(); } +frc::Pose2d SwerveSubsystem::GetOdomPose() { + return swerveDrive.GetOdomPose(); +} -frc::Pose2d SwerveSubsystem::GetRobotPose() { return swerveDrive.GetPose(); } +frc::Pose2d SwerveSubsystem::GetRobotPose() { + return swerveDrive.GetPose(); +} frc::ChassisSpeeds SwerveSubsystem::GetFieldRelativeSpeed() { return frc::ChassisSpeeds::FromRobotRelativeSpeeds( @@ -63,8 +75,8 @@ units::ampere_t SwerveSubsystem::GetSimulatedCurrentDraw() const { return swerveDrive.GetSimulatedCurrentDraw(); } -frc2::CommandPtr -SwerveSubsystem::FollowChoreoTrajectory(std::function pathName) { +frc2::CommandPtr SwerveSubsystem::FollowChoreoTrajectory( + std::function pathName) { return frc2::cmd::Either( frc2::cmd::Sequence( frc2::cmd::RunOnce([this, pathName] { @@ -104,7 +116,7 @@ frc2::CommandPtr SwerveSubsystem::PointWheelsToAngle( return frc2::cmd::RunOnce( [this, wheelAngle] { std::array states; - for (auto &state : states) { + for (auto& state : states) { state.angle = wheelAngle(); state.speed = 0_mps; } @@ -146,7 +158,7 @@ void SwerveSubsystem::SetupPathplanner() { } void SwerveSubsystem::LoadChoreoTrajectories() { - for (const auto &entry : std::filesystem::directory_iterator( + for (const auto& entry : std::filesystem::directory_iterator( frc::filesystem::GetDeployDirectory() + "/choreo/")) { std::string fileName = entry.path().stem().string(); if (fileName != "choreo") { @@ -247,13 +259,15 @@ void SwerveSubsystem::CalculateFoundNotePose( } } -frc::Pose2d SwerveSubsystem::GetFoundNotePose() const { return latestNotePose; } +frc::Pose2d SwerveSubsystem::GetFoundNotePose() const { + return latestNotePose; +} -frc2::CommandPtr -SwerveSubsystem::Drive(std::function xVel, - std::function yVel, - std::function omega, - bool fieldRelative, bool openLoop) { +frc2::CommandPtr SwerveSubsystem::Drive( + std::function xVel, + std::function yVel, + std::function omega, bool fieldRelative, + bool openLoop) { return frc2::cmd::Run( [this, xVel, yVel, omega, fieldRelative, openLoop] { swerveDrive.Drive(xVel(), yVel(), omega(), fieldRelative, @@ -263,8 +277,8 @@ SwerveSubsystem::Drive(std::function xVel, .WithName("Drive Command"); } -frc2::CommandPtr -SwerveSubsystem::PIDToPose(std::function goalPose) { +frc2::CommandPtr SwerveSubsystem::PIDToPose( + std::function goalPose) { return frc2::cmd::Sequence( frc2::cmd::RunOnce( [this, goalPose] { @@ -336,40 +350,40 @@ frc2::CommandPtr SwerveSubsystem::AlignToAmp() { .WithName("AlignToAmp"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4iQuasistaticTorque(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4iQuasistaticTorque( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }), steerMk4iTorqueSysid.Quasistatic(dir)) .WithName("Steer Mk4i Quasistatic Torque"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4iDynamicTorque(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4iDynamicTorque( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }), steerMk4iTorqueSysid.Dynamic(dir)) .WithName("Steer Mk4i Dynamic Torque"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4nQuasistaticTorque(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4nQuasistaticTorque( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }), steerMk4nTorqueSysid.Quasistatic(dir)) .WithName("Steer Mk4n Quasistatic Torque"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4nDynamicTorque(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4nDynamicTorque( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }), steerMk4nTorqueSysid.Dynamic(dir)) .WithName("Steer Mk4n Dynamic Torque"); } -frc2::CommandPtr -SwerveSubsystem::SysIdDriveQuasistaticTorque(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdDriveQuasistaticTorque( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -380,8 +394,8 @@ SwerveSubsystem::SysIdDriveQuasistaticTorque(frc2::sysid::Direction dir) { .WithName("Drive Quasistatic Torque"); } -frc2::CommandPtr -SwerveSubsystem::SysIdDriveDynamicTorque(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdDriveDynamicTorque( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -392,8 +406,8 @@ SwerveSubsystem::SysIdDriveDynamicTorque(frc2::sysid::Direction dir) { .WithName("Drive Dynamic Torque"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4iQuasistaticVoltage(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4iQuasistaticVoltage( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -401,8 +415,8 @@ SwerveSubsystem::SysIdSteerMk4iQuasistaticVoltage(frc2::sysid::Direction dir) { .WithName("Steer Mk4i Quasistatic Voltage"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4iDynamicVoltage(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4iDynamicVoltage( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -410,8 +424,8 @@ SwerveSubsystem::SysIdSteerMk4iDynamicVoltage(frc2::sysid::Direction dir) { .WithName("Steer Mk4i Dynamic Voltage"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4nQuasistaticVoltage(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4nQuasistaticVoltage( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -419,8 +433,8 @@ SwerveSubsystem::SysIdSteerMk4nQuasistaticVoltage(frc2::sysid::Direction dir) { .WithName("Steer Mk4n Quasistatic Voltage"); } -frc2::CommandPtr -SwerveSubsystem::SysIdSteerMk4nDynamicVoltage(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdSteerMk4nDynamicVoltage( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -428,8 +442,8 @@ SwerveSubsystem::SysIdSteerMk4nDynamicVoltage(frc2::sysid::Direction dir) { .WithName("Steer Mk4n Dynamic Voltage"); } -frc2::CommandPtr -SwerveSubsystem::SysIdDriveQuasistaticVoltage(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdDriveQuasistaticVoltage( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), @@ -440,8 +454,8 @@ SwerveSubsystem::SysIdDriveQuasistaticVoltage(frc2::sysid::Direction dir) { .WithName("Drive Quasistatic Voltage"); } -frc2::CommandPtr -SwerveSubsystem::SysIdDriveDynamicVoltage(frc2::sysid::Direction dir) { +frc2::CommandPtr SwerveSubsystem::SysIdDriveDynamicVoltage( + frc2::sysid::Direction dir) { return frc2::cmd::Sequence( frc2::cmd::RunOnce([] { ctre::phoenix6::SignalLogger::Start(); }, {this}), diff --git a/src/main/include/Autos.h b/src/main/include/Autos.h index 66850cc..20a8ea3 100644 --- a/src/main/include/Autos.h +++ b/src/main/include/Autos.h @@ -15,10 +15,12 @@ #include class Autos { -public: - explicit Autos(SwerveSubsystem &swerveSub, ShooterSubsystem &shooterSub, - IntakeSubsystem &intakeSub, FeederSubsystem &feederSub) - : swerveSub(swerveSub), shooterSub(shooterSub), intakeSub(intakeSub), + public: + explicit Autos(SwerveSubsystem& swerveSub, ShooterSubsystem& shooterSub, + IntakeSubsystem& intakeSub, FeederSubsystem& feederSub) + : swerveSub(swerveSub), + shooterSub(shooterSub), + intakeSub(intakeSub), feederSub(feederSub) { pathplanner::NamedCommands::registerCommand( "Shoot", frc2::cmd::Print("Shooting Note")); @@ -36,17 +38,17 @@ class Autos { frc::SmartDashboard::PutData("Auto Chooser", &autoChooser); } - frc2::Command *GetSelectedCommand() { return selectCommand.get(); } + frc2::Command* GetSelectedCommand() { return selectCommand.get(); } -private: + private: enum AutoSelector { CHOREO_TEST, CHOREO_LIME }; frc::SendableChooser autoChooser; - SwerveSubsystem &swerveSub; - ShooterSubsystem &shooterSub; - IntakeSubsystem &intakeSub; - FeederSubsystem &feederSub; + SwerveSubsystem& swerveSub; + ShooterSubsystem& shooterSub; + IntakeSubsystem& intakeSub; + FeederSubsystem& feederSub; frc2::CommandPtr selectCommand{frc2::cmd::None()}; }; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 4760855..28a4d6c 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -18,7 +18,7 @@ #include "str/SwerveModule.h" class Robot : public frc::TimedRobot { -public: + public: Robot() : frc::TimedRobot(consts::LOOP_PERIOD) {} void RobotInit() override; void RobotPeriodic() override; @@ -38,8 +38,8 @@ class Robot : public frc::TimedRobot { void SimulationPeriodic() override; void UpdateVision(); -private: - frc2::Command *m_autonomousCommand = nullptr; + private: + frc2::Command* m_autonomousCommand = nullptr; RobotContainer m_container; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 7cfa180..17d1548 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -16,28 +16,28 @@ #include "subsystems/SwerveSubsystem.h" class RobotContainer { -public: + public: RobotContainer(); - frc2::Command *GetAutonomousCommand(); + frc2::Command* GetAutonomousCommand(); frc2::CommandPtr IntakeNote(); - SwerveSubsystem &GetSwerveSubsystem(); - ShooterSubsystem &GetShooterSubsystem(); - IntakeSubsystem &GetIntakeSubsystem(); - FeederSubsystem &GetFeederSubsystem(); + SwerveSubsystem& GetSwerveSubsystem(); + ShooterSubsystem& GetShooterSubsystem(); + IntakeSubsystem& GetIntakeSubsystem(); + FeederSubsystem& GetFeederSubsystem(); // str::Vision &GetVision(); - str::NoteVisualizer &GetNoteVisualizer(); + str::NoteVisualizer& GetNoteVisualizer(); -private: + private: void ConfigureBindings(); frc2::CommandXboxController driverController{0}; frc2::CommandXboxController operatorController{1}; frc2::CommandPtr RumbleDriver(std::function timeToRumble); - frc2::CommandPtr - RumbleOperator(std::function timeToRumble); + frc2::CommandPtr RumbleOperator( + std::function timeToRumble); SwerveSubsystem swerveSubsystem; ShooterSubsystem shooterSubsystem; diff --git a/src/main/include/constants/Constants.h b/src/main/include/constants/Constants.h index d58c0db..35977a7 100644 --- a/src/main/include/constants/Constants.h +++ b/src/main/include/constants/Constants.h @@ -19,5 +19,5 @@ inline const frc::AprilTagFieldLayout aprilTagLayout = inline constexpr frc::Translation2d inFrontOfAmpLocation{1.838_m, 7.172_m}; inline constexpr frc::Translation2d ampLocation{1.838_m, 7.782_m}; inline constexpr units::meter_t closeToAmpDistance{5_ft}; -} // namespace yearSpecific -} // namespace consts +} // namespace yearSpecific +} // namespace consts diff --git a/src/main/include/constants/FeederConstants.h b/src/main/include/constants/FeederConstants.h index 913f67b..e926ffd 100644 --- a/src/main/include/constants/FeederConstants.h +++ b/src/main/include/constants/FeederConstants.h @@ -17,16 +17,16 @@ namespace consts { namespace feeder { namespace can_ids { inline constexpr int FEEDER = 18; -} // namespace can_ids +} // namespace can_ids namespace ports { inline constexpr int NOTE_SENSOR_PORT = 2; -} // namespace ports +} // namespace ports namespace current_limits { inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A; inline constexpr units::ampere_t STATOR_CURRENT_LIMIT = 180_A; -} // namespace current_limits +} // namespace current_limits namespace physical { @@ -41,12 +41,12 @@ inline constexpr units::meter_t WHEEL_RADIUS = 1_in; // From onshape doc inline constexpr units::kilogram_square_meter_t FEEDER_MOI = 5.332702 * 1_in * 1_in * 1_lb; -} // namespace physical +} // namespace physical namespace gains { inline constexpr units::volt_t NOTE_FEED_VOLTAGE = 10_V; inline constexpr units::volt_t NOTE_EJECT_VOLTAGE = -10_V; inline constexpr units::second_t NOTE_SENSOR_DEBOUNCE_TIME = 0.1_s; -} // namespace gains -} // namespace feeder -} // namespace consts +} // namespace gains +} // namespace feeder +} // namespace consts diff --git a/src/main/include/constants/IntakeConstants.h b/src/main/include/constants/IntakeConstants.h index ac1643a..3019881 100644 --- a/src/main/include/constants/IntakeConstants.h +++ b/src/main/include/constants/IntakeConstants.h @@ -17,12 +17,12 @@ namespace consts { namespace intake { namespace can_ids { inline constexpr int INTAKE = 17; -} // namespace can_ids +} // namespace can_ids namespace current_limits { inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A; inline constexpr units::ampere_t STATOR_CURRENT_LIMIT = 180_A; -} // namespace current_limits +} // namespace current_limits namespace physical { @@ -37,12 +37,12 @@ inline constexpr units::meter_t WHEEL_RADIUS = 1_in; // From onshape doc inline constexpr units::kilogram_square_meter_t INTAKE_MOI = 5.350445 * 1_in * 1_in * 1_lb; -} // namespace physical +} // namespace physical namespace gains { inline constexpr units::ampere_t NOTE_SPIKE_THRESHOLD = 100_A; inline constexpr units::volt_t NOTE_INTAKE_VOLTAGE = 10_V; inline constexpr units::volt_t NOTE_EJECT_VOLTAGE = -10_V; -} // namespace gains -} // namespace intake -} // namespace consts +} // namespace gains +} // namespace intake +} // namespace consts diff --git a/src/main/include/constants/ShooterConstants.h b/src/main/include/constants/ShooterConstants.h index efd5ce2..e4c4ad7 100644 --- a/src/main/include/constants/ShooterConstants.h +++ b/src/main/include/constants/ShooterConstants.h @@ -16,12 +16,12 @@ namespace shooter { namespace can_ids { inline constexpr int TOP_SHOOTER = 15; inline constexpr int BOTTOM_SHOOTER = 16; -} // namespace can_ids +} // namespace can_ids namespace current_limits { inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A; inline constexpr units::ampere_t STATOR_CURRENT_LIMIT = 180_A; -} // namespace current_limits +} // namespace current_limits namespace physical { @@ -37,7 +37,7 @@ inline constexpr units::meter_t WHEEL_RADIUS = 2_in; // From onshape doc inline constexpr units::kilogram_square_meter_t FLYWHEEL_MOI = 5.01 * 1_in * 1_in * 1_lb; -} // namespace physical +} // namespace physical namespace gains { inline constexpr units::turns_per_second_t VEL_TOLERANCE = 10_rpm; @@ -47,7 +47,7 @@ inline constexpr units::volt_t SHOOTER_KS{0.02364}; inline constexpr str::gains::radial::turn_volt_kp_unit_t SHOOTER_KP{0.047275}; inline constexpr str::gains::radial::turn_volt_ki_unit_t SHOOTER_KI{0}; inline constexpr str::gains::radial::turn_volt_kd_unit_t SHOOTER_KD{0}; -} // namespace gains +} // namespace gains struct ShooterSpeeds { units::turns_per_second_t topSpeed; @@ -59,7 +59,7 @@ inline constexpr ShooterSpeeds SUBWOOFER_SPEEDS{5000_rpm, 5000_rpm}; inline constexpr ShooterSpeeds PASS_SPEEDS{5000_rpm, 5000_rpm}; struct MeterHash { - size_t operator()(const units::meter_t &m) const { + size_t operator()(const units::meter_t& m) const { return std::hash{}(m.value()); } }; @@ -70,5 +70,5 @@ inline static wpi::interpolating_map BOTTOM_SHOOTER_LUT{}; enum class PRESET_SPEEDS { OFF, AMP, SPEAKER_DIST, SUBWOOFER, PASS }; -} // namespace shooter -} // namespace consts +} // namespace shooter +} // namespace consts diff --git a/src/main/include/constants/SwerveConstants.h b/src/main/include/constants/SwerveConstants.h index 260c874..27fd9b5 100644 --- a/src/main/include/constants/SwerveConstants.h +++ b/src/main/include/constants/SwerveConstants.h @@ -32,13 +32,13 @@ inline constexpr int BR_STEER = 12; inline constexpr int BR_ENC = 13; inline constexpr int IMU = 14; -} // namespace can_ids +} // namespace can_ids namespace current_limits { inline constexpr units::ampere_t STEER_TORQUE_CURRENT_LIMIT = 40_A; inline constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 60_A; inline constexpr units::ampere_t SLIP_CURRENT_LIMIT = 180_A; -} // namespace current_limits +} // namespace current_limits namespace physical { @@ -134,7 +134,7 @@ inline constexpr units::meters_per_second_squared_t DRIVE_MAX_ACCEL = 2000_fps_sq; inline constexpr units::radians_per_second_squared_t DRIVE_MAX_ROT_ACCEL = 720_deg_per_s_sq; -} // namespace physical +} // namespace physical namespace gains { inline constexpr units::radians_per_second_t MK4I_STEER_CRUISE_VEL = @@ -172,7 +172,7 @@ inline constexpr units::volt_t DRIVE_KS_V{1}; inline constexpr str::gains::radial::turn_amp_kp_unit_t DRIVE_KP{9}; inline constexpr str::gains::radial::turn_amp_ki_unit_t DRIVE_KI{0}; inline constexpr str::gains::radial::turn_amp_kd_unit_t DRIVE_KD{0}; -} // namespace gains +} // namespace gains namespace pathplanning { @@ -212,6 +212,6 @@ inline constexpr units::meters_per_second_t translationalVelPIDDeadband = 0.5_fps; inline constexpr units::radians_per_second_t rotationalVelPIDDeadband = 5_deg_per_s; -} // namespace pathplanning -} // namespace swerve -} // namespace consts +} // namespace pathplanning +} // namespace swerve +} // namespace consts diff --git a/src/main/include/constants/VisionConstants.h b/src/main/include/constants/VisionConstants.h index 7934f17..8ed2bdc 100644 --- a/src/main/include/constants/VisionConstants.h +++ b/src/main/include/constants/VisionConstants.h @@ -37,5 +37,5 @@ inline const std::string NOTE_CAM_NAME{"note_cam"}; inline const frc::Transform3d ROBOT_TO_NOTE_CAM{ frc::Translation3d{-15.664852_in, 0_in, 9.437251_in}, frc::Rotation3d{0_rad, 20_deg, 180_deg}}; -} // namespace vision -} // namespace consts +} // namespace vision +} // namespace consts diff --git a/src/main/include/str/Camera.h b/src/main/include/str/Camera.h index a1af5d2..083a3da 100644 --- a/src/main/include/str/Camera.h +++ b/src/main/include/str/Camera.h @@ -23,7 +23,7 @@ namespace str { class Camera { -public: + public: Camera(std::string cameraName, frc::Transform3d robotToCamera, Eigen::Matrix singleTagStdDev, Eigen::Matrix multiTagDevs, bool simulate); @@ -34,7 +34,7 @@ class Camera { std::optional GetDistanceToNote(); std::optional GetAngleToNote(); -private: + private: bool simulate; std::unique_ptr photonEstimator; std::unique_ptr camera; @@ -59,8 +59,8 @@ class Camera { units::radian_t cacheYaw = 0_rad; units::meter_t cacheDist = 0_m; - std::vector - CreateTorusVertices(units::meter_t majorRadius, units::meter_t minorRadius, - int numMajorDivisions, int numMinorDivisons); + std::vector CreateTorusVertices( + units::meter_t majorRadius, units::meter_t minorRadius, + int numMajorDivisions, int numMinorDivisons); }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/ChoreoSwerveCommandWithForce.h b/src/main/include/str/ChoreoSwerveCommandWithForce.h index f8318e9..6efe2ae 100644 --- a/src/main/include/str/ChoreoSwerveCommandWithForce.h +++ b/src/main/include/str/ChoreoSwerveCommandWithForce.h @@ -19,7 +19,7 @@ namespace str { */ class ChoreoSwerveCommandWithForce : public frc2::CommandHelper { -public: + public: /** * Creates a new ChoreoSwerveCommand that controls a swerve drivetrain * @@ -60,7 +60,7 @@ class ChoreoSwerveCommandWithForce /// Command will end once this returns true bool IsFinished() override; -private: + private: frc::Timer m_timer; choreolib::ChoreoTrajectory m_traj; std::function m_pose; @@ -70,4 +70,4 @@ class ChoreoSwerveCommandWithForce std::function)> m_outputYForce; std::function m_mirrorTrajectory; }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/DataUtils.h b/src/main/include/str/DataUtils.h index ff6a3dd..def8d64 100644 --- a/src/main/include/str/DataUtils.h +++ b/src/main/include/str/DataUtils.h @@ -6,8 +6,8 @@ namespace str { class DataUtils { -public: + public: DataUtils() = delete; static void LogGitInfo(); }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/DriverstationUtils.h b/src/main/include/str/DriverstationUtils.h index 925afb9..1fba571 100644 --- a/src/main/include/str/DriverstationUtils.h +++ b/src/main/include/str/DriverstationUtils.h @@ -18,7 +18,8 @@ static bool IsOnRed() { return false; } -template static T NegateIfRed(T input) { +template +static T NegateIfRed(T input) { return IsOnRed() ? input * -1 : input; } -} // namespace str +} // namespace str diff --git a/src/main/include/str/Gains.h b/src/main/include/str/Gains.h index b50ebbb..47eaa80 100644 --- a/src/main/include/str/Gains.h +++ b/src/main/include/str/Gains.h @@ -11,7 +11,7 @@ namespace str { namespace gains { -namespace linear {} // namespace linear +namespace linear {} // namespace linear namespace radial { using radial_velocity = @@ -61,6 +61,6 @@ using turn_amp_kd_unit = units::compound_unit>; using turn_amp_kd_unit_t = units::unit_t; -} // namespace radial -} // namespace gains -} // namespace str +} // namespace radial +} // namespace gains +} // namespace str diff --git a/src/main/include/str/Math.h b/src/main/include/str/Math.h index a8bff85..acca68a 100644 --- a/src/main/include/str/Math.h +++ b/src/main/include/str/Math.h @@ -8,13 +8,13 @@ namespace str { namespace math { -static bool IsPointInsideField(const frc::Translation2d &point) { +static bool IsPointInsideField(const frc::Translation2d& point) { return point.X() >= 0_m && point.Y() >= 0_m && point.X() <= consts::yearSpecific::aprilTagLayout.GetFieldLength() && point.Y() <= consts::yearSpecific::aprilTagLayout.GetFieldWidth(); } -static bool IsPoseInsideField(const frc::Pose2d &poseToCheck) { +static bool IsPoseInsideField(const frc::Pose2d& poseToCheck) { units::radian_t currentRotation = poseToCheck.Rotation().Radians(); units::meter_t ox = (consts::swerve::physical::TOTAL_WIDTH / 2); units::meter_t oy = (consts::swerve::physical::TOTAL_LENGTH / 2); @@ -48,5 +48,5 @@ static bool IsPoseInsideField(const frc::Pose2d &poseToCheck) { math::IsPointInsideField(blCorner) && math::IsPointInsideField(brCorner); } -} // namespace math -} // namespace str +} // namespace math +} // namespace str diff --git a/src/main/include/str/NoteVisualizer.h b/src/main/include/str/NoteVisualizer.h index 8fe5535..18a08f7 100644 --- a/src/main/include/str/NoteVisualizer.h +++ b/src/main/include/str/NoteVisualizer.h @@ -32,18 +32,18 @@ struct FlyingNote { }; class NoteVisualizer { -public: + public: NoteVisualizer(); void Periodic(); void LaunchNote(frc::Pose3d currentRobotPose, frc::ChassisSpeeds robotCurrentVelocity, frc::Transform3d noteExitPose, units::meters_per_second_t initialVelocity); - void DisplayRobotNote(bool hasNote, const frc::Pose2d &robotPosition); + void DisplayRobotNote(bool hasNote, const frc::Pose2d& robotPosition); -private: + private: void UpdateLaunchedNotes(units::second_t loopTime); - void ProjectileMotion(FlyingNote ¬e, units::second_t loopTime); + void ProjectileMotion(FlyingNote& note, units::second_t loopTime); void CleanUp(); units::second_t lastLoopTime; @@ -63,44 +63,44 @@ class NoteVisualizer { std::array initialNoteLocations{ // BLUE SIDE - frc::Pose3d{frc::Translation3d{NOTE_HORIZONTAL_SPACING, - ALLY_NOTE_VERTICAL_OFFSET, - NOTE_GROUND_HEIGHT}, - frc::Rotation3d{}}, - frc::Pose3d{frc::Translation3d{NOTE_HORIZONTAL_SPACING, - ALLY_NOTE_VERTICAL_OFFSET + - (NOTE_VERTICAL_SPACING * 1), - NOTE_GROUND_HEIGHT}, + frc::Pose3d{ + frc::Translation3d{NOTE_HORIZONTAL_SPACING, ALLY_NOTE_VERTICAL_OFFSET, + NOTE_GROUND_HEIGHT}, + frc::Rotation3d{}}, + frc::Pose3d{frc::Translation3d{ + NOTE_HORIZONTAL_SPACING, + ALLY_NOTE_VERTICAL_OFFSET + (NOTE_VERTICAL_SPACING * 1), + NOTE_GROUND_HEIGHT}, frc::Rotation3d{}}, - frc::Pose3d{frc::Translation3d{NOTE_HORIZONTAL_SPACING, - ALLY_NOTE_VERTICAL_OFFSET + - (NOTE_VERTICAL_SPACING * 2), - NOTE_GROUND_HEIGHT}, + frc::Pose3d{frc::Translation3d{ + NOTE_HORIZONTAL_SPACING, + ALLY_NOTE_VERTICAL_OFFSET + (NOTE_VERTICAL_SPACING * 2), + NOTE_GROUND_HEIGHT}, frc::Rotation3d{}}, // MIDDLE - frc::Pose3d{frc::Translation3d{NOTE_MIDDLE_OFFSET, - MIDDLE_NOTE_VERTICAL_OFFSET, - NOTE_GROUND_HEIGHT}, - frc::Rotation3d{}}, - frc::Pose3d{frc::Translation3d{NOTE_MIDDLE_OFFSET, - MIDDLE_NOTE_VERTICAL_OFFSET - - (NOTE_MIDDLE_SPACING * 1), - NOTE_GROUND_HEIGHT}, + frc::Pose3d{ + frc::Translation3d{NOTE_MIDDLE_OFFSET, MIDDLE_NOTE_VERTICAL_OFFSET, + NOTE_GROUND_HEIGHT}, + frc::Rotation3d{}}, + frc::Pose3d{frc::Translation3d{ + NOTE_MIDDLE_OFFSET, + MIDDLE_NOTE_VERTICAL_OFFSET - (NOTE_MIDDLE_SPACING * 1), + NOTE_GROUND_HEIGHT}, frc::Rotation3d{}}, - frc::Pose3d{frc::Translation3d{NOTE_MIDDLE_OFFSET, - MIDDLE_NOTE_VERTICAL_OFFSET - - (NOTE_MIDDLE_SPACING * 2), - NOTE_GROUND_HEIGHT}, + frc::Pose3d{frc::Translation3d{ + NOTE_MIDDLE_OFFSET, + MIDDLE_NOTE_VERTICAL_OFFSET - (NOTE_MIDDLE_SPACING * 2), + NOTE_GROUND_HEIGHT}, frc::Rotation3d{}}, - frc::Pose3d{frc::Translation3d{NOTE_MIDDLE_OFFSET, - MIDDLE_NOTE_VERTICAL_OFFSET - - (NOTE_MIDDLE_SPACING * 3), - NOTE_GROUND_HEIGHT}, + frc::Pose3d{frc::Translation3d{ + NOTE_MIDDLE_OFFSET, + MIDDLE_NOTE_VERTICAL_OFFSET - (NOTE_MIDDLE_SPACING * 3), + NOTE_GROUND_HEIGHT}, frc::Rotation3d{}}, - frc::Pose3d{frc::Translation3d{NOTE_MIDDLE_OFFSET, - MIDDLE_NOTE_VERTICAL_OFFSET - - (NOTE_MIDDLE_SPACING * 4), - NOTE_GROUND_HEIGHT}, + frc::Pose3d{frc::Translation3d{ + NOTE_MIDDLE_OFFSET, + MIDDLE_NOTE_VERTICAL_OFFSET - (NOTE_MIDDLE_SPACING * 4), + NOTE_GROUND_HEIGHT}, frc::Rotation3d{}}, // RED SIDE frc::Pose3d{frc::Translation3d{ @@ -131,4 +131,4 @@ class NoteVisualizer { nt::StructPublisher robotNotePub{ nt->GetStructTopic("RobotNote").Publish()}; }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/SwerveDrive.h b/src/main/include/str/SwerveDrive.h index e310842..cf6bd9d 100644 --- a/src/main/include/str/SwerveDrive.h +++ b/src/main/include/str/SwerveDrive.h @@ -22,23 +22,23 @@ namespace str { class SwerveDrive { -public: + public: SwerveDrive(); - void ResetPose(const frc::Pose2d &resetPose); - void DriveRobotRelative(const frc::ChassisSpeeds &robotRelativeSpeeds); + void ResetPose(const frc::Pose2d& resetPose); + void DriveRobotRelative(const frc::ChassisSpeeds& robotRelativeSpeeds); void Drive(units::meters_per_second_t xVel, units::meters_per_second_t yVel, units::radians_per_second_t omega, bool fieldRelative, bool openLoop = false); void SetModuleStates( - const std::array &desiredStates, + const std::array& desiredStates, bool optimize = true, bool openLoop = false, - const std::array &moduleTorqueCurrentsFF = {}); - void AddVisionMeasurement(const frc::Pose2d &visionMeasurement, + const std::array& moduleTorqueCurrentsFF = {}); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp, - const Eigen::Vector3d &stdDevs); + const Eigen::Vector3d& stdDevs); units::ampere_t GetSimulatedCurrentDraw() const; - void SetXModuleForces(const std::array &xForce); - void SetYModuleForces(const std::array &yForce); + void SetXModuleForces(const std::array& xForce); + void SetYModuleForces(const std::array& yForce); void UpdateSwerveOdom(); void UpdateNTEntries(); frc::ChassisSpeeds GetRobotRelativeSpeeds() const; @@ -55,22 +55,22 @@ class SwerveDrive { void SetMk4iCharacterizationVoltageSteer(units::volt_t volts); void SetMk4nCharacterizationVoltageSteer(units::volt_t volts); void SetCharacterizationVoltageDrive(units::volt_t volts); - void LogMk4iSteerTorque(frc::sysid::SysIdRoutineLog *log); - void LogMk4nSteerTorque(frc::sysid::SysIdRoutineLog *log); - void LogDriveTorque(frc::sysid::SysIdRoutineLog *log); - void LogMk4iSteerVoltage(frc::sysid::SysIdRoutineLog *log); - void LogMk4nSteerVoltage(frc::sysid::SysIdRoutineLog *log); - void LogDriveVoltage(frc::sysid::SysIdRoutineLog *log); + void LogMk4iSteerTorque(frc::sysid::SysIdRoutineLog* log); + void LogMk4nSteerTorque(frc::sysid::SysIdRoutineLog* log); + void LogDriveTorque(frc::sysid::SysIdRoutineLog* log); + void LogMk4iSteerVoltage(frc::sysid::SysIdRoutineLog* log); + void LogMk4nSteerVoltage(frc::sysid::SysIdRoutineLog* log); + void LogDriveVoltage(frc::sysid::SysIdRoutineLog* log); str::SwerveModuleSteerGains GetSteerGains() const; void SetSteerGains(str::SwerveModuleSteerGains newGains); str::SwerveModuleDriveGains GetDriveGains() const; void SetDriveGains(str::SwerveModuleDriveGains newGains); -private: + private: std::array ConvertModuleForcesToTorqueCurrent( - const std::array &xForce, - const std::array &yForce); + const std::array& xForce, + const std::array& yForce); bool IsSlipping(); @@ -141,9 +141,9 @@ class SwerveDrive { swervePhysicalFront, steerGainsMk4i, driveGains}}; ctre::phoenix6::hardware::Pigeon2 imu{consts::swerve::can_ids::IMU, "*"}; - ctre::phoenix6::sim::Pigeon2SimState &imuSimState = imu.GetSimState(); + ctre::phoenix6::sim::Pigeon2SimState& imuSimState = imu.GetSimState(); - std::array allSignals; + std::array allSignals; std::array modulePositions; std::array moduleStates; @@ -186,4 +186,4 @@ class SwerveDrive { nt::DoublePublisher odomUpdateRatePub{ nt->GetDoubleTopic("OdomUpdateRate").Publish()}; }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/SwerveModule.h b/src/main/include/str/SwerveModule.h index b954fc3..1876810 100644 --- a/src/main/include/str/SwerveModule.h +++ b/src/main/include/str/SwerveModule.h @@ -22,7 +22,7 @@ namespace str { class SwerveModule { -public: + public: explicit SwerveModule(SwerveModuleConstants constants, SwerveModulePhysical physicalAttrib, SwerveModuleSteerGains steerGains, @@ -37,7 +37,7 @@ class SwerveModule { frc::SwerveModuleState GetCurrentState(); frc::SwerveModuleState UpdateSimulation(units::second_t deltaTime, units::volt_t supplyVoltage); - std::array GetSignals(); + std::array GetSignals(); bool OptimizeBusSignals(); std::string GetName() const; units::ampere_t GetSimulatedCurrentDraw() const; @@ -50,7 +50,7 @@ class SwerveModule { str::SwerveModuleSteerGains GetSteerGains() const; str::SwerveModuleDriveGains GetDriveGains() const; -private: + private: bool ConfigureSteerMotor(bool invertSteer, units::scalar_t steerGearing, units::ampere_t supplyCurrentLimit, units::ampere_t torqueCurrentLimit); @@ -60,16 +60,16 @@ class SwerveModule { void ConfigureControlSignals(); units::radian_t ConvertDriveMotorRotationsToWheelRotations( units::radian_t motorRotations) const; - units::radians_per_second_t - ConvertDriveMotorVelToWheelVel(units::radians_per_second_t motorVel) const; - units::meter_t - ConvertWheelRotationsToWheelDistance(units::radian_t wheelRotations) const; - units::meters_per_second_t - ConvertWheelVelToLinearVel(units::radians_per_second_t wheelVel) const; - units::radians_per_second_t - ConvertLinearVelToWheelVel(units::meters_per_second_t linVel) const; - units::radians_per_second_t - ConvertWheelVelToMotorVel(units::radians_per_second_t wheelVel) const; + units::radians_per_second_t ConvertDriveMotorVelToWheelVel( + units::radians_per_second_t motorVel) const; + units::meter_t ConvertWheelRotationsToWheelDistance( + units::radian_t wheelRotations) const; + units::meters_per_second_t ConvertWheelVelToLinearVel( + units::radians_per_second_t wheelVel) const; + units::radians_per_second_t ConvertLinearVelToWheelVel( + units::meters_per_second_t linVel) const; + units::radians_per_second_t ConvertWheelVelToMotorVel( + units::radians_per_second_t wheelVel) const; ctre::phoenix6::hardware::TalonFX steerMotor; ctre::phoenix6::hardware::TalonFX driveMotor; @@ -117,4 +117,4 @@ class SwerveModule { SwerveModuleSim moduleSim; }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/SwerveModuleHelpers.h b/src/main/include/str/SwerveModuleHelpers.h index 4e8401b..03bb1a9 100644 --- a/src/main/include/str/SwerveModuleHelpers.h +++ b/src/main/include/str/SwerveModuleHelpers.h @@ -49,7 +49,7 @@ struct SwerveModuleSteerGains { str::gains::radial::turn_amp_ki_unit_t kI; str::gains::radial::turn_amp_kd_unit_t kD; - bool operator==(const SwerveModuleSteerGains &rhs) const { + bool operator==(const SwerveModuleSteerGains& rhs) const { return units::essentiallyEqual(kA, rhs.kA, 1e-6) && units::essentiallyEqual(kV, rhs.kV, 1e-6) && units::essentiallyEqual(kS, rhs.kS, 1e-6) && @@ -57,7 +57,7 @@ struct SwerveModuleSteerGains { units::essentiallyEqual(kI, rhs.kI, 1e-6) && units::essentiallyEqual(kD, rhs.kD, 1e-6); } - bool operator!=(const SwerveModuleSteerGains &rhs) const { + bool operator!=(const SwerveModuleSteerGains& rhs) const { return !operator==(rhs); } }; @@ -70,7 +70,7 @@ struct SwerveModuleDriveGains { str::gains::radial::turn_amp_ki_unit_t kI; str::gains::radial::turn_amp_kd_unit_t kD; - bool operator==(const SwerveModuleDriveGains &rhs) const { + bool operator==(const SwerveModuleDriveGains& rhs) const { return units::essentiallyEqual(kA, rhs.kA, 1e-6) && units::essentiallyEqual(kV, rhs.kV, 1e-6) && units::essentiallyEqual(kS, rhs.kS, 1e-6) && @@ -78,7 +78,7 @@ struct SwerveModuleDriveGains { units::essentiallyEqual(kI, rhs.kI, 1e-6) && units::essentiallyEqual(kD, rhs.kD, 1e-6); } - bool operator!=(const SwerveModuleDriveGains &rhs) const { + bool operator!=(const SwerveModuleDriveGains& rhs) const { return !operator==(rhs); } }; @@ -91,4 +91,4 @@ struct WheelRadiusCharData { frc::SlewRateLimiter omegaLimiter{1_rad_per_s / 1_s}; }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/SwerveModuleSim.h b/src/main/include/str/SwerveModuleSim.h index 962bab0..9c3cfc1 100644 --- a/src/main/include/str/SwerveModuleSim.h +++ b/src/main/include/str/SwerveModuleSim.h @@ -15,28 +15,28 @@ namespace str { class SwerveModuleSim { -public: + public: explicit SwerveModuleSim( SwerveModuleConstants constants, SwerveModulePhysical physicalAttrib, - ctre::phoenix6::sim::TalonFXSimState &driveSimState, - ctre::phoenix6::sim::TalonFXSimState &steerSimState, - ctre::phoenix6::sim::CANcoderSimState &steerEncoderSimState); + ctre::phoenix6::sim::TalonFXSimState& driveSimState, + ctre::phoenix6::sim::TalonFXSimState& steerSimState, + ctre::phoenix6::sim::CANcoderSimState& steerEncoderSimState); frc::SwerveModuleState Update(units::second_t deltaTime, units::volt_t supplyVoltage); units::ampere_t GetSteerCurrentDraw() const; units::ampere_t GetDriveCurrentDraw() const; -private: + private: units::volt_t AddFrictionVoltage(units::volt_t outputVoltage, units::volt_t frictionVoltage); frc::sim::DCMotorSim driveSim; frc::sim::DCMotorSim steerSim; - ctre::phoenix6::sim::TalonFXSimState &driveSimState; - ctre::phoenix6::sim::TalonFXSimState &steerSimState; - ctre::phoenix6::sim::CANcoderSimState &steerEncoderSimState; + ctre::phoenix6::sim::TalonFXSimState& driveSimState; + ctre::phoenix6::sim::TalonFXSimState& steerSimState; + ctre::phoenix6::sim::CANcoderSimState& steerEncoderSimState; bool driveInverted; bool steerInverted; @@ -50,4 +50,4 @@ class SwerveModuleSim { units::meter_t wheelRadius; }; -} // namespace str +} // namespace str diff --git a/src/main/include/str/Units.h b/src/main/include/str/Units.h index 1e86ae5..368572f 100644 --- a/src/main/include/str/Units.h +++ b/src/main/include/str/Units.h @@ -9,10 +9,11 @@ namespace units { -template bool essentiallyEqual(T a, T b, units::scalar_t epsilon) { +template +bool essentiallyEqual(T a, T b, units::scalar_t epsilon) { return units::math::abs(a - b) <= ((units::math::abs(a) > units::math::abs(b) ? units::math::abs(b) : units::math::abs(a)) * epsilon); } -} // namespace units +} // namespace units diff --git a/src/main/include/str/Vision.h b/src/main/include/str/Vision.h index 3b2d651..c070fe9 100644 --- a/src/main/include/str/Vision.h +++ b/src/main/include/str/Vision.h @@ -15,17 +15,17 @@ namespace str { class Vision { -public: + public: Vision() = default; void SimulationPeriodic(frc::Pose2d simRobotPose); std::vector> GetCameraEstimatedPoses(); std::vector>> GetPoseStdDevs( - const std::vector> &poses); + const std::vector>& poses); std::optional GetDistanceToNote(); std::optional GetAngleToNote(); -private: + private: std::array cameras{ Camera{consts::vision::FL_CAM_NAME, consts::vision::FL_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, @@ -44,4 +44,4 @@ class Vision { consts::vision::SINGLE_TAG_STD_DEV, consts::vision::MULTI_TAG_STD_DEV, false}; }; -} // namespace str +} // namespace str diff --git a/src/main/include/subsystems/FeederSubsystem.h b/src/main/include/subsystems/FeederSubsystem.h index 968d040..1f44848 100644 --- a/src/main/include/subsystems/FeederSubsystem.h +++ b/src/main/include/subsystems/FeederSubsystem.h @@ -19,10 +19,9 @@ #include #include "frc/DigitalInput.h" -#include "networktables/BooleanTopic.h" class FeederSubsystem : public frc2::SubsystemBase { -public: + public: FeederSubsystem(); frc2::CommandPtr Feed(); @@ -38,7 +37,7 @@ class FeederSubsystem : public frc2::SubsystemBase { void SimulationPeriodic() override; bool HasNote() const; -private: + private: bool ConfigureFeederMotor(bool invert, units::scalar_t intakeGearing, units::ampere_t supplyCurrentLimit, units::ampere_t statorCurrentLimit); @@ -57,7 +56,7 @@ class FeederSubsystem : public frc2::SubsystemBase { feederMotor.GetMotorVoltage(); ctre::phoenix6::controls::VoltageOut feederMotorVoltageSetter{0_V}; - ctre::phoenix6::sim::TalonFXSimState &feederMotorSim = + ctre::phoenix6::sim::TalonFXSimState& feederMotorSim = feederMotor.GetSimState(); frc::LinearSystem<1, 1, 1> feederPlant{frc::LinearSystemId::FlywheelSystem( diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h index e12823f..38603de 100644 --- a/src/main/include/subsystems/IntakeSubsystem.h +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -22,7 +22,7 @@ #include class IntakeSubsystem : public frc2::SubsystemBase { -public: + public: IntakeSubsystem(); /** @@ -38,7 +38,7 @@ class IntakeSubsystem : public frc2::SubsystemBase { frc2::CommandPtr PoopNote(); frc2::CommandPtr FakeNote(); -private: + private: void UpdateNTEntries(); bool ConfigureIntakeMotor(bool invert, units::scalar_t intakeGearing, units::ampere_t supplyCurrentLimit, @@ -53,7 +53,7 @@ class IntakeSubsystem : public frc2::SubsystemBase { intakeMotor.GetTorqueCurrent(); ctre::phoenix6::controls::VoltageOut intakeMotorVoltageSetter{0_V}; - ctre::phoenix6::sim::TalonFXSimState &intakeMotorSim = + ctre::phoenix6::sim::TalonFXSimState& intakeMotorSim = intakeMotor.GetSimState(); frc::LinearSystem<1, 1, 1> intakePlant{frc::LinearSystemId::FlywheelSystem( diff --git a/src/main/include/subsystems/ShooterSubsystem.h b/src/main/include/subsystems/ShooterSubsystem.h index da8429e..9e8b314 100644 --- a/src/main/include/subsystems/ShooterSubsystem.h +++ b/src/main/include/subsystems/ShooterSubsystem.h @@ -24,7 +24,7 @@ #include "constants/ShooterConstants.h" class ShooterSubsystem : public frc2::SubsystemBase { -public: + public: ShooterSubsystem(); /** @@ -33,22 +33,22 @@ class ShooterSubsystem : public frc2::SubsystemBase { void Periodic() override; void SimulationPeriodic() override; void SetupLUTs( - const std::map &speeds); + const std::map& speeds); - frc2::CommandPtr - RunShooter(std::function preset); - frc2::CommandPtr - RunShooter(std::function preset, - std::function distance); + frc2::CommandPtr RunShooter( + std::function preset); + frc2::CommandPtr RunShooter( + std::function preset, + std::function distance); frc2::CommandPtr TopWheelSysIdQuasistatic(frc2::sysid::Direction direction); frc2::CommandPtr TopWheelSysIdDynamic(frc2::sysid::Direction direction); - frc2::CommandPtr - BottomWheelSysIdQuasistatic(frc2::sysid::Direction direction); + frc2::CommandPtr BottomWheelSysIdQuasistatic( + frc2::sysid::Direction direction); frc2::CommandPtr BottomWheelSysIdDynamic(frc2::sysid::Direction direction); - const frc2::Trigger &UpToSpeed() const { return upToSpeedTrigger; } + const frc2::Trigger& UpToSpeed() const { return upToSpeedTrigger; } -private: + private: void UpdateNTEntries(); bool IsUpToSpeed() { @@ -90,9 +90,9 @@ class ShooterSubsystem : public frc2::SubsystemBase { ctre::phoenix6::controls::CoastOut coastSetter{}; - ctre::phoenix6::sim::TalonFXSimState &topMotorSim = + ctre::phoenix6::sim::TalonFXSimState& topMotorSim = topWheelMotor.GetSimState(); - ctre::phoenix6::sim::TalonFXSimState &bottomMotorSim = + ctre::phoenix6::sim::TalonFXSimState& bottomMotorSim = bottomWheelMotor.GetSimState(); frc::LinearSystem<1, 1, 1> shooterPlant{frc::LinearSystemId::FlywheelSystem( @@ -123,33 +123,33 @@ class ShooterSubsystem : public frc2::SubsystemBase { frc2::sysid::SysIdRoutine topWheelSysIdRoutine{ frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr}, - frc2::sysid::Mechanism{[this](units::volt_t driveVoltage) { - topWheelMotor.SetControl( - topMotorVoltageSetter.WithOutput( - driveVoltage)); - }, - [this](frc::sysid::SysIdRoutineLog *log) { - log->Motor("top-shooter-wheel") - .voltage(topMotorVoltageSig.GetValue()) - .position(currentTopWheelPosition) - .velocity(currentTopWheelVelocity); - }, - this}}; + frc2::sysid::Mechanism{ + [this](units::volt_t driveVoltage) { + topWheelMotor.SetControl( + topMotorVoltageSetter.WithOutput(driveVoltage)); + }, + [this](frc::sysid::SysIdRoutineLog* log) { + log->Motor("top-shooter-wheel") + .voltage(topMotorVoltageSig.GetValue()) + .position(currentTopWheelPosition) + .velocity(currentTopWheelVelocity); + }, + this}}; frc2::sysid::SysIdRoutine bottomWheelSysIdRoutine{ frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr}, - frc2::sysid::Mechanism{[this](units::volt_t driveVoltage) { - bottomWheelMotor.SetControl( - bottomMotorVoltageSetter.WithOutput( - driveVoltage)); - }, - [this](frc::sysid::SysIdRoutineLog *log) { - log->Motor("bottom-shooter-wheel") - .voltage(bottomMotorVoltageSig.GetValue()) - .position(currentBottomWheelPosition) - .velocity(currentBottomWheelVelocity); - }, - this}}; + frc2::sysid::Mechanism{ + [this](units::volt_t driveVoltage) { + bottomWheelMotor.SetControl( + bottomMotorVoltageSetter.WithOutput(driveVoltage)); + }, + [this](frc::sysid::SysIdRoutineLog* log) { + log->Motor("bottom-shooter-wheel") + .voltage(bottomMotorVoltageSig.GetValue()) + .position(currentBottomWheelPosition) + .velocity(currentBottomWheelVelocity); + }, + this}}; std::shared_ptr nt{ nt::NetworkTableInstance::GetDefault().GetTable("Shooter")}; diff --git a/src/main/include/subsystems/SwerveSubsystem.h b/src/main/include/subsystems/SwerveSubsystem.h index 4884c7a..a9432ea 100644 --- a/src/main/include/subsystems/SwerveSubsystem.h +++ b/src/main/include/subsystems/SwerveSubsystem.h @@ -19,7 +19,7 @@ #include "constants/Constants.h" class SwerveSubsystem : public frc2::SubsystemBase { -public: + public: SwerveSubsystem(); /** @@ -33,13 +33,13 @@ class SwerveSubsystem : public frc2::SubsystemBase { frc::ChassisSpeeds GetFieldRelativeSpeed(); frc::ChassisSpeeds GetRobotRelativeSpeed() const; void UpdateSwerveOdom(); - void AddVisionMeasurement(const frc::Pose2d &visionMeasurement, + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp, - const Eigen::Vector3d &stdDevs); - frc2::CommandPtr - FollowChoreoTrajectory(std::function pathName); - frc2::CommandPtr - PointWheelsToAngle(std::function wheelAngle); + const Eigen::Vector3d& stdDevs); + frc2::CommandPtr FollowChoreoTrajectory( + std::function pathName); + frc2::CommandPtr PointWheelsToAngle( + std::function wheelAngle); frc2::CommandPtr XPattern(); frc2::CommandPtr Drive(std::function xVel, std::function yVel, @@ -65,13 +65,13 @@ class SwerveSubsystem : public frc2::SubsystemBase { void CalculateFoundNotePose(std::optional distanceToNote, std::optional angleToNote); frc::Pose2d GetFoundNotePose() const; - frc2::CommandPtr - NoteAssist(std::function xVel, - std::function yVel, - std::function rotOverride, - std::function notePose); + frc2::CommandPtr NoteAssist( + std::function xVel, + std::function yVel, + std::function rotOverride, + std::function notePose); -private: + private: void SetupPathplanner(); void LoadChoreoTrajectories(); frc::Translation2d GetAmpLocation(); @@ -150,7 +150,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { swerveDrive.SetMk4iCharacterizationTorqueSteer( voltsToSend); }, - [this](frc::sysid::SysIdRoutineLog *log) { + [this](frc::sysid::SysIdRoutineLog* log) { swerveDrive.LogMk4iSteerTorque(log); }, this, "swerve-steer-mk4i"}}; @@ -168,7 +168,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { swerveDrive.SetMk4nCharacterizationTorqueSteer( voltsToSend); }, - [this](frc::sysid::SysIdRoutineLog *log) { + [this](frc::sysid::SysIdRoutineLog* log) { swerveDrive.LogMk4nSteerTorque(log); }, this, "swerve-steer-mk4n"}}; @@ -184,7 +184,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { swerveDrive.SetMk4iCharacterizationVoltageSteer( voltsToSend); }, - [this](frc::sysid::SysIdRoutineLog *log) { + [this](frc::sysid::SysIdRoutineLog* log) { swerveDrive.LogMk4iSteerVoltage(log); }, this, "swerve-steer-mk4i"}}; @@ -200,7 +200,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { swerveDrive.SetMk4nCharacterizationVoltageSteer( voltsToSend); }, - [this](frc::sysid::SysIdRoutineLog *log) { + [this](frc::sysid::SysIdRoutineLog* log) { swerveDrive.LogMk4nSteerVoltage(log); }, this, "swerve-steer-mk4n"}}; @@ -218,7 +218,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { swerveDrive.SetCharacterizationTorqueDrive( voltsToSend); }, - [this](frc::sysid::SysIdRoutineLog *log) { + [this](frc::sysid::SysIdRoutineLog* log) { swerveDrive.LogDriveTorque(log); }, this, "swerve-drive"}}; @@ -234,7 +234,7 @@ class SwerveSubsystem : public frc2::SubsystemBase { swerveDrive.SetCharacterizationVoltageDrive( voltsToSend); }, - [this](frc::sysid::SysIdRoutineLog *log) { + [this](frc::sysid::SysIdRoutineLog* log) { swerveDrive.LogDriveVoltage(log); }, this, "swerve-drive"}}; diff --git a/src/test/cpp/main.cpp b/src/test/cpp/main.cpp index d77a559..6db09ca 100644 --- a/src/test/cpp/main.cpp +++ b/src/test/cpp/main.cpp @@ -6,7 +6,7 @@ #include "gtest/gtest.h" -int main(int argc, char **argv) { +int main(int argc, char** argv) { HAL_Initialize(500, 0); ::testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS();