From b9fe126d00ff59bd6733e2fcd2666315b33230bf Mon Sep 17 00:00:00 2001 From: Otto <105565584+GreenTomato5@users.noreply.github.com> Date: Sat, 3 Aug 2024 12:06:42 -0800 Subject: [PATCH] =?UTF-8?q?Prettier=20formatting=20=E2=9C=A8=20=20(#11)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * It might work?... Hopefully? * changes testing branch name in the on thing * Oops I forgot to add that one file * Apply Prettier format * Remove testing thing. 4 spaced tabs wins! * Remove Node Modules * Add node_modules to git ignore * Pulls before other steps * Merge confligs gona make me crash out * Apply Prettier format * Fix step names * Apply Prettier format * Apply Prettier format * Excuse other files --------- Co-authored-by: github-actions Co-authored-by: SSBASLD <77936828+SSBASLD@users.noreply.github.com> --- .github/scripts/constants.py | 2 +- .github/workflows/format.yml | 72 ++ .gitignore | 3 + .prettierrc | 7 + .vscode/settings.json | 5 +- package-lock.json | 146 ++++ package.json | 16 + src/main/java/frc/robot/Constants.java | 478 ++++++------ src/main/java/frc/robot/Main.java | 19 +- src/main/java/frc/robot/Robot.java | 278 +++---- .../java/frc/robot/commands/AutoCommands.java | 69 +- .../java/frc/robot/subsystems/Subsystem.java | 153 ++-- .../frc/robot/subsystems/SubsystemStates.java | 2 +- .../java/frc/robot/subsystems/Trigger.java | 25 +- .../frc/robot/subsystems/ampBar/AmpBar.java | 105 +-- .../frc/robot/subsystems/ampBar/AmpBarIO.java | 52 +- .../robot/subsystems/ampBar/AmpBarIOReal.java | 192 ++--- .../robot/subsystems/ampBar/AmpBarIOSim.java | 195 ++--- .../robot/subsystems/ampBar/AmpBarStates.java | 44 +- .../frc/robot/subsystems/drive/Drive.java | 684 +++++++++--------- .../robot/subsystems/drive/DriveStates.java | 58 +- .../frc/robot/subsystems/drive/GyroIO.java | 19 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 89 +-- .../frc/robot/subsystems/drive/Module.java | 403 ++++++----- .../frc/robot/subsystems/drive/ModuleIO.java | 65 +- .../subsystems/drive/ModuleIOHybrid.java | 358 ++++----- .../robot/subsystems/drive/ModuleIOSim.java | 88 +-- .../subsystems/drive/ModuleIOSparkMax.java | 326 +++++---- .../subsystems/drive/ModuleIOTalonFX.java | 358 ++++----- .../drive/PhoenixOdometryThread.java | 199 ++--- .../drive/SparkMaxOdometryThread.java | 145 ++-- .../frc/robot/subsystems/intake/Intake.java | 103 +-- .../frc/robot/subsystems/intake/IntakeIO.java | 44 +- .../robot/subsystems/intake/IntakeIOSim.java | 182 ++--- .../subsystems/intake/IntakeIOSparkMax.java | 121 ++-- .../robot/subsystems/intake/IntakeStates.java | 70 +- .../frc/robot/subsystems/manager/Manager.java | 267 ++++--- .../subsystems/manager/ManagerStates.java | 85 ++- .../frc/robot/subsystems/shooter/Shooter.java | 103 +-- .../robot/subsystems/shooter/ShooterIO.java | 36 +- .../subsystems/shooter/ShooterIOSim.java | 111 +-- .../subsystems/shooter/ShooterIOTalonFX.java | 110 +-- .../subsystems/shooter/ShooterStates.java | 32 +- src/main/java/frc/robot/util/FFConstants.java | 13 +- .../java/frc/robot/util/LocalADStarAK.java | 270 +++---- .../java/frc/robot/util/NoteSimulator.java | 203 +++--- 46 files changed, 3422 insertions(+), 2983 deletions(-) create mode 100644 .github/workflows/format.yml create mode 100644 .prettierrc create mode 100644 package-lock.json create mode 100644 package.json diff --git a/.github/scripts/constants.py b/.github/scripts/constants.py index 565d0cc..345033b 100644 --- a/.github/scripts/constants.py +++ b/.github/scripts/constants.py @@ -4,7 +4,7 @@ # List of files to excuse (constants and things we didnt make and stuff we wont use) # TODO: Put un-used Module IO abstraction constants in constants (Not super needed but important) -excused_files = ["Constants.java", "BuildConstants.java", "LocalADStarAK.java", "ModuleIOSparkMax.java", "ModuleIOTalonFX.java"] +excused_files = ["Constants.java", "BuildConstants.java", "LocalADStarAK.java", "ModuleIOSparkMax.java", "ModuleIOTalonFX.java", "PhoenixOdometryThread.java", "SparkMaxOdometryThread.java"] # Not really dirs becasue the full ones didnt work excused_dirs = [ diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000..db45065 --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,72 @@ +name: Format With Prettier + +on: + pull_request: + branches: + - main + push: + branches: + - main + +permissions: + contents: write + actions: read + +jobs: + format: + runs-on: ubuntu-latest + + steps: + - name: Checkout Repository + uses: actions/checkout@v3 + + - name: Pull latest changes + run: | + if [ "${GITHUB_EVENT_NAME}" == "pull_request" ]; then + git fetch origin + git checkout "${GITHUB_HEAD_REF}" + git pull origin "${GITHUB_HEAD_REF}" + elif [ "${GITHUB_EVENT_NAME}" == "push" ]; then + git fetch origin + git pull origin "${GITHUB_REF#refs/heads/}" + fi + + - name: Setup Node + uses: actions/setup-node@v3 + with: + node-version: "16" + + - name: Clean Install + run: | + if [ -d "node_modules" ]; then + rm -rf node_modules + fi + npm install + + - name: Apply Prettier Foramtting + run: npx prettier --write "**/*.java" + + - name: Check for Changes + id: git-check + run: | + git diff --exit-code || echo "Changes detected" + continue-on-error: true + + - name: Commit Changes + if: steps.git-check.outcome == 'success' + run: | + git config --global user.name 'github-actions' + git config --global user.email 'github-actions@github.com' + git add . + git commit -m 'Apply Prettier format' || echo "No changes to commit" + if [ "${GITHUB_EVENT_NAME}" == "pull_request" ]; then + git push origin "${GITHUB_HEAD_REF}" + elif [ "${GITHUB_EVENT_NAME}" == "push" ]; then + git push origin "${GITHUB_REF#refs/heads/}" + fi + env: + GITHUB_TOKEN: ${{ secrets.GH_TOKEN }} + + - name: Notify if no Changes + if: steps.git-check.outcome == 'failure' + run: echo "Formatting not Required" diff --git a/.gitignore b/.gitignore index 6122193..7f19e96 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,9 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. +# Node Modules +node_modules/ + settings.json ### C++ ### diff --git a/.prettierrc b/.prettierrc new file mode 100644 index 0000000..319f19d --- /dev/null +++ b/.prettierrc @@ -0,0 +1,7 @@ +{ + "plugins": ["prettier-plugin-java"], + "tabWidth": 4, + "useTabs": true, + "printWidth": 100 + } + \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index b4cac76..b0a1681 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -38,5 +38,8 @@ "[java]": { "editor.defaultFormatter": "redhat.java" }, - "java.debug.settings.onBuildFailureProceed": true + "java.debug.settings.onBuildFailureProceed": true, + "[yaml]": { + "editor.defaultFormatter": "esbenp.prettier-vscode" + } } diff --git a/package-lock.json b/package-lock.json new file mode 100644 index 0000000..dcfd641 --- /dev/null +++ b/package-lock.json @@ -0,0 +1,146 @@ +{ + "name": "2024-rewrite", + "version": "1.0.0", + "lockfileVersion": 3, + "requires": true, + "packages": { + "": { + "name": "2024-rewrite", + "version": "1.0.0", + "license": "ISC", + "devDependencies": { + "prettier": "^3.3.3", + "prettier-plugin-java": "^2.6.4" + } + }, + "node_modules/@chevrotain/cst-dts-gen": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/cst-dts-gen/-/cst-dts-gen-11.0.3.tgz", + "integrity": "sha512-BvIKpRLeS/8UbfxXxgC33xOumsacaeCKAjAeLyOn7Pcp95HiRbrpl14S+9vaZLolnbssPIUuiUd8IvgkRyt6NQ==", + "dev": true, + "dependencies": { + "@chevrotain/gast": "11.0.3", + "@chevrotain/types": "11.0.3", + "lodash-es": "4.17.21" + } + }, + "node_modules/@chevrotain/gast": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/gast/-/gast-11.0.3.tgz", + "integrity": "sha512-+qNfcoNk70PyS/uxmj3li5NiECO+2YKZZQMbmjTqRI3Qchu8Hig/Q9vgkHpI3alNjr7M+a2St5pw5w5F6NL5/Q==", + "dev": true, + "dependencies": { + "@chevrotain/types": "11.0.3", + "lodash-es": "4.17.21" + } + }, + "node_modules/@chevrotain/regexp-to-ast": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/regexp-to-ast/-/regexp-to-ast-11.0.3.tgz", + "integrity": "sha512-1fMHaBZxLFvWI067AVbGJav1eRY7N8DDvYCTwGBiE/ytKBgP8azTdgyrKyWZ9Mfh09eHWb5PgTSO8wi7U824RA==", + "dev": true + }, + "node_modules/@chevrotain/types": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/types/-/types-11.0.3.tgz", + "integrity": "sha512-gsiM3G8b58kZC2HaWR50gu6Y1440cHiJ+i3JUvcp/35JchYejb2+5MVeJK0iKThYpAa/P2PYFV4hoi44HD+aHQ==", + "dev": true + }, + "node_modules/@chevrotain/utils": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/utils/-/utils-11.0.3.tgz", + "integrity": "sha512-YslZMgtJUyuMbZ+aKvfF3x1f5liK4mWNxghFRv7jqRR9C3R3fAOGTTKvxXDa2Y1s9zSbcpuO0cAxDYsc9SrXoQ==", + "dev": true + }, + "node_modules/chevrotain": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/chevrotain/-/chevrotain-11.0.3.tgz", + "integrity": "sha512-ci2iJH6LeIkvP9eJW6gpueU8cnZhv85ELY8w8WiFtNjMHA5ad6pQLaJo9mEly/9qUyCpvqX8/POVUTf18/HFdw==", + "dev": true, + "dependencies": { + "@chevrotain/cst-dts-gen": "11.0.3", + "@chevrotain/gast": "11.0.3", + "@chevrotain/regexp-to-ast": "11.0.3", + "@chevrotain/types": "11.0.3", + "@chevrotain/utils": "11.0.3", + "lodash-es": "4.17.21" + } + }, + "node_modules/chevrotain-allstar": { + "version": "0.3.1", + "resolved": "https://registry.npmjs.org/chevrotain-allstar/-/chevrotain-allstar-0.3.1.tgz", + "integrity": "sha512-b7g+y9A0v4mxCW1qUhf3BSVPg+/NvGErk/dOkrDaHA0nQIQGAtrOjlX//9OQtRlSCy+x9rfB5N8yC71lH1nvMw==", + "dev": true, + "dependencies": { + "lodash-es": "^4.17.21" + }, + "peerDependencies": { + "chevrotain": "^11.0.0" + } + }, + "node_modules/java-parser": { + "version": "2.3.2", + "resolved": "https://registry.npmjs.org/java-parser/-/java-parser-2.3.2.tgz", + "integrity": "sha512-/O42UbEHy3VVJw8W0ruHkQjW75oWvQx4QisoUDRIGir6q3/IZ4JslDMPMYEqp7LU56PYJkH5uXdQiBaCXt/Opw==", + "dev": true, + "dependencies": { + "chevrotain": "11.0.3", + "chevrotain-allstar": "0.3.1", + "lodash": "4.17.21" + } + }, + "node_modules/lodash": { + "version": "4.17.21", + "resolved": "https://registry.npmjs.org/lodash/-/lodash-4.17.21.tgz", + "integrity": "sha512-v2kDEe57lecTulaDIuNTPy3Ry4gLGJ6Z1O3vE1krgXZNrsQ+LFTGHVxVjcXPs17LhbZVGedAJv8XZ1tvj5FvSg==", + "dev": true + }, + "node_modules/lodash-es": { + "version": "4.17.21", + "resolved": "https://registry.npmjs.org/lodash-es/-/lodash-es-4.17.21.tgz", + "integrity": "sha512-mKnC+QJ9pWVzv+C4/U3rRsHapFfHvQFoFB92e52xeyGMcX6/OlIl78je1u8vePzYZSkkogMPJ2yjxxsb89cxyw==", + "dev": true + }, + "node_modules/prettier": { + "version": "3.3.3", + "resolved": "https://registry.npmjs.org/prettier/-/prettier-3.3.3.tgz", + "integrity": "sha512-i2tDNA0O5IrMO757lfrdQZCc2jPNDVntV0m/+4whiDfWaTKfMNgR7Qz0NAeGz/nRqF4m5/6CLzbP4/liHt12Ew==", + "dev": true, + "bin": { + "prettier": "bin/prettier.cjs" + }, + "engines": { + "node": ">=14" + }, + "funding": { + "url": "https://github.com/prettier/prettier?sponsor=1" + } + }, + "node_modules/prettier-plugin-java": { + "version": "2.6.4", + "resolved": "https://registry.npmjs.org/prettier-plugin-java/-/prettier-plugin-java-2.6.4.tgz", + "integrity": "sha512-57iGIFM4xSCqzHc4G6RLeC0DJk+i6Vd1JDj5xcIe7GsWZjRSl8WWkpL0f4BB0gZ+jDZ8R1uJaxtnMgnRtzjLDQ==", + "dev": true, + "dependencies": { + "java-parser": "2.3.2", + "lodash": "4.17.21", + "prettier": "3.2.5" + } + }, + "node_modules/prettier-plugin-java/node_modules/prettier": { + "version": "3.2.5", + "resolved": "https://registry.npmjs.org/prettier/-/prettier-3.2.5.tgz", + "integrity": "sha512-3/GWa9aOC0YeD7LUfvOG2NiDyhOWRvt1k+rcKhOuYnMY24iiCphgneUfJDyFXd6rZCAnuLBv6UeAULtrhT/F4A==", + "dev": true, + "bin": { + "prettier": "bin/prettier.cjs" + }, + "engines": { + "node": ">=14" + }, + "funding": { + "url": "https://github.com/prettier/prettier?sponsor=1" + } + } + } +} diff --git a/package.json b/package.json new file mode 100644 index 0000000..a051808 --- /dev/null +++ b/package.json @@ -0,0 +1,16 @@ +{ + "name": "2024-rewrite", + "version": "1.0.0", + "description": "", + "main": "index.js", + "scripts": { + "test": "echo \"Error: no test specified\" && exit 1" + }, + "keywords": [], + "author": "", + "license": "ISC", + "devDependencies": { + "prettier": "^3.3.3", + "prettier-plugin-java": "^2.6.4" + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 093662f..4a130bc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,275 +31,293 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final double SIM_UPDATE_TIME = 0.05; - // Conversion Factors - public static final double RADIAN_CF = (Math.PI * 2); - public static final double RPM_TO_RPS_CF = 60; - public static final double DIAM_TO_RADIUS_CF = 2.0; - public static final double AVG_TWO_ITEM_F = 2.0; + public static final double SIM_UPDATE_TIME = 0.05; - public static final Mode currentMode = Mode.SIM; + // Conversion Factors + public static final double RADIAN_CF = (Math.PI * 2); + public static final double RPM_TO_RPS_CF = 60; + public static final double DIAM_TO_RADIUS_CF = 2.0; + public static final double AVG_TWO_ITEM_F = 2.0; - public static final double MAX_VOLTS = 12.0; - public static final double MIN_VOLTS = -12.0; + public static final Mode currentMode = Mode.SIM; - public static final XboxController controller = new XboxController(0); - public static final XboxController operatorController = new XboxController(1); + public static final double MAX_VOLTS = 12.0; + public static final double MIN_VOLTS = -12.0; - public static enum Mode { - /** Running on a real robot. */ - REAL, + public static final XboxController controller = new XboxController(0); + public static final XboxController operatorController = new XboxController(1); - /** Running a physics simulator. */ - SIM, + public static enum Mode { + /** Running on a real robot. */ + REAL, - /** Replaying from a log file. */ - REPLAY - } + /** Running a physics simulator. */ + SIM, - public static final class Intake { + /** Replaying from a log file. */ + REPLAY, + } - public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d(0.31, 0, 0.24); + public static final class Intake { - // CAN IDs - public static final int PIVOT_ID = 32; - public static final int SPINNER_ID = 20; + public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d( + 0.31, + 0, + 0.24 + ); - // PID - public static final PIDConstants SIM_IN_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants SIM_OUT_PID = new PIDConstants(1, 0, 0); + // CAN IDs + public static final int PIVOT_ID = 32; + public static final int SPINNER_ID = 20; - public static final PIDConstants REAL_IN_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_OUT_PID = new PIDConstants(1, 0, 0); + // PID + public static final PIDConstants SIM_IN_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants SIM_OUT_PID = new PIDConstants(1, 0, 0); - // Sim Configs + public static final PIDConstants REAL_IN_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_OUT_PID = new PIDConstants(1, 0, 0); - // Pivot - public static final int NUM_PIVOT_MOTORS = 1; - public static final double PIVOT_GEARING = 67.5; - public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(180.0); - public static final double PIVOT_MOI = 0.192383865; - public static final double PIVOT_LENGTH_METERS = 0.3; + // Sim Configs - // Spinner - public static final int NUM_SPINNER_MOTORS = 1; - public static final double SPINNER_MOI = 0.01; - public static final double SPINNER_GEARING = 1.0; - public static final double OFF = 0.0; + // Pivot + public static final int NUM_PIVOT_MOTORS = 1; + public static final double PIVOT_GEARING = 67.5; + public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(180.0); + public static final double PIVOT_MOI = 0.192383865; + public static final double PIVOT_LENGTH_METERS = 0.3; - // In Rads (Pivot setpoints) - public static final double DOWN = Units.degreesToRadians(180.0); - public static final double IN = Units.degreesToRadians(0.0); + // Spinner + public static final int NUM_SPINNER_MOTORS = 1; + public static final double SPINNER_MOI = 0.01; + public static final double SPINNER_GEARING = 1.0; + public static final double OFF = 0.0; - // In RPS (Spinner Setpoints) - public static final double REVERSE = -10.0; - public static final double ON = 10.0; - } + // In Rads (Pivot setpoints) + public static final double DOWN = Units.degreesToRadians(180.0); + public static final double IN = Units.degreesToRadians(0.0); - public static final class Shooter { - public static final double ERROR_OF_MARGIN = 2.0; + // In RPS (Spinner Setpoints) + public static final double REVERSE = -10.0; + public static final double ON = 10.0; + } - // PID - public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); - public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); + public static final class Shooter { - // CAN IDs - public static final int LEFT_SHOOTER_ID = 15; - public static final int RIGHT_SHOOTER_ID = 14; + public static final double ERROR_OF_MARGIN = 2.0; - // Shooter Setpoints (RPS) - public static final double OFF = 0.0; - public static final double FEEDING_AMP = 25.0; - public static final double SHOOTING = 50.0; + // PID + public static final PIDConstants SIM_PID = new PIDConstants(1, 0, 0); + public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); - // Sim Configs - public static final int NUM_MOTORS = 2; - public static final double SHOOTER_GEARING = 1.5; - public static final double SHOOTER_MOI = 0.004; + // CAN IDs + public static final int LEFT_SHOOTER_ID = 15; + public static final int RIGHT_SHOOTER_ID = 14; - // spinner circumfrence need to check with mech - public static final double CIRCUMFRENCE_OF_SHOOTER_SPINNER = 4; - } + // Shooter Setpoints (RPS) + public static final double OFF = 0.0; + public static final double FEEDING_AMP = 25.0; + public static final double SHOOTING = 50.0; - public static final class AmpBar { + // Sim Configs + public static final int NUM_MOTORS = 2; + public static final double SHOOTER_GEARING = 1.5; + public static final double SHOOTER_MOI = 0.004; - public static final Translation3d ZEROED_PIVOT_TRANSLATION = - new Translation3d(-0.317, 0, 0.525); + // spinner circumfrence need to check with mech + public static final double CIRCUMFRENCE_OF_SHOOTER_SPINNER = 4; + } - public static final double ERROR_OF_MARGIN = 0.1; + public static final class AmpBar { - // PID - public static final PIDConstants SIM_PID = new PIDConstants(3, 0, 1.5); - public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); + public static final Translation3d ZEROED_PIVOT_TRANSLATION = new Translation3d( + -0.317, + 0, + 0.525 + ); - // Motor CAN IDs - public static final int LEFT_PIVOT_ID = 31; - public static final int RIGHT_PIVOT_ID = 30; - public static final int SPINNER_ID = 38; + public static final double ERROR_OF_MARGIN = 0.1; - // Sim Configs + // PID + public static final PIDConstants SIM_PID = new PIDConstants(3, 0, 1.5); + public static final PIDConstants REAL_PID = new PIDConstants(1, 0, 0); - // Spinner - public static final double SPINNER_GEARING = 0.5; - public static final double SPINNER_MOI = 0.5; - public static final int NUM_SPINNER_MOTORS = 1; + // Motor CAN IDs + public static final int LEFT_PIVOT_ID = 31; + public static final int RIGHT_PIVOT_ID = 30; + public static final int SPINNER_ID = 38; - // Pivot - public static final int NUM_PIVOT_MOTORS = 2; - public static final double PIVOT_GEARING = 0.05; - public static final double PIVOT_MOI = 0.05; - public static final double PIVOT_LENGTH_METERS = 0.378; - public static final double MIN_PIVOT_POSITION = -Units.degreesToRadians(114.163329); - public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(0); + // Sim Configs - // Pivot and Spinner Setpoints + // Spinner + public static final double SPINNER_GEARING = 0.5; + public static final double SPINNER_MOI = 0.5; + public static final int NUM_SPINNER_MOTORS = 1; - // In RPS (Spinner Setpoints) - public static final double SHOOTING = -0.5; - public static final double FEEDING = -0.1; - public static final double OFF = 0.0; + // Pivot + public static final int NUM_PIVOT_MOTORS = 2; + public static final double PIVOT_GEARING = 0.05; + public static final double PIVOT_MOI = 0.05; + public static final double PIVOT_LENGTH_METERS = 0.378; + public static final double MIN_PIVOT_POSITION = -Units.degreesToRadians(114.163329); + public static final double MAX_PIVOT_POSITION = Units.degreesToRadians(0); + + // Pivot and Spinner Setpoints - // IN Rads (Pivot Setpoints) - public static final double OUT = -Units.degreesToRadians(100.0); - public static final double FEEDING_POSITION = -Units.degreesToRadians(93.0); - public static final double IN = Units.degreesToRadians(0.0); - } + // In RPS (Spinner Setpoints) + public static final double SHOOTING = -0.5; + public static final double FEEDING = -0.1; + public static final double OFF = 0.0; - public static final class Drive { + // IN Rads (Pivot Setpoints) + public static final double OUT = -Units.degreesToRadians(100.0); + public static final double FEEDING_POSITION = -Units.degreesToRadians(93.0); + public static final double IN = Units.degreesToRadians(0.0); + } - public static final double DISCRETIZE_TIME_SECONDS = 0.02; - public static final double CONTROLLER_DEADBAND = 0.1; - public static final int NUM_MODULES = 4; + public static final class Drive { - /* Rotation and Translation Modifers + public static final double DISCRETIZE_TIME_SECONDS = 0.02; + public static final double CONTROLLER_DEADBAND = 0.1; + public static final int NUM_MODULES = 4; + + /* Rotation and Translation Modifers rm = rotation multiplier tm = translation multipliers aa = auto align */ - public static final double REGULAR_RM = 1.0; - public static final double REGULAR_TM = 1.0; - public static final double SLOW_RM = 0.5; - public static final double SLOW_TM = 0.2; - public static final double AA_RM = 0.8; - public static final double AA_TM = 0.8; - public static final double FAST_RM = 1.5; - public static final double FAST_TM = 2.0; - - // Auto Config - public static final PIDConstants TRANSLATION_PID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants ROTATION_PID = new PIDConstants(5.0, 0.0, 0.0); - public static final double MAX_MODULE_SPEED = 6.0; - - // Configs - public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); - public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); - public static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - - public static final class Pidgeon2 { - public static final int DEVICE_ID = 20; - public static final double UPDATE_FREQUENCY = 100.0; - } - - public static final class Module { - public static final double ODOMETRY_FREQUENCY = 250.0; - - // There isnt one for real its just all 0 so idk whats good with that - public static final FFConstants REPLAY_FF = new FFConstants(0.1, 0.13); - public static final PIDConstants REPLAY_DRIVE_PID = new PIDConstants(0.05, 0.0, 0.0); - public static final PIDConstants REPLAY_TURN_PID = new PIDConstants(7.0, 0.0, 0.0); - - public static final FFConstants SIM_FF = new FFConstants(0.0, 0.13); - public static final PIDConstants SIM_DRIVE_PID = new PIDConstants(0.1, 0.0, 0.0); - public static final PIDConstants SIM_TURN_PID = new PIDConstants(10.0, 0.0, 0.0); - - public static final int NUM_TURN_MOTORS = 1; - public static final int NUM_DRIVE_MOTORS = 1; - - public static final class Sim { - public static final double LOOP_PERIOD_SECS = 0.02; - - // Configs - public static final double DRIVE_GEARING = 6.75; - public static final double DRIVE_MOI = 0.025; - - public static final double TURN_GEARING = 150.0 / 7.0; - public static final double TURN_MOI = 0.004; - } - - // TODO: Put constants from those abstractions in here - public static final class SparkMax {} - - public static final class TalonFX {} - - public static final class Hybrid { - // These are for l2 Mk4i mods, should be L3 plus - public static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - public static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - public static final double DRIVE_CURRENT_LIMIT = 40.0; - public static final int TURN_CURRENT_LIMIT = 30; - // Stuff - public static final double TALON_UPDATE_FREQUENCY_HZ = 50.0; - - public static final int SPARK_TIMEOUT_MS = 250; - public static final int SPARK_MEASURMENT_PERIOD_MS = 10; - public static final int SPARK_AVG_DEPTH = 2; - public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; - - // CAN/Device IDS and offsets - public static final int DRIVE0_ID = 0; - public static final int TURN0_ID = 1; - public static final int CANCODER0_ID = 2; - public static final double OFFSET0 = 0.0; - - public static final int DRIVE1_ID = 0; - public static final int TURN1_ID = 1; - public static final int CANCODER1_ID = 2; - public static final double OFFSET1 = 0.0; - - public static final int DRIVE2_ID = 0; - public static final int TURN2_ID = 1; - public static final int CANCODER2_ID = 2; - public static final double OFFSET2 = 0.0; - - public static final int DRIVE3_ID = 0; - public static final int TURN3_ID = 1; - public static final int CANCODER3_ID = 2; - public static final double OFFSET3 = 0.0; - } - } - - public static final class OdoThread { - public static final class Phoenix { - - public static final int QUE_CAPACITY = 20; - public static final double SLEEP_TIME = 1000.0; - } - - public static final class SparkMax { - public static final int QUE_CAPACITY = 20; - public static final double PERIOD = 1.0; - } - } - } - - public static final class NoteSim { - public static final double AIR_DENSITY = 1.225; - public static final double DRAG_COEFFICIENT = 0.45; - public static final double CROSSECTION_AREA = 0.11; - public static final double MASS = 0.235; - - public static final Pose3d SHOOTER_POSE3D = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final Translation2d FIELD_SIZE = - new Translation2d(16.54, 8.21); // stolen from 3015 constants - - public static final double dt = 0.2; // change in time for note sim - public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); - public static final double OUT_OF_FIELD_MARGIN = .025; - } + public static final double REGULAR_RM = 1.0; + public static final double REGULAR_TM = 1.0; + public static final double SLOW_RM = 0.5; + public static final double SLOW_TM = 0.2; + public static final double AA_RM = 0.8; + public static final double AA_TM = 0.8; + public static final double FAST_RM = 1.5; + public static final double FAST_TM = 2.0; + + // Auto Config + public static final PIDConstants TRANSLATION_PID = new PIDConstants(5.0, 0.0, 0.0); + public static final PIDConstants ROTATION_PID = new PIDConstants(5.0, 0.0, 0.0); + public static final double MAX_MODULE_SPEED = 6.0; + + // Configs + public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); + public static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5); + public static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(25.0); + public static final double DRIVE_BASE_RADIUS = Math.hypot( + TRACK_WIDTH_X / 2.0, + TRACK_WIDTH_Y / 2.0 + ); + public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; + + public static final class Pidgeon2 { + + public static final int DEVICE_ID = 20; + public static final double UPDATE_FREQUENCY = 100.0; + } + + public static final class Module { + + public static final double ODOMETRY_FREQUENCY = 250.0; + + // There isnt one for real its just all 0 so idk whats good with that + public static final FFConstants REPLAY_FF = new FFConstants(0.1, 0.13); + public static final PIDConstants REPLAY_DRIVE_PID = new PIDConstants(0.05, 0.0, 0.0); + public static final PIDConstants REPLAY_TURN_PID = new PIDConstants(7.0, 0.0, 0.0); + + public static final FFConstants SIM_FF = new FFConstants(0.0, 0.13); + public static final PIDConstants SIM_DRIVE_PID = new PIDConstants(0.1, 0.0, 0.0); + public static final PIDConstants SIM_TURN_PID = new PIDConstants(10.0, 0.0, 0.0); + + public static final int NUM_TURN_MOTORS = 1; + public static final int NUM_DRIVE_MOTORS = 1; + + public static final class Sim { + + public static final double LOOP_PERIOD_SECS = 0.02; + + // Configs + public static final double DRIVE_GEARING = 6.75; + public static final double DRIVE_MOI = 0.025; + + public static final double TURN_GEARING = 150.0 / 7.0; + public static final double TURN_MOI = 0.004; + } + + // TODO: Put constants from those abstractions in here + public static final class SparkMax {} + + public static final class TalonFX {} + + public static final class Hybrid { + + // These are for l2 Mk4i mods, should be L3 plus + public static final double DRIVE_GEAR_RATIO = + (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + public static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + public static final double DRIVE_CURRENT_LIMIT = 40.0; + public static final int TURN_CURRENT_LIMIT = 30; + // Stuff + public static final double TALON_UPDATE_FREQUENCY_HZ = 50.0; + + public static final int SPARK_TIMEOUT_MS = 250; + public static final int SPARK_MEASURMENT_PERIOD_MS = 10; + public static final int SPARK_AVG_DEPTH = 2; + public static final double SPARK_FRAME_PERIOD = 1000.0 / ODOMETRY_FREQUENCY; + + // CAN/Device IDS and offsets + public static final int DRIVE0_ID = 0; + public static final int TURN0_ID = 1; + public static final int CANCODER0_ID = 2; + public static final double OFFSET0 = 0.0; + + public static final int DRIVE1_ID = 0; + public static final int TURN1_ID = 1; + public static final int CANCODER1_ID = 2; + public static final double OFFSET1 = 0.0; + + public static final int DRIVE2_ID = 0; + public static final int TURN2_ID = 1; + public static final int CANCODER2_ID = 2; + public static final double OFFSET2 = 0.0; + + public static final int DRIVE3_ID = 0; + public static final int TURN3_ID = 1; + public static final int CANCODER3_ID = 2; + public static final double OFFSET3 = 0.0; + } + } + + public static final class OdoThread { + + public static final class Phoenix { + + public static final int QUE_CAPACITY = 20; + public static final double SLEEP_TIME = 1000.0; + } + + public static final class SparkMax { + + public static final int QUE_CAPACITY = 20; + public static final double PERIOD = 1.0; + } + } + } + + public static final class NoteSim { + + public static final double AIR_DENSITY = 1.225; + public static final double DRAG_COEFFICIENT = 0.45; + public static final double CROSSECTION_AREA = 0.11; + public static final double MASS = 0.235; + + public static final Pose3d SHOOTER_POSE3D = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)); + public static final Translation2d FIELD_SIZE = new Translation2d(16.54, 8.21); // stolen from 3015 constants + + public static final double dt = 0.2; // change in time for note sim + public static final Translation3d GRAVITY_TRANSLATION3D = new Translation3d(0, 0, 9.8); + public static final double OUT_OF_FIELD_MARGIN = .025; + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index e8af1c1..40d8330 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -21,14 +21,15 @@ * call. */ public final class Main { - private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 667119e..34958f8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,143 +37,143 @@ */ public class Robot extends LoggedRobot { - public Manager managerSubsystem; - private SendableChooser autoChooser; - - private AutoCommands autoCommands = new AutoCommands(this); - - private Command autonomousCommand; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - managerSubsystem = new Manager(); - - NamedCommands.registerCommand("Intaking", autoCommands.intaking()); - NamedCommands.registerCommand("Shooting", autoCommands.shooting()); - NamedCommands.registerCommand("Return To Idle", autoCommands.returnToIdle()); - NamedCommands.registerCommand("Speeding Up", autoCommands.startSpinningUp()); - NamedCommands.registerCommand("Spin and Intake", autoCommands.spinAndIntake()); - - // Record metadata - Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); - Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncomitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } - - // Set up data receivers & replay source - switch (Constants.currentMode) { - case REAL: - // Running on a real robot, log to a USB stick ("/U/logs") - Logger.addDataReceiver(new WPILOGWriter()); - Logger.addDataReceiver(new NT4Publisher()); - break; - - case SIM: - // Running a physics simulator, log to NT - Logger.addDataReceiver(new NT4Publisher()); - break; - - case REPLAY: - // Replaying a log, set up replay source - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); - break; - } - - // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. - // Logger.disableDeterministicTimestamps() - - // Start AdvantageKit logger - Logger.start(); - - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - } - - /** This function is called periodically during all modes. */ - @Override - public void robotPeriodic() { - managerSubsystem.periodic(); - - // Logs note sim logging and updating sims - NoteSimulator.update(); - NoteSimulator.logNoteInfo(); - - CommandScheduler.getInstance().run(); - } - - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() { - managerSubsystem.stop(); - } - - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() {} - - /** This function is called once the robot enters Auto. */ - @Override - public void autonomousInit() { - autonomousCommand = getAutonomousCommand(); - - // schedule the autonomous command (example) - if (autonomousCommand != null) { - autonomousCommand.schedule(); - } - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - if (autonomousCommand != null) { - autonomousCommand.cancel(); - } - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() {} - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} - - public Command getAutonomousCommand() { - return autoChooser.getSelected(); - } + public Manager managerSubsystem; + private SendableChooser autoChooser; + + private AutoCommands autoCommands = new AutoCommands(this); + + private Command autonomousCommand; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + managerSubsystem = new Manager(); + + NamedCommands.registerCommand("Intaking", autoCommands.intaking()); + NamedCommands.registerCommand("Shooting", autoCommands.shooting()); + NamedCommands.registerCommand("Return To Idle", autoCommands.returnToIdle()); + NamedCommands.registerCommand("Speeding Up", autoCommands.startSpinningUp()); + NamedCommands.registerCommand("Spin and Intake", autoCommands.spinAndIntake()); + + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (Constants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver( + new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")) + ); + break; + } + + // See http://bit.ly/3YIzFZ6 for more information on timestamps in AdvantageKit. + // Logger.disableDeterministicTimestamps() + + // Start AdvantageKit logger + Logger.start(); + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { + managerSubsystem.periodic(); + + // Logs note sim logging and updating sims + NoteSimulator.update(); + NoteSimulator.logNoteInfo(); + + CommandScheduler.getInstance().run(); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() { + managerSubsystem.stop(); + } + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() {} + + /** This function is called once the robot enters Auto. */ + @Override + public void autonomousInit() { + autonomousCommand = getAutonomousCommand(); + + // schedule the autonomous command (example) + if (autonomousCommand != null) { + autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() {} + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } } diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 6bd4150..a21dc3b 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -7,35 +7,42 @@ import frc.robot.subsystems.manager.ManagerStates; public class AutoCommands { - Robot robot = null; - - public AutoCommands(Robot robot) { - this.robot = robot; - } - - public Command intaking() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING))); - } - - public Command shooting() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING))); - } - - public Command returnToIdle() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE))); - } - - public Command startSpinningUp() { - return Commands.sequence( - new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SPINNING_UP))); - } - - public Command spinAndIntake() { - return Commands.sequence( - new InstantCommand( - () -> robot.managerSubsystem.setState(ManagerStates.SPINNING_AND_INTAKING))); - } + + Robot robot = null; + + public AutoCommands(Robot robot) { + this.robot = robot; + } + + public Command intaking() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.INTAKING)) + ); + } + + public Command shooting() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SHOOTING)) + ); + } + + public Command returnToIdle() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.IDLE)) + ); + } + + public Command startSpinningUp() { + return Commands.sequence( + new InstantCommand(() -> robot.managerSubsystem.setState(ManagerStates.SPINNING_UP)) + ); + } + + public Command spinAndIntake() { + return Commands.sequence( + new InstantCommand(() -> + robot.managerSubsystem.setState(ManagerStates.SPINNING_AND_INTAKING) + ) + ); + } } diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java index 9ab6b5c..3b2cb1d 100644 --- a/src/main/java/frc/robot/subsystems/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -11,79 +11,82 @@ import org.littletonrobotics.junction.Logger; public abstract class Subsystem { - private Map>> triggerMap = - new HashMap>>(); - - private StateType state = null; - private Timer stateTimer = new Timer(); - private String subsystemName; - - public Subsystem(String subsystemName, StateType defaultState) { - if (defaultState == null) { - throw new RuntimeException("Default state cannot be null!"); - } - this.subsystemName = subsystemName; - this.state = defaultState; - - stateTimer.start(); - } - - // State operation - public void periodic() { - Logger.recordOutput(subsystemName + "/state", state.getStateString()); - if (!DriverStation.isEnabled()) return; - - runState(); - - checkTriggers(); - } - - protected abstract void runState(); - - // SmartDashboard utils - protected void putSmartDashboard(String key, String value) { - SmartDashboard.putString("[" + subsystemName + "] " + key, value); - } - - protected void putSmartDashboard(String key, double value) { - SmartDashboard.putNumber("[" + subsystemName + "] " + key, value); - } - - // Triggers for state transitions - protected void addTrigger(StateType startType, StateType endType, BooleanSupplier check) { - if (triggerMap.get(startType) == null) { - triggerMap.put(startType, new ArrayList>()); - } - triggerMap.get(startType).add(new Trigger(check, endType)); - } - - private void checkTriggers() { - List> triggers = triggerMap.get(state); - if (triggers == null) return; - for (var trigger : triggers) { - if (trigger.isTriggered()) { - setState(trigger.getResultState()); - return; - } - } - } - - // Other utilities - public StateType getState() { - return state; - } - - public void setState(StateType state) { - if (this.state != state) stateTimer.reset(); - this.state = state; - } - - /** - * Gets amount of time the state machine has been in the current state. - * - * @return time in seconds. - */ - protected double getStateTime() { - return stateTimer.get(); - } + + private Map>> triggerMap = new HashMap< + StateType, + ArrayList> + >(); + + private StateType state = null; + private Timer stateTimer = new Timer(); + private String subsystemName; + + public Subsystem(String subsystemName, StateType defaultState) { + if (defaultState == null) { + throw new RuntimeException("Default state cannot be null!"); + } + this.subsystemName = subsystemName; + this.state = defaultState; + + stateTimer.start(); + } + + // State operation + public void periodic() { + Logger.recordOutput(subsystemName + "/state", state.getStateString()); + if (!DriverStation.isEnabled()) return; + + runState(); + + checkTriggers(); + } + + protected abstract void runState(); + + // SmartDashboard utils + protected void putSmartDashboard(String key, String value) { + SmartDashboard.putString("[" + subsystemName + "] " + key, value); + } + + protected void putSmartDashboard(String key, double value) { + SmartDashboard.putNumber("[" + subsystemName + "] " + key, value); + } + + // Triggers for state transitions + protected void addTrigger(StateType startType, StateType endType, BooleanSupplier check) { + if (triggerMap.get(startType) == null) { + triggerMap.put(startType, new ArrayList>()); + } + triggerMap.get(startType).add(new Trigger(check, endType)); + } + + private void checkTriggers() { + List> triggers = triggerMap.get(state); + if (triggers == null) return; + for (var trigger : triggers) { + if (trigger.isTriggered()) { + setState(trigger.getResultState()); + return; + } + } + } + + // Other utilities + public StateType getState() { + return state; + } + + public void setState(StateType state) { + if (this.state != state) stateTimer.reset(); + this.state = state; + } + + /** + * Gets amount of time the state machine has been in the current state. + * + * @return time in seconds. + */ + protected double getStateTime() { + return stateTimer.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/SubsystemStates.java b/src/main/java/frc/robot/subsystems/SubsystemStates.java index d2ffcaf..7664a70 100644 --- a/src/main/java/frc/robot/subsystems/SubsystemStates.java +++ b/src/main/java/frc/robot/subsystems/SubsystemStates.java @@ -1,5 +1,5 @@ package frc.robot.subsystems; public interface SubsystemStates { - String getStateString(); + String getStateString(); } diff --git a/src/main/java/frc/robot/subsystems/Trigger.java b/src/main/java/frc/robot/subsystems/Trigger.java index a4751ca..cd358ff 100644 --- a/src/main/java/frc/robot/subsystems/Trigger.java +++ b/src/main/java/frc/robot/subsystems/Trigger.java @@ -3,19 +3,20 @@ import java.util.function.BooleanSupplier; public class Trigger { - BooleanSupplier supplier; - StateType state; - public Trigger(BooleanSupplier supplier, StateType state) { - this.supplier = supplier; - this.state = state; - } + BooleanSupplier supplier; + StateType state; - public boolean isTriggered() { - return supplier.getAsBoolean(); - } + public Trigger(BooleanSupplier supplier, StateType state) { + this.supplier = supplier; + this.state = state; + } - public StateType getResultState() { - return state; - } + public boolean isTriggered() { + return supplier.getAsBoolean(); + } + + public StateType getResultState() { + return state; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java index 8d06179..ba55f54 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBar.java @@ -7,53 +7,60 @@ import org.littletonrobotics.junction.Logger; public class AmpBar extends Subsystem { - AmpBarIO io; - AmpBarIOInputsAutoLogged inputs; - - public AmpBar(AmpBarIO io) { - super("Amp Bar", AmpBarStates.OFF); - this.io = io; - inputs = new AmpBarIOInputsAutoLogged(); - - switch (Constants.currentMode) { - case REAL: - io.configurePID( - Constants.AmpBar.REAL_PID.kP, - Constants.AmpBar.REAL_PID.kI, - Constants.AmpBar.REAL_PID.kD); - break; - case SIM: - io.configurePID( - Constants.AmpBar.SIM_PID.kP, Constants.AmpBar.SIM_PID.kP, Constants.AmpBar.SIM_PID.kP); - break; - case REPLAY: - io.configurePID(0, 0, 0); - break; - default: - break; - } - } - - @Override - protected void runState() { - io.setSpinnerSpeedpoint(getState().getMotorSpeedpoint()); - io.setPivotPosition(getState().getPivotPositionSetpoint()); - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - super.periodic(); - - Logger.recordOutput( - "Amp Bar/Amp Bar Pose3d", - new Pose3d( - Constants.AmpBar.ZEROED_PIVOT_TRANSLATION, - new Rotation3d(0, io.getPivotPosition(), 0))); - Logger.processInputs("Amp Bar", inputs); - io.updateInput(inputs); - } + + AmpBarIO io; + AmpBarIOInputsAutoLogged inputs; + + public AmpBar(AmpBarIO io) { + super("Amp Bar", AmpBarStates.OFF); + this.io = io; + inputs = new AmpBarIOInputsAutoLogged(); + + switch (Constants.currentMode) { + case REAL: + io.configurePID( + Constants.AmpBar.REAL_PID.kP, + Constants.AmpBar.REAL_PID.kI, + Constants.AmpBar.REAL_PID.kD + ); + break; + case SIM: + io.configurePID( + Constants.AmpBar.SIM_PID.kP, + Constants.AmpBar.SIM_PID.kP, + Constants.AmpBar.SIM_PID.kP + ); + break; + case REPLAY: + io.configurePID(0, 0, 0); + break; + default: + break; + } + } + + @Override + protected void runState() { + io.setSpinnerSpeedpoint(getState().getMotorSpeedpoint()); + io.setPivotPosition(getState().getPivotPositionSetpoint()); + } + + public void stop() { + io.stop(); + } + + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput( + "Amp Bar/Amp Bar Pose3d", + new Pose3d( + Constants.AmpBar.ZEROED_PIVOT_TRANSLATION, + new Rotation3d(0, io.getPivotPosition(), 0) + ) + ); + Logger.processInputs("Amp Bar", inputs); + io.updateInput(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java index 4ad59db..5560918 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIO.java @@ -3,40 +3,40 @@ import org.littletonrobotics.junction.AutoLog; public interface AmpBarIO { + @AutoLog + public static class AmpBarIOInputs { - @AutoLog - public static class AmpBarIOInputs { - public String ampBarState; + public String ampBarState; - // Pivot - public double pivotPosition; - public double pivotSetpoint; - public double pivotAppliedVoltage; - // Spinners - public double spinnerSpeed; - public double spinnerSetpoint; - public double spinnerAppliedVoltage; - } + // Pivot + public double pivotPosition; + public double pivotSetpoint; + public double pivotAppliedVoltage; + // Spinners + public double spinnerSpeed; + public double spinnerSetpoint; + public double spinnerAppliedVoltage; + } - public default void updateInput(AmpBarIOInputs inputs) {} + public default void updateInput(AmpBarIOInputs inputs) {} - public default void setPivotPosition(double position) {} + public default void setPivotPosition(double position) {} - public default void setSpinnerSpeedpoint(double speed) {} + public default void setSpinnerSpeedpoint(double speed) {} - public default void stop() {} + public default void stop() {} - public default double getPivotPosition() { - return 0.0; - } + public default double getPivotPosition() { + return 0.0; + } - public default double getSpinnerSpeed() { - return 0.0; - } + public default double getSpinnerSpeed() { + return 0.0; + } - public default void configurePID(double kP, double kI, double kD) {} + public default void configurePID(double kP, double kI, double kD) {} - public default boolean atSetPoint() { - return false; - } + public default boolean atSetPoint() { + return false; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java index 876cc81..11f76fe 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOReal.java @@ -10,99 +10,101 @@ public class AmpBarIOReal implements AmpBarIO { - private final CANSparkMax leftMotor; - private final CANSparkMax rightMotor; - private final TalonFX spinnerMotor; - - private RelativeEncoder pivotEncoder; - private String stateString; - private PIDController controller; - - private double pivotMotorAppliedVoltage; - private double pivotPositionSetpoint; - - private double spinnerSpeedpoint; - private double spinnerAppliedVoltage; - - public AmpBarIOReal() { - leftMotor = new CANSparkMax(Constants.AmpBar.LEFT_PIVOT_ID, MotorType.kBrushless); - rightMotor = new CANSparkMax(Constants.AmpBar.RIGHT_PIVOT_ID, MotorType.kBrushless); - spinnerMotor = new TalonFX(Constants.AmpBar.SPINNER_ID); - - leftMotor.setInverted(false); - rightMotor.follow(leftMotor, true); - leftMotor.setIdleMode(IdleMode.kCoast); - rightMotor.setIdleMode(IdleMode.kCoast); - - pivotEncoder = leftMotor.getEncoder(); - - pivotEncoder.setPosition(0); - pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); - pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - - controller = new PIDController(0, 0, 0); - - stateString = ""; - pivotMotorAppliedVoltage = 0; - pivotPositionSetpoint = 0; - spinnerSpeedpoint = 0; - spinnerAppliedVoltage = 0; - } - - @Override - public void updateInput(AmpBarIOInputs inputs) { - inputs.ampBarState = stateString; - - inputs.pivotPosition = pivotEncoder.getPosition(); - inputs.pivotSetpoint = pivotPositionSetpoint; - inputs.pivotAppliedVoltage = pivotMotorAppliedVoltage; - - inputs.spinnerSpeed = getSpinnerSpeed(); - inputs.spinnerSetpoint = spinnerSpeedpoint; - inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; - } - - @Override - public void setPivotPosition(double position) { - pivotPositionSetpoint = position; - pivotMotorAppliedVoltage = - controller.calculate(pivotEncoder.getPosition(), pivotPositionSetpoint); - leftMotor.setVoltage(pivotMotorAppliedVoltage); - } - - public void stop() { - spinnerAppliedVoltage = 0; - pivotMotorAppliedVoltage = 0; - leftMotor.stopMotor(); - rightMotor.stopMotor(); - spinnerMotor.stopMotor(); - } - - @Override - public void setSpinnerSpeedpoint(double speed) { - spinnerSpeedpoint = speed; - spinnerAppliedVoltage = speed; - spinnerMotor.setVoltage(speed); - } - - @Override - public double getPivotPosition() { - return pivotEncoder.getPosition(); - } - - @Override - public double getSpinnerSpeed() { - return spinnerMotor.getVelocity().getValueAsDouble(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - controller.setPID(kP, kI, kD); - } - - @Override - public boolean atSetPoint() { - double motorPosition = getPivotPosition(); - return Math.abs(motorPosition - pivotPositionSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; - } + private final CANSparkMax leftMotor; + private final CANSparkMax rightMotor; + private final TalonFX spinnerMotor; + + private RelativeEncoder pivotEncoder; + private String stateString; + private PIDController controller; + + private double pivotMotorAppliedVoltage; + private double pivotPositionSetpoint; + + private double spinnerSpeedpoint; + private double spinnerAppliedVoltage; + + public AmpBarIOReal() { + leftMotor = new CANSparkMax(Constants.AmpBar.LEFT_PIVOT_ID, MotorType.kBrushless); + rightMotor = new CANSparkMax(Constants.AmpBar.RIGHT_PIVOT_ID, MotorType.kBrushless); + spinnerMotor = new TalonFX(Constants.AmpBar.SPINNER_ID); + + leftMotor.setInverted(false); + rightMotor.follow(leftMotor, true); + leftMotor.setIdleMode(IdleMode.kCoast); + rightMotor.setIdleMode(IdleMode.kCoast); + + pivotEncoder = leftMotor.getEncoder(); + + pivotEncoder.setPosition(0); + pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); + pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); + + controller = new PIDController(0, 0, 0); + + stateString = ""; + pivotMotorAppliedVoltage = 0; + pivotPositionSetpoint = 0; + spinnerSpeedpoint = 0; + spinnerAppliedVoltage = 0; + } + + @Override + public void updateInput(AmpBarIOInputs inputs) { + inputs.ampBarState = stateString; + + inputs.pivotPosition = pivotEncoder.getPosition(); + inputs.pivotSetpoint = pivotPositionSetpoint; + inputs.pivotAppliedVoltage = pivotMotorAppliedVoltage; + + inputs.spinnerSpeed = getSpinnerSpeed(); + inputs.spinnerSetpoint = spinnerSpeedpoint; + inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; + } + + @Override + public void setPivotPosition(double position) { + pivotPositionSetpoint = position; + pivotMotorAppliedVoltage = controller.calculate( + pivotEncoder.getPosition(), + pivotPositionSetpoint + ); + leftMotor.setVoltage(pivotMotorAppliedVoltage); + } + + public void stop() { + spinnerAppliedVoltage = 0; + pivotMotorAppliedVoltage = 0; + leftMotor.stopMotor(); + rightMotor.stopMotor(); + spinnerMotor.stopMotor(); + } + + @Override + public void setSpinnerSpeedpoint(double speed) { + spinnerSpeedpoint = speed; + spinnerAppliedVoltage = speed; + spinnerMotor.setVoltage(speed); + } + + @Override + public double getPivotPosition() { + return pivotEncoder.getPosition(); + } + + @Override + public double getSpinnerSpeed() { + return spinnerMotor.getVelocity().getValueAsDouble(); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } + + @Override + public boolean atSetPoint() { + double motorPosition = getPivotPosition(); + return Math.abs(motorPosition - pivotPositionSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java index 042bec9..c70e781 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarIOSim.java @@ -7,101 +7,102 @@ import frc.robot.Constants; public class AmpBarIOSim implements AmpBarIO { - SingleJointedArmSim pivotSim; - DCMotorSim spinnerSim; - PIDController controller; - - String stateString; - - double pivotAppliedVoltage; - double pivotSetpoint; - - double spinnerAppliedVoltage; - double spinnerSpeedpoint; - - public AmpBarIOSim() { - // the sim values are random - pivotSim = - new SingleJointedArmSim( - DCMotor.getNEO(Constants.AmpBar.NUM_PIVOT_MOTORS), - Constants.AmpBar.PIVOT_GEARING, - Constants.AmpBar.PIVOT_MOI, - Constants.AmpBar.PIVOT_LENGTH_METERS, - Constants.AmpBar.MIN_PIVOT_POSITION, - Constants.AmpBar.MAX_PIVOT_POSITION, - false, - 0); - spinnerSim = - new DCMotorSim( - DCMotor.getKrakenX60(Constants.AmpBar.NUM_SPINNER_MOTORS), - Constants.AmpBar.SPINNER_GEARING, - Constants.AmpBar.SPINNER_MOI); - - controller = new PIDController(0, 0, 0); - stateString = ""; - - pivotAppliedVoltage = 0; - pivotSetpoint = 0; - spinnerAppliedVoltage = 0; - spinnerSpeedpoint = 0; - } - - @Override - public void updateInput(AmpBarIOInputs inputs) { - inputs.ampBarState = stateString; - - inputs.pivotPosition = getPivotPosition(); - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - - inputs.spinnerSpeed = getSpinnerSpeed(); - inputs.spinnerSetpoint = spinnerSpeedpoint; - inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; - - pivotSim.update(Constants.SIM_UPDATE_TIME); - spinnerSim.update(Constants.SIM_UPDATE_TIME); - } - - @Override - public void setPivotPosition(double position) { - pivotSetpoint = position; - pivotAppliedVoltage = controller.calculate(pivotSim.getAngleRads(), pivotSetpoint); - pivotSim.setInputVoltage(pivotAppliedVoltage); - } - - @Override - public void setSpinnerSpeedpoint(double speed) { - spinnerSpeedpoint = speed; - spinnerAppliedVoltage = speed; - spinnerSim.setInputVoltage(speed); - } - - @Override - public void stop() { - pivotAppliedVoltage = 0; - spinnerAppliedVoltage = 0; - spinnerSim.setInputVoltage(0); - pivotSim.setInputVoltage(0); - } - - @Override - public double getPivotPosition() { - return pivotSim.getAngleRads(); - } - - @Override - public double getSpinnerSpeed() { - return spinnerSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - } - - @Override - public void configurePID(double kP, double kI, double kD) { - controller.setPID(kP, kI, kD); - } - - @Override - public boolean atSetPoint() { - double motorPosition = getPivotPosition(); - return Math.abs(motorPosition - pivotSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; - } + + SingleJointedArmSim pivotSim; + DCMotorSim spinnerSim; + PIDController controller; + + String stateString; + + double pivotAppliedVoltage; + double pivotSetpoint; + + double spinnerAppliedVoltage; + double spinnerSpeedpoint; + + public AmpBarIOSim() { + // the sim values are random + pivotSim = new SingleJointedArmSim( + DCMotor.getNEO(Constants.AmpBar.NUM_PIVOT_MOTORS), + Constants.AmpBar.PIVOT_GEARING, + Constants.AmpBar.PIVOT_MOI, + Constants.AmpBar.PIVOT_LENGTH_METERS, + Constants.AmpBar.MIN_PIVOT_POSITION, + Constants.AmpBar.MAX_PIVOT_POSITION, + false, + 0 + ); + spinnerSim = new DCMotorSim( + DCMotor.getKrakenX60(Constants.AmpBar.NUM_SPINNER_MOTORS), + Constants.AmpBar.SPINNER_GEARING, + Constants.AmpBar.SPINNER_MOI + ); + + controller = new PIDController(0, 0, 0); + stateString = ""; + + pivotAppliedVoltage = 0; + pivotSetpoint = 0; + spinnerAppliedVoltage = 0; + spinnerSpeedpoint = 0; + } + + @Override + public void updateInput(AmpBarIOInputs inputs) { + inputs.ampBarState = stateString; + + inputs.pivotPosition = getPivotPosition(); + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + + inputs.spinnerSpeed = getSpinnerSpeed(); + inputs.spinnerSetpoint = spinnerSpeedpoint; + inputs.spinnerAppliedVoltage = spinnerAppliedVoltage; + + pivotSim.update(Constants.SIM_UPDATE_TIME); + spinnerSim.update(Constants.SIM_UPDATE_TIME); + } + + @Override + public void setPivotPosition(double position) { + pivotSetpoint = position; + pivotAppliedVoltage = controller.calculate(pivotSim.getAngleRads(), pivotSetpoint); + pivotSim.setInputVoltage(pivotAppliedVoltage); + } + + @Override + public void setSpinnerSpeedpoint(double speed) { + spinnerSpeedpoint = speed; + spinnerAppliedVoltage = speed; + spinnerSim.setInputVoltage(speed); + } + + @Override + public void stop() { + pivotAppliedVoltage = 0; + spinnerAppliedVoltage = 0; + spinnerSim.setInputVoltage(0); + pivotSim.setInputVoltage(0); + } + + @Override + public double getPivotPosition() { + return pivotSim.getAngleRads(); + } + + @Override + public double getSpinnerSpeed() { + return spinnerSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + } + + @Override + public void configurePID(double kP, double kI, double kD) { + controller.setPID(kP, kI, kD); + } + + @Override + public boolean atSetPoint() { + double motorPosition = getPivotPosition(); + return Math.abs(motorPosition - pivotSetpoint) <= Constants.AmpBar.ERROR_OF_MARGIN; + } } diff --git a/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java b/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java index 4dadaef..f468c30 100644 --- a/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java +++ b/src/main/java/frc/robot/subsystems/ampBar/AmpBarStates.java @@ -4,31 +4,31 @@ import frc.robot.subsystems.SubsystemStates; public enum AmpBarStates implements SubsystemStates { - OFF(Constants.AmpBar.IN, Constants.AmpBar.OFF, "Amp Bar Off"), - SHOOTING(Constants.AmpBar.OUT, Constants.AmpBar.SHOOTING, "Shooting Amp"), - FEEDING(Constants.AmpBar.FEEDING_POSITION, Constants.AmpBar.FEEDING, "Getting Fed"), - HOLDING_NOTE(Constants.AmpBar.OUT, Constants.AmpBar.OFF, "Holding a Note"); + OFF(Constants.AmpBar.IN, Constants.AmpBar.OFF, "Amp Bar Off"), + SHOOTING(Constants.AmpBar.OUT, Constants.AmpBar.SHOOTING, "Shooting Amp"), + FEEDING(Constants.AmpBar.FEEDING_POSITION, Constants.AmpBar.FEEDING, "Getting Fed"), + HOLDING_NOTE(Constants.AmpBar.OUT, Constants.AmpBar.OFF, "Holding a Note"); - private double pivotPositionSetpoint; - private double spinnerMotorSpeedpoint; - private String stateString; + private double pivotPositionSetpoint; + private double spinnerMotorSpeedpoint; + private String stateString; - AmpBarStates(double pivotMotorSetpoint, double spinnerMotorSpeedpoint, String stateString) { - this.pivotPositionSetpoint = pivotMotorSetpoint; - this.spinnerMotorSpeedpoint = spinnerMotorSpeedpoint; - this.stateString = stateString; - } + AmpBarStates(double pivotMotorSetpoint, double spinnerMotorSpeedpoint, String stateString) { + this.pivotPositionSetpoint = pivotMotorSetpoint; + this.spinnerMotorSpeedpoint = spinnerMotorSpeedpoint; + this.stateString = stateString; + } - public double getPivotPositionSetpoint() { - return pivotPositionSetpoint; - } + public double getPivotPositionSetpoint() { + return pivotPositionSetpoint; + } - public double getMotorSpeedpoint() { - return spinnerMotorSpeedpoint; - } + public double getMotorSpeedpoint() { + return spinnerMotorSpeedpoint; + } - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c600b97..8fc9490 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -38,331 +38,361 @@ import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - static final Lock odometryLock = new ReentrantLock(); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR - - private DriveStates state; - private String subsystemName; - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = new Rotation2d(); - private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; - private SwerveDrivePoseEstimator poseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) { - - subsystemName = "Drive"; - state = DriveStates.REGULAR_DRIVE; - - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); - - // Start threads (no-op for each if no signals have been created) - PhoenixOdometryThread.getInstance().start(); - SparkMaxOdometryThread.getInstance().start(); - - pathPlannerInit(); - } - - public void runState() { - drive( - this, - () -> Constants.controller.getLeftY(), - () -> Constants.controller.getLeftX(), - () -> -Constants.controller.getRightX(), - getState().getRotationModifier(), - getState().getTranslationModifier()); - } - - public DriveStates getState() { - return state; - } - - public void setState(DriveStates state) { - this.state = state; - } - - public void drive( - Drive drive, - DoubleSupplier xSupplier, - DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier, - double rotationMultiplier, - double translationMultiplier) { - // Apply deadband - double linearMagnitude = - MathUtil.applyDeadband( - Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), - Constants.Drive.CONTROLLER_DEADBAND); - Rotation2d linearDirection = new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); - double omega = - MathUtil.applyDeadband(omegaSupplier.getAsDouble(), Constants.Drive.CONTROLLER_DEADBAND); - - // Square values - linearMagnitude = linearMagnitude * linearMagnitude; - omega = Math.copySign(omega * omega, omega); - - // Calcaulate new linear velocity - Translation2d linearVelocity = - new Pose2d(new Translation2d(), linearDirection) - .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) - .getTranslation(); - - // Convert to field relative speeds & send command - boolean isFlipped = - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec() * translationMultiplier, - omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, - isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation())); - } - - @Override - public void periodic() { - super.periodic(); - odometryLock.lock(); // Prevents odometry updates while reading data - gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); - } - odometryLock.unlock(); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); - } - - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - - // Update odometry - double[] sampleTimestamps = - modules[0].getOdometryTimestamps(); // All signals are sampled together - int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = - new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - for (int moduleIndex = 0; moduleIndex < Constants.Drive.NUM_MODULES; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - // Update gyro angle - if (gyroInputs.connected) { - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; - } else { - // Use the angle delta from the kinematics and module deltas - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - } - - // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); - } - - if (Constants.controller.getRightBumper() && getState() == DriveStates.REGULAR_DRIVE) { - setState(DriveStates.SLOW_MODE); - } else if (Constants.controller.getLeftBumper() && getState() == DriveStates.REGULAR_DRIVE) { - setState(DriveStates.SPEED_MAXXING); - } else if (!Constants.controller.getRightBumper() && !Constants.controller.getLeftBumper()) { - setState(DriveStates.REGULAR_DRIVE); - } - } - - /** - * Runs the drive at the desired velocity. - * - * @param speeds Speeds in meters/sec - */ - public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints - ChassisSpeeds discreteSpeeds = - ChassisSpeeds.discretize(speeds, Constants.Drive.DISCRETIZE_TIME_SECONDS); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, Constants.Drive.MAX_LINEAR_SPEED); - - // Send setpoints to modules - SwerveModuleState[] optimizedSetpointStates = - new SwerveModuleState[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); - } - - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); - } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); - } - kinematics.resetHeadings(headings); - stop(); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - states[i] = modules[i].getState(); - } - return states; - } - - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ - private SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] states = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; - for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { - states[i] = modules[i].getPosition(); - } - return states; - } - - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public ChassisSpeeds getChassisSpeed() { - ChassisSpeeds robotChassisSpeed = kinematics.toChassisSpeeds(getModuleStates()); - return robotChassisSpeed; - } - - public double calculateVelocity() { - double robotSpeed = - Math.sqrt( - Math.pow(getChassisSpeed().vxMetersPerSecond, 2) - + Math.pow(getChassisSpeed().vyMetersPerSecond, 2)); - return robotSpeed; - } - - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); - } - - /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); - } - - /** - * Adds a vision measurement to the pose estimator. - * - * @param visionPose The pose of the robot as measured by the vision camera. - * @param timestamp The timestamp of the vision measurement in seconds. - */ - public void addVisionMeasurement(Pose2d visionPose, double timestamp) { - poseEstimator.addVisionMeasurement(visionPose, timestamp); - } - - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return Constants.Drive.MAX_LINEAR_SPEED; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return Constants.Drive.MAX_ANGULAR_SPEED; - } - - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d( - Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), - new Translation2d( - Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), - new Translation2d( - -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF), - new Translation2d( - -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, - -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF) - }; - } - - public void pathPlannerInit() { - AutoBuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in - // your Constants class - Constants.Drive.TRANSLATION_PID, // Translation PID constants - Constants.Drive.ROTATION_PID, // Rotation PID constants - Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s - Constants.Drive - .DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to - // furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options - // here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } + + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[Constants.Drive.NUM_MODULES]; // FL, FR, BL, BR + + private DriveStates state; + private String subsystemName; + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; + private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + lastModulePositions, + new Pose2d() + ); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO + ) { + subsystemName = "Drive"; + state = DriveStates.REGULAR_DRIVE; + + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Start threads (no-op for each if no signals have been created) + PhoenixOdometryThread.getInstance().start(); + SparkMaxOdometryThread.getInstance().start(); + + pathPlannerInit(); + } + + public void runState() { + drive( + this, + () -> Constants.controller.getLeftY(), + () -> Constants.controller.getLeftX(), + () -> -Constants.controller.getRightX(), + getState().getRotationModifier(), + getState().getTranslationModifier() + ); + } + + public DriveStates getState() { + return state; + } + + public void setState(DriveStates state) { + this.state = state; + } + + public void drive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, + double rotationMultiplier, + double translationMultiplier + ) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), + Constants.Drive.CONTROLLER_DEADBAND + ); + Rotation2d linearDirection = new Rotation2d( + xSupplier.getAsDouble(), + ySupplier.getAsDouble() + ); + double omega = MathUtil.applyDeadband( + omegaSupplier.getAsDouble(), + Constants.Drive.CONTROLLER_DEADBAND + ); + + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); + + // Calcaulate new linear velocity + Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec() * + translationMultiplier, + omega * drive.getMaxAngularSpeedRadPerSec() * rotationMultiplier, + isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation() + ) + ); + } + + @Override + public void periodic() { + super.periodic(); + odometryLock.lock(); // Prevents odometry updates while reading data + gyroIO.updateInputs(gyroInputs); + for (var module : modules) { + module.updateInputs(); + } + odometryLock.unlock(); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + } + + // Update odometry + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = + new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + SwerveModulePosition[] moduleDeltas = + new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + for (int moduleIndex = 0; moduleIndex < Constants.Drive.NUM_MODULES; moduleIndex++) { + modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters - + lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle + ); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + } + + // Apply update + poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + } + + if (Constants.controller.getRightBumper() && getState() == DriveStates.REGULAR_DRIVE) { + setState(DriveStates.SLOW_MODE); + } else if ( + Constants.controller.getLeftBumper() && getState() == DriveStates.REGULAR_DRIVE + ) { + setState(DriveStates.SPEED_MAXXING); + } else if ( + !Constants.controller.getRightBumper() && !Constants.controller.getLeftBumper() + ) { + setState(DriveStates.REGULAR_DRIVE); + } + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize( + speeds, + Constants.Drive.DISCRETIZE_TIME_SECONDS + ); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, + Constants.Drive.MAX_LINEAR_SPEED + ); + + // Send setpoints to modules + SwerveModuleState[] optimizedSetpointStates = + new SwerveModuleState[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + // The module returns the optimized state, useful for logging + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + } + + // Log setpoint states + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + states[i] = modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[Constants.Drive.NUM_MODULES]; + for (int i = 0; i < Constants.Drive.NUM_MODULES; i++) { + states[i] = modules[i].getPosition(); + } + return states; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + public ChassisSpeeds getChassisSpeed() { + ChassisSpeeds robotChassisSpeed = kinematics.toChassisSpeeds(getModuleStates()); + return robotChassisSpeed; + } + + public double calculateVelocity() { + double robotSpeed = Math.sqrt( + Math.pow(getChassisSpeed().vxMetersPerSecond, 2) + + Math.pow(getChassisSpeed().vyMetersPerSecond, 2) + ); + return robotSpeed; + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + poseEstimator.addVisionMeasurement(visionPose, timestamp); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return Constants.Drive.MAX_LINEAR_SPEED; + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return Constants.Drive.MAX_ANGULAR_SPEED; + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d( + Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + new Translation2d( + Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + new Translation2d( + -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + new Translation2d( + -Constants.Drive.TRACK_WIDTH_X / Constants.DIAM_TO_RADIUS_CF, + -Constants.Drive.TRACK_WIDTH_Y / Constants.DIAM_TO_RADIUS_CF + ), + }; + } + + public void pathPlannerInit() { + AutoBuilder.configureHolonomic( + this::getPose, // Robot pose supplier + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::runVelocity, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( + // HolonomicPathFollowerConfig, this should likely live in + // your Constants class + Constants.Drive.TRANSLATION_PID, // Translation PID constants + Constants.Drive.ROTATION_PID, // Rotation PID constants + Constants.Drive.MAX_MODULE_SPEED, // Max module speed, in m/s + Constants.Drive.DRIVE_BASE_RADIUS, // Drive base radius in meters. Distance from robot center to + // furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options + // here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveStates.java b/src/main/java/frc/robot/subsystems/drive/DriveStates.java index d3e6446..5f07146 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveStates.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveStates.java @@ -4,33 +4,33 @@ import frc.robot.subsystems.SubsystemStates; public enum DriveStates implements SubsystemStates { - REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), - SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), - AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), - SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); - - // Someone make auto align so that state is useful please - - DriveStates(String stateString, double translationModifier, double rotationModifier) { - this.stateString = stateString; - this.translationModifier = translationModifier; - this.rotationModifier = rotationModifier; - } - - String stateString; - double translationModifier; - double rotationModifier; - - @Override - public String getStateString() { - return stateString; - } - - public double getTranslationModifier() { - return translationModifier; - } - - public double getRotationModifier() { - return rotationModifier; - } + REGULAR_DRIVE("Regular Drive", Constants.Drive.REGULAR_TM, Constants.Drive.REGULAR_RM), + SLOW_MODE("Slow Mode", Constants.Drive.SLOW_TM, Constants.Drive.SLOW_RM), + AUTO_ALIGN("Auto Aligning", Constants.Drive.AA_TM, Constants.Drive.AA_RM), + SPEED_MAXXING("Speed Maxxing", Constants.Drive.FAST_TM, Constants.Drive.FAST_RM); + + // Someone make auto align so that state is useful please + + DriveStates(String stateString, double translationModifier, double rotationModifier) { + this.stateString = stateString; + this.translationModifier = translationModifier; + this.rotationModifier = rotationModifier; + } + + String stateString; + double translationModifier; + double rotationModifier; + + @Override + public String getStateString() { + return stateString; + } + + public double getTranslationModifier() { + return translationModifier; + } + + public double getRotationModifier() { + return rotationModifier; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 18ce6fd..8efbde0 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -17,14 +17,15 @@ import org.littletonrobotics.junction.AutoLog; public interface GyroIO { - @AutoLog - public static class GyroIOInputs { - public boolean connected = false; - public Rotation2d yawPosition = new Rotation2d(); - public double[] odometryYawTimestamps = new double[] {}; - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; - public double yawVelocityRadPerSec = 0.0; - } + @AutoLog + public static class GyroIOInputs { - public default void updateInputs(GyroIOInputs inputs) {} + public boolean connected = false; + public Rotation2d yawPosition = new Rotation2d(); + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double yawVelocityRadPerSec = 0.0; + } + + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 08cf5e3..391357c 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -26,51 +26,52 @@ /** IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(Constants.Drive.Pidgeon2.DEVICE_ID); - private final StatusSignal yaw = pigeon.getYaw(); - private final Queue yawPositionQueue; - private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - public GyroIOPigeon2(boolean phoenixDrive) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Constants.Drive.Module.ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(Constants.Drive.Pidgeon2.UPDATE_FREQUENCY); - pigeon.optimizeBusUtilization(); - if (phoenixDrive) { - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); - } else { - yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - boolean valid = yaw.refresh().getStatus().isOK(); - if (valid) { - return OptionalDouble.of(yaw.getValueAsDouble()); - } else { - return OptionalDouble.empty(); - } - }); - } - } + private final Pigeon2 pigeon = new Pigeon2(Constants.Drive.Pidgeon2.DEVICE_ID); + private final StatusSignal yaw = pigeon.getYaw(); + private final Queue yawPositionQueue; + private final Queue yawTimestampQueue; + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + public GyroIOPigeon2(boolean phoenixDrive) { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(Constants.Drive.Module.ODOMETRY_FREQUENCY); + yawVelocity.setUpdateFrequency(Constants.Drive.Pidgeon2.UPDATE_FREQUENCY); + pigeon.optimizeBusUtilization(); + if (phoenixDrive) { + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(pigeon, pigeon.getYaw()); + } else { + yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + boolean valid = yaw.refresh().getStatus().isOK(); + if (valid) { + return OptionalDouble.of(yaw.getValueAsDouble()); + } else { + return OptionalDouble.empty(); + } + }); + } + } - inputs.odometryYawTimestamps = - yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryYawPositions = - yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) - .toArray(Rotation2d[]::new); - yawTimestampQueue.clear(); - yawPositionQueue.clear(); - } + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + + inputs.odometryYawTimestamps = yawTimestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryYawPositions = yawPositionQueue + .stream() + .map((Double value) -> Rotation2d.fromDegrees(value)) + .toArray(Rotation2d[]::new); + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index d48f770..cd764c7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -23,202 +23,209 @@ import org.littletonrobotics.junction.Logger; public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(Constants.Drive.WHEEL_RADIUS); - - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.currentMode) { - // TODO: Configure PID in rela mabye? idk why its off - case REAL: - case REPLAY: - driveFeedforward = - new SimpleMotorFeedforward( - Constants.Drive.Module.REPLAY_FF.kS, Constants.Drive.Module.REPLAY_FF.kV); - driveFeedback = - new PIDController( - Constants.Drive.Module.REPLAY_DRIVE_PID.kP, - Constants.Drive.Module.REPLAY_DRIVE_PID.kI, - Constants.Drive.Module.REPLAY_DRIVE_PID.kD); - turnFeedback = - new PIDController( - Constants.Drive.Module.REPLAY_TURN_PID.kP, - Constants.Drive.Module.REPLAY_TURN_PID.kI, - Constants.Drive.Module.REPLAY_TURN_PID.kD); - break; - case SIM: - driveFeedforward = - new SimpleMotorFeedforward( - Constants.Drive.Module.SIM_FF.kS, Constants.Drive.Module.SIM_FF.kV); - driveFeedback = - new PIDController( - Constants.Drive.Module.SIM_DRIVE_PID.kP, - Constants.Drive.Module.SIM_DRIVE_PID.kI, - Constants.Drive.Module.SIM_DRIVE_PID.kD); - turnFeedback = - new PIDController( - Constants.Drive.Module.SIM_TURN_PID.kP, - Constants.Drive.Module.SIM_TURN_PID.kI, - Constants.Drive.Module.SIM_TURN_PID.kD); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - turnFeedback = new PIDController(0.0, 0.0, 0.0); - break; - } - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } - - /** - * Update inputs without running the rest of the periodic logic. This is useful since these - * updates need to be properly thread-locked. - */ - public void updateInputs() { - io.updateInputs(inputs); - } - - public void periodic() { - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - // Run closed loop turn control - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } - } - - // Calculate positions for odometry - int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; - for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; - Rotation2d angle = - inputs.odometryTurnPositions[i].plus( - turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - } - - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - // Update setpoints, controllers run in "periodic" - angleSetpoint = optimizedState.angle; - speedSetpoint = optimizedState.speedMetersPerSecond; - - return optimizedState; - } - - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - - /** Disables all outputs to motors. */ - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - /** Returns the current turn angle of the module. */ - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); - } - } - - /** Returns the current drive position of the module in meters. */ - public double getPositionMeters() { - return inputs.drivePositionRad * WHEEL_RADIUS; - } - - /** Returns the current drive velocity of the module in meters per second. */ - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; - } - - /** Returns the module position (turn angle and drive position). */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - - /** Returns the timestamps of the samples received this cycle. */ - public double[] getOdometryTimestamps() { - return inputs.odometryTimestamps; - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } + + private static final double WHEEL_RADIUS = Units.inchesToMeters(Constants.Drive.WHEEL_RADIUS); + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final int index; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + public Module(ModuleIO io, int index) { + this.io = io; + this.index = index; + + // Switch constants based on mode (the physics simulator is treated as a + // separate robot with different tuning) + switch (Constants.currentMode) { + // TODO: Configure PID in rela mabye? idk why its off + case REAL: + case REPLAY: + driveFeedforward = new SimpleMotorFeedforward( + Constants.Drive.Module.REPLAY_FF.kS, + Constants.Drive.Module.REPLAY_FF.kV + ); + driveFeedback = new PIDController( + Constants.Drive.Module.REPLAY_DRIVE_PID.kP, + Constants.Drive.Module.REPLAY_DRIVE_PID.kI, + Constants.Drive.Module.REPLAY_DRIVE_PID.kD + ); + turnFeedback = new PIDController( + Constants.Drive.Module.REPLAY_TURN_PID.kP, + Constants.Drive.Module.REPLAY_TURN_PID.kI, + Constants.Drive.Module.REPLAY_TURN_PID.kD + ); + break; + case SIM: + driveFeedforward = new SimpleMotorFeedforward( + Constants.Drive.Module.SIM_FF.kS, + Constants.Drive.Module.SIM_FF.kV + ); + driveFeedback = new PIDController( + Constants.Drive.Module.SIM_DRIVE_PID.kP, + Constants.Drive.Module.SIM_DRIVE_PID.kI, + Constants.Drive.Module.SIM_DRIVE_PID.kD + ); + turnFeedback = new PIDController( + Constants.Drive.Module.SIM_TURN_PID.kP, + Constants.Drive.Module.SIM_TURN_PID.kI, + Constants.Drive.Module.SIM_TURN_PID.kD + ); + break; + default: + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + driveFeedback = new PIDController(0.0, 0.0, 0.0); + turnFeedback = new PIDController(0.0, 0.0, 0.0); + break; + } + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } + + /** + * Update inputs without running the rest of the periodic logic. This is useful since these + * updates need to be properly thread-locked. + */ + public void updateInputs() { + io.updateInputs(inputs); + } + + public void periodic() { + Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + + // On first cycle, reset relative turn encoder + // Wait until absolute angle is nonzero in case it wasn't initialized yet + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + // Run closed loop turn control + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()) + ); + + // Run closed loop drive control + // Only allowed if closed loop turn control is running + if (speedSetpoint != null) { + // Scale velocity based on turn error + // + // When the error is 90°, the velocity setpoint should be 0. As the wheel turns + // towards the setpoint, its velocity should increase. This is achieved by + // taking the component of the velocity in the direction of the setpoint. + double adjustSpeedSetpoint = + speedSetpoint * Math.cos(turnFeedback.getPositionError()); + + // Run drive controller + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec) + ); + } + } + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; + Rotation2d angle = + inputs.odometryTurnPositions[i].plus( + turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d() + ); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Runs the module with the specified setpoint state. Returns the optimized state. */ + public SwerveModuleState runSetpoint(SwerveModuleState state) { + // Optimize state based on current angle + // Controllers run in "periodic" when the setpoint is not null + var optimizedState = SwerveModuleState.optimize(state, getAngle()); + + // Update setpoints, controllers run in "periodic" + angleSetpoint = optimizedState.angle; + speedSetpoint = optimizedState.speedMetersPerSecond; + + return optimizedState; + } + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + public void runCharacterization(double volts) { + // Closed loop turn control + angleSetpoint = new Rotation2d(); + + // Open loop drive control + io.setDriveVoltage(volts); + speedSetpoint = null; + } + + /** Disables all outputs to motors. */ + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); + + // Disable closed loop control for turn and drive + angleSetpoint = null; + speedSetpoint = null; + } + + /** Sets whether brake mode is enabled. */ + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** Returns the drive velocity in radians/sec. */ + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 200afa3..91ba0e3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,36 +17,37 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - public static class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double[] driveCurrentAmps = new double[] {}; - - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double[] turnCurrentAmps = new double[] {}; - - public double[] odometryTimestamps = new double[] {}; - public double[] odometryDrivePositionsRad = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} - - /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} - - /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} - - /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} - - /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + @AutoLog + public static class ModuleIOInputs { + + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java index 8a4744a..0c902f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOHybrid.java @@ -27,175 +27,191 @@ * CANcoder */ public class ModuleIOHybrid implements ModuleIO { - private final TalonFX driveTalon; - private final CANSparkMax turnSparkMax; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final RelativeEncoder turnRelativeEncoder; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOHybrid(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE0_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN0_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER0_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET0); - break; - case 1: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE1_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN1_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER1_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET1); - break; - case 2: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE2_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN2_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER2_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET2); - break; - case 3: - driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE3_ID); - turnSparkMax = - new CANSparkMax(Constants.Drive.Module.Hybrid.TURN3_ID, MotorType.kBrushless); - cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER3_ID); - absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET3); - break; - default: - throw new RuntimeException("Invalid module index"); - } - - // TalonFX configuration - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = - Constants.Drive.Module.Hybrid.DRIVE_CURRENT_LIMIT; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.ODOMETRY_FREQUENCY, drivePosition); - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.Hybrid.TALON_UPDATE_FREQUENCY_HZ, - driveVelocity, - driveAppliedVolts, - driveCurrent); - driveTalon.optimizeBusUtilization(); - - // SparkMax configuration - turnSparkMax.restoreFactoryDefaults(); - turnSparkMax.setCANTimeout(Constants.Drive.Module.Hybrid.SPARK_TIMEOUT_MS); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - turnSparkMax.setSmartCurrentLimit(Constants.Drive.Module.Hybrid.TURN_CURRENT_LIMIT); - turnSparkMax.enableVoltageCompensation(Constants.MAX_VOLTS); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod( - Constants.Drive.Module.Hybrid.SPARK_MEASURMENT_PERIOD_MS); - turnRelativeEncoder.setAverageDepth(Constants.Drive.Module.Hybrid.SPARK_AVG_DEPTH); - - turnSparkMax.setCANTimeout(0); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (Constants.Drive.Module.Hybrid.SPARK_FRAME_PERIOD)); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - - // Turn Stuff - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(cancoder.getAbsolutePosition().getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations( - turnRelativeEncoder.getPosition() / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - // Other stuff - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble( - (Double value) -> - Units.rotationsToRadians(value) - / Constants.Drive.Module.Hybrid.DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map( - (Double value) -> - Rotation2d.fromRotations(value / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + + private final TalonFX driveTalon; + private final CANSparkMax turnSparkMax; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final RelativeEncoder turnRelativeEncoder; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOHybrid(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE0_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN0_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER0_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET0); + break; + case 1: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE1_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN1_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER1_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET1); + break; + case 2: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE2_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN2_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER2_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET2); + break; + case 3: + driveTalon = new TalonFX(Constants.Drive.Module.Hybrid.DRIVE3_ID); + turnSparkMax = new CANSparkMax( + Constants.Drive.Module.Hybrid.TURN3_ID, + MotorType.kBrushless + ); + cancoder = new CANcoder(Constants.Drive.Module.Hybrid.CANCODER3_ID); + absoluteEncoderOffset = new Rotation2d(Constants.Drive.Module.Hybrid.OFFSET3); + break; + default: + throw new RuntimeException("Invalid module index"); + } + + // TalonFX configuration + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = + Constants.Drive.Module.Hybrid.DRIVE_CURRENT_LIMIT; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.ODOMETRY_FREQUENCY, + drivePosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.Hybrid.TALON_UPDATE_FREQUENCY_HZ, + driveVelocity, + driveAppliedVolts, + driveCurrent + ); + driveTalon.optimizeBusUtilization(); + + // SparkMax configuration + turnSparkMax.restoreFactoryDefaults(); + turnSparkMax.setCANTimeout(Constants.Drive.Module.Hybrid.SPARK_TIMEOUT_MS); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + turnSparkMax.setSmartCurrentLimit(Constants.Drive.Module.Hybrid.TURN_CURRENT_LIMIT); + turnSparkMax.enableVoltageCompensation(Constants.MAX_VOLTS); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod( + Constants.Drive.Module.Hybrid.SPARK_MEASURMENT_PERIOD_MS + ); + turnRelativeEncoder.setAverageDepth(Constants.Drive.Module.Hybrid.SPARK_AVG_DEPTH); + + turnSparkMax.setCANTimeout(0); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, + (int) (Constants.Drive.Module.Hybrid.SPARK_FRAME_PERIOD) + ); + turnPositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + + // Turn Stuff + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + cancoder.getAbsolutePosition().getValueAsDouble() + ).minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations( + turnRelativeEncoder.getPosition() / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO + ); + inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() + ) / + Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] { turnSparkMax.getOutputCurrent() }; + + // Other stuff + inputs.odometryTimestamps = timestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue + .stream() + .mapToDouble( + (Double value) -> + Units.rotationsToRadians(value) / Constants.Drive.Module.Hybrid.DRIVE_GEAR_RATIO + ) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue + .stream() + .map((Double value) -> + Rotation2d.fromRotations(value / Constants.Drive.Module.Hybrid.TURN_GEAR_RATIO) + ) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index e5f64b7..e552626 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -29,54 +29,56 @@ */ public class ModuleIOSim implements ModuleIO { - private DCMotorSim driveSim = - new DCMotorSim( - DCMotor.getNEO(Constants.Drive.Module.NUM_DRIVE_MOTORS), - Constants.Drive.Module.Sim.DRIVE_GEARING, - Constants.Drive.Module.Sim.DRIVE_MOI); - private DCMotorSim turnSim = - new DCMotorSim( - DCMotor.getNEO(Constants.Drive.Module.NUM_TURN_MOTORS), - Constants.Drive.Module.Sim.TURN_GEARING, - Constants.Drive.Module.Sim.TURN_MOI); + private DCMotorSim driveSim = new DCMotorSim( + DCMotor.getNEO(Constants.Drive.Module.NUM_DRIVE_MOTORS), + Constants.Drive.Module.Sim.DRIVE_GEARING, + Constants.Drive.Module.Sim.DRIVE_MOI + ); + private DCMotorSim turnSim = new DCMotorSim( + DCMotor.getNEO(Constants.Drive.Module.NUM_TURN_MOTORS), + Constants.Drive.Module.Sim.TURN_GEARING, + Constants.Drive.Module.Sim.TURN_MOI + ); - private final Rotation2d turnAbsoluteInitPosition = - new Rotation2d(Math.random() * Constants.RADIAN_CF); - private double driveAppliedVolts = 0.0; - private double turnAppliedVolts = 0.0; + private final Rotation2d turnAbsoluteInitPosition = new Rotation2d( + Math.random() * Constants.RADIAN_CF + ); + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; - @Override - public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); - turnSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); + @Override + public void updateInputs(ModuleIOInputs inputs) { + driveSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); + turnSim.update(Constants.Drive.Module.Sim.LOOP_PERIOD_SECS); - inputs.drivePositionRad = driveSim.getAngularPositionRad(); - inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); - inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = new double[] {Math.abs(driveSim.getCurrentDrawAmps())}; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = new double[] { Math.abs(driveSim.getCurrentDrawAmps()) }; - inputs.turnAbsolutePosition = - new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); - inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); - inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus( + turnAbsoluteInitPosition + ); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = new double[] { Math.abs(turnSim.getCurrentDrawAmps()) }; - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; - } + inputs.odometryTimestamps = new double[] { Timer.getFPGATimestamp() }; + inputs.odometryDrivePositionsRad = new double[] { inputs.drivePositionRad }; + inputs.odometryTurnPositions = new Rotation2d[] { inputs.turnPosition }; + } - @Override - public void setDriveVoltage(double volts) { - // :( it doesent work if you clamp volts regularly idk. It really just stopped working - driveAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); - driveSim.setInputVoltage(volts); - } + @Override + public void setDriveVoltage(double volts) { + // :( it doesent work if you clamp volts regularly idk. It really just stopped working + driveAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); + driveSim.setInputVoltage(volts); + } - @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); - turnSim.setInputVoltage(turnAppliedVolts); - } + @Override + public void setTurnVoltage(double volts) { + turnAppliedVolts = MathUtil.clamp(volts, Constants.MIN_VOLTS, Constants.MAX_VOLTS); + turnSim.setInputVoltage(turnAppliedVolts); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index f7e8849..52272ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -40,164 +40,170 @@ * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue timestampQueue; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY)); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY)); - timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = driveEncoder.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = turnRelativeEncoder.getPosition(); - if (turnSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = - new Rotation2d( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } + + // Gear ratios for SDS MK4i L2, adjust as necessary + private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + private final Queue timestampQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOSparkMax(int index) { + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(1, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(2, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(0); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveSparkMax = new CANSparkMax(3, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(4, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(1); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveSparkMax = new CANSparkMax(5, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(6, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveSparkMax = new CANSparkMax(7, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(3); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); + + driveSparkMax.setCANTimeout(250); + turnSparkMax.setCANTimeout(250); + + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + + turnSparkMax.setInverted(isTurnMotorInverted); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, + (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY) + ); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, + (int) (1000.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY) + ); + timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + double value = driveEncoder.getPosition(); + if (driveSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + turnPositionQueue = SparkMaxOdometryThread.getInstance() + .registerSignal(() -> { + double value = turnRelativeEncoder.getPosition(); + if (turnSparkMax.getLastError() == REVLibError.kOk) { + return OptionalDouble.of(value); + } else { + return OptionalDouble.empty(); + } + }); + + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = Units.rotationsToRadians(driveEncoder.getPosition()) / + DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() + ) / + DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); + inputs.driveCurrentAmps = new double[] { driveSparkMax.getOutputCurrent() }; + + inputs.turnAbsolutePosition = new Rotation2d( + (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) * 2.0 * Math.PI + ).minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations( + turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO + ); + inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() + ) / + TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] { turnSparkMax.getOutputCurrent() }; + + inputs.odometryTimestamps = timestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue + .stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue + .stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index dd179d4..7b1caa4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -41,179 +41,187 @@ * "/Drive/ModuleX/TurnAbsolutePositionRad" */ public class ModuleIOTalonFX implements ModuleIO { - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - - private final Queue timestampQueue; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOTalonFX(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(0); - turnTalon = new TalonFX(1); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveTalon = new TalonFX(6); - turnTalon = new TalonFX(7); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; - driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; - turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - turnTalon.getConfigurator().apply(turnConfig); - setTurnBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getSupplyCurrent(); - - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Constants.Drive.Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); - } + + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + private final Queue timestampQueue; + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Gear ratios for SDS MK4i L2, adjust as necessary + private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double TURN_GEAR_RATIO = 150.0 / 7.0; + + private final boolean isTurnMotorInverted = true; + private final Rotation2d absoluteEncoderOffset; + + public ModuleIOTalonFX(int index) { + switch (index) { + case 0: + driveTalon = new TalonFX(0); + turnTalon = new TalonFX(1); + cancoder = new CANcoder(2); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 1: + driveTalon = new TalonFX(3); + turnTalon = new TalonFX(4); + cancoder = new CANcoder(5); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 2: + driveTalon = new TalonFX(6); + turnTalon = new TalonFX(7); + cancoder = new CANcoder(8); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + case 3: + driveTalon = new TalonFX(9); + turnTalon = new TalonFX(10); + cancoder = new CANcoder(11); + absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED + break; + default: + throw new RuntimeException("Invalid module index"); + } + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveTalon.getConfigurator().apply(driveConfig); + setDriveBrakeMode(true); + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnTalon.getConfigurator().apply(turnConfig); + setTurnBrakeMode(true); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + drivePosition = driveTalon.getPosition(); + drivePositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(driveTalon, driveTalon.getPosition()); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnPositionQueue = PhoenixOdometryThread.getInstance() + .registerSignal(turnTalon, turnTalon.getPosition()); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + Constants.Drive.Module.ODOMETRY_FREQUENCY, + drivePosition, + turnPosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent + ); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent + ); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / + DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / + DRIVE_GEAR_RATIO; + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] { driveCurrent.getValueAsDouble() }; + + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + turnAbsolutePosition.getValueAsDouble() + ).minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations( + turnPosition.getValueAsDouble() / TURN_GEAR_RATIO + ); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / + TURN_GEAR_RATIO; + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] { turnCurrent.getValueAsDouble() }; + + inputs.odometryTimestamps = timestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue + .stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue + .stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); + timestampQueue.clear(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = isTurnMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index f64d1e0..bf21e7a 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -35,108 +35,115 @@ * time synchronization. */ public class PhoenixOdometryThread extends Thread { - private final Lock signalsLock = - new ReentrantLock(); // Prevents conflicts when registering signals - private BaseStatusSignal[] signals = new BaseStatusSignal[0]; - private final List> queues = new ArrayList<>(); - private final List> timestampQueues = new ArrayList<>(); - private boolean isCANFD = false; - private static PhoenixOdometryThread instance = null; + private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + private boolean isCANFD = false; - public static PhoenixOdometryThread getInstance() { - if (instance == null) { - instance = new PhoenixOdometryThread(); - } - return instance; - } + private static PhoenixOdometryThread instance = null; - private PhoenixOdometryThread() { - setName("PhoenixOdometryThread"); - setDaemon(true); - } + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } - @Override - public void start() { - if (timestampQueues.size() > 0) { - super.start(); - } - } + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } - public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY); - signalsLock.lock(); - Drive.odometryLock.lock(); - try { - isCANFD = CANBus.isNetworkFD(device.getNetwork()); - BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; - System.arraycopy(signals, 0, newSignals, 0, signals.length); - newSignals[signals.length] = signal; - signals = newSignals; - queues.add(queue); - } finally { - signalsLock.unlock(); - Drive.odometryLock.unlock(); - } - return queue; - } + @Override + public void start() { + if (timestampQueues.size() > 0) { + super.start(); + } + } - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY + ); + signalsLock.lock(); + Drive.odometryLock.lock(); + try { + isCANFD = CANBus.isNetworkFD(device.getNetwork()); + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + } finally { + signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } - @Override - public void run() { - while (true) { - // Wait for updates from all signals - signalsLock.lock(); - try { - if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY, signals); - } else { - // "waitForAll" does not support blocking on multiple - // signals with a bus that is not CAN FD, regardless - // of Pro licensing. No reasoning for this behavior - // is provided by the documentation. - Thread.sleep( - (long) - (Constants.Drive.OdoThread.Phoenix.SLEEP_TIME - / Constants.Drive.Module.ODOMETRY_FREQUENCY)); - if (signals.length > 0) BaseStatusSignal.refreshAll(signals); - } - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - signalsLock.unlock(); - } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.Phoenix.QUE_CAPACITY + ); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - // Save new data to queues - Drive.odometryLock.lock(); - try { - double timestamp = Logger.getRealTimestamp() / 1e6; - double totalLatency = 0.0; - for (BaseStatusSignal signal : signals) { - totalLatency += signal.getTimestamp().getLatency(); - } - if (signals.length > 0) { - timestamp -= totalLatency / signals.length; - } - for (int i = 0; i < signals.length; i++) { - queues.get(i).offer(signals[i].getValueAsDouble()); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } finally { - Drive.odometryLock.unlock(); - } - } - } + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD) { + BaseStatusSignal.waitForAll( + 2.0 / Constants.Drive.Module.ODOMETRY_FREQUENCY, + signals + ); + } else { + // "waitForAll" does not support blocking on multiple + // signals with a bus that is not CAN FD, regardless + // of Pro licensing. No reasoning for this behavior + // is provided by the documentation. + Thread.sleep( + (long) (Constants.Drive.OdoThread.Phoenix.SLEEP_TIME / + Constants.Drive.Module.ODOMETRY_FREQUENCY) + ); + if (signals.length > 0) BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drive.odometryLock.lock(); + try { + double timestamp = Logger.getRealTimestamp() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : signals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (signals.length > 0) { + timestamp -= totalLatency / signals.length; + } + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index 68d2196..fea8279 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -30,80 +30,87 @@ * blocking thread. A Notifier thread is used to gather samples with consistent timing. */ public class SparkMaxOdometryThread { - private List> signals = new ArrayList<>(); - private List> queues = new ArrayList<>(); - private List> timestampQueues = new ArrayList<>(); - private final Notifier notifier; - private static SparkMaxOdometryThread instance = null; + private List> signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + private List> timestampQueues = new ArrayList<>(); - public static SparkMaxOdometryThread getInstance() { - if (instance == null) { - instance = new SparkMaxOdometryThread(); - } - return instance; - } + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; - private SparkMaxOdometryThread() { - notifier = new Notifier(this::periodic); - notifier.setName("SparkMaxOdometryThread"); - } + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } - public void start() { - if (timestampQueues.size() > 0) { - notifier.startPeriodic( - Constants.Drive.OdoThread.SparkMax.PERIOD / Constants.Drive.Module.ODOMETRY_FREQUENCY); - } - } + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + } - public Queue registerSignal(Supplier signal) { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY); - Drive.odometryLock.lock(); - try { - signals.add(signal); - queues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public void start() { + if (timestampQueues.size() > 0) { + notifier.startPeriodic( + Constants.Drive.OdoThread.SparkMax.PERIOD / + Constants.Drive.Module.ODOMETRY_FREQUENCY + ); + } + } - public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY); - Drive.odometryLock.lock(); - try { - timestampQueues.add(queue); - } finally { - Drive.odometryLock.unlock(); - } - return queue; - } + public Queue registerSignal(Supplier signal) { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY + ); + Drive.odometryLock.lock(); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } - private void periodic() { - Drive.odometryLock.lock(); - double timestamp = Logger.getRealTimestamp() / 1e6; - try { - double[] values = new double[signals.size()]; - boolean isValid = true; - for (int i = 0; i < signals.size(); i++) { - OptionalDouble value = signals.get(i).get(); - if (value.isPresent()) { - values[i] = value.getAsDouble(); - } else { - isValid = false; - break; - } - } - if (isValid) { - for (int i = 0; i < queues.size(); i++) { - queues.get(i).offer(values[i]); - } - for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); - } - } - } finally { - Drive.odometryLock.unlock(); - } - } + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>( + Constants.Drive.OdoThread.SparkMax.QUE_CAPACITY + ); + Drive.odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + private void periodic() { + Drive.odometryLock.lock(); + double timestamp = Logger.getRealTimestamp() / 1e6; + try { + double[] values = new double[signals.size()]; + boolean isValid = true; + for (int i = 0; i < signals.size(); i++) { + OptionalDouble value = signals.get(i).get(); + if (value.isPresent()) { + values[i] = value.getAsDouble(); + } else { + isValid = false; + break; + } + } + if (isValid) { + for (int i = 0; i < queues.size(); i++) { + queues.get(i).offer(values[i]); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } + } finally { + Drive.odometryLock.unlock(); + } + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index d7e1a57..0bae12c 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -9,52 +9,59 @@ import org.littletonrobotics.junction.Logger; public class Intake extends Subsystem { - IntakeIO io; - IntakeIOInputsAutoLogged inputs; - - public Intake(IntakeIO io) { - super("Intake", IntakeStates.OFF); - this.io = io; - inputs = new IntakeIOInputsAutoLogged(); - - // Configure PIDs here - switch (Constants.currentMode) { - case REAL: - io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); - break; - case REPLAY: - io.configurePID(new PIDConstants(0, 0, 0), new PIDConstants(0, 0, 0)); - break; - case SIM: - io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); - break; - default: - break; - } - } - - protected void runState() { - io.setSetpoints( - getState().getPivotSetPoint(), getState().getMotorSetPoint(), getState().getUsingPID()); - if (getState() == IntakeStates.INTAKING) { - NoteSimulator.attachToShooter(); - } - } - - public void stop() { - io.stop(); - } - - @Override - public void periodic() { - super.periodic(); - - Logger.recordOutput( - "Intake/Intake Pose", - new Pose3d( - Constants.Intake.ZEROED_PIVOT_TRANSLATION, new Rotation3d(0, io.getPosition(), 0))); - - Logger.processInputs("Intake", inputs); - io.updateInputs(inputs); - } + + IntakeIO io; + IntakeIOInputsAutoLogged inputs; + + public Intake(IntakeIO io) { + super("Intake", IntakeStates.OFF); + this.io = io; + inputs = new IntakeIOInputsAutoLogged(); + + // Configure PIDs here + switch (Constants.currentMode) { + case REAL: + io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); + break; + case REPLAY: + io.configurePID(new PIDConstants(0, 0, 0), new PIDConstants(0, 0, 0)); + break; + case SIM: + io.configurePID(Constants.Intake.REAL_OUT_PID, Constants.Intake.REAL_IN_PID); + break; + default: + break; + } + } + + protected void runState() { + io.setSetpoints( + getState().getPivotSetPoint(), + getState().getMotorSetPoint(), + getState().getUsingPID() + ); + if (getState() == IntakeStates.INTAKING) { + NoteSimulator.attachToShooter(); + } + } + + public void stop() { + io.stop(); + } + + @Override + public void periodic() { + super.periodic(); + + Logger.recordOutput( + "Intake/Intake Pose", + new Pose3d( + Constants.Intake.ZEROED_PIVOT_TRANSLATION, + new Rotation3d(0, io.getPosition(), 0) + ) + ); + + Logger.processInputs("Intake", inputs); + io.updateInputs(inputs); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 16a6117..119e7ca 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -5,32 +5,36 @@ import org.littletonrobotics.junction.AutoLog; public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { - public Pose3d position; + @AutoLog + public static class IntakeIOInputs { - public double wheelSpeed; - public double wheelAppliedVoltage; - public double wheelSpeedpoint; + public Pose3d position; - public double pivotPosition; - public double pivotAppliedVoltage; - public double pivotSetpoint; - public double pivotSetpointError; + public double wheelSpeed; + public double wheelAppliedVoltage; + public double wheelSpeedpoint; - public boolean usingInPID; - } + public double pivotPosition; + public double pivotAppliedVoltage; + public double pivotSetpoint; + public double pivotSetpointError; - public default void updateInputs(IntakeIOInputs inputs) {} + public boolean usingInPID; + } - public default void setSetpoints( - double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useIn) {} + public default void updateInputs(IntakeIOInputs inputs) {} - public default void stop() {} + public default void setSetpoints( + double pivotMotorSetpoint, + double intakeMotorSetpoint, + boolean useIn + ) {} - public default double getPosition() { - return 0.0; - } + public default void stop() {} - public default void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) {} + public default double getPosition() { + return 0.0; + } + + public default void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) {} } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 9c375fa..fcf00f5 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -8,92 +8,98 @@ import frc.robot.Constants; public class IntakeIOSim implements IntakeIO { - SingleJointedArmSim pivotSimModel; - DCMotorSim spinnerWheelSim; - - PIDController outPivotController; - PIDController inPIDController; - - PIDController pivotController; - - double wheelAppliedVoltage; - double pivotAppliedVoltage; - - double wheelSpeedpoint; - double pivotSetpoint; - - boolean usingInPID; - - public IntakeIOSim() { - pivotSimModel = - new SingleJointedArmSim( - DCMotor.getKrakenX60(Constants.Intake.NUM_PIVOT_MOTORS), - Constants.Intake.PIVOT_GEARING, // gearing - Constants.Intake.PIVOT_MOI, // MOI - Constants.Intake.PIVOT_LENGTH_METERS, // arm length - 0, // min angle -- hard stop - Constants.Intake.MAX_PIVOT_POSITION, // max angle -- floor - false, - 0); - - spinnerWheelSim = - new DCMotorSim( - DCMotor.getFalcon500(Constants.Intake.NUM_SPINNER_MOTORS), - Constants.Intake.SPINNER_GEARING, - Constants.Intake.SPINNER_MOI); - - outPivotController = new PIDController(0, 0, 0); - inPIDController = new PIDController(0, 0, 0); - } - - public void setSetpoints( - double pivotMotorSetPoint, double intakeMotorSetpoint, boolean useInPID) { - usingInPID = useInPID; - - if (useInPID) pivotController = inPIDController; - else pivotController = outPivotController; - - pivotAppliedVoltage = - pivotController.calculate(pivotSimModel.getAngleRads(), pivotMotorSetPoint); - - wheelAppliedVoltage = intakeMotorSetpoint; - - pivotSimModel.setInputVoltage(pivotAppliedVoltage); - spinnerWheelSim.setInputVoltage(intakeMotorSetpoint); - - wheelSpeedpoint = intakeMotorSetpoint; - pivotSetpoint = pivotMotorSetPoint; - } - - public void updateInputs(IntakeIOInputs inputs) { - inputs.wheelSpeed = spinnerWheelSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.wheelAppliedVoltage = wheelAppliedVoltage; - inputs.wheelSpeedpoint = wheelSpeedpoint; - - inputs.pivotPosition = pivotSimModel.getAngleRads(); - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotSetpointError = Math.abs(pivotSimModel.getAngleRads() - pivotSetpoint); - - inputs.usingInPID = usingInPID; - - pivotSimModel.update(Constants.SIM_UPDATE_TIME); - spinnerWheelSim.update(Constants.SIM_UPDATE_TIME); - } - - public double getPosition() { - return pivotSimModel.getAngleRads(); - } - - public void stop() { - wheelAppliedVoltage = 0.0; - pivotAppliedVoltage = 0.0; - pivotSimModel.setInputVoltage(0); - spinnerWheelSim.setInputVoltage(0); - } - - public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { - outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); - inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); - } + + SingleJointedArmSim pivotSimModel; + DCMotorSim spinnerWheelSim; + + PIDController outPivotController; + PIDController inPIDController; + + PIDController pivotController; + + double wheelAppliedVoltage; + double pivotAppliedVoltage; + + double wheelSpeedpoint; + double pivotSetpoint; + + boolean usingInPID; + + public IntakeIOSim() { + pivotSimModel = new SingleJointedArmSim( + DCMotor.getKrakenX60(Constants.Intake.NUM_PIVOT_MOTORS), + Constants.Intake.PIVOT_GEARING, // gearing + Constants.Intake.PIVOT_MOI, // MOI + Constants.Intake.PIVOT_LENGTH_METERS, // arm length + 0, // min angle -- hard stop + Constants.Intake.MAX_PIVOT_POSITION, // max angle -- floor + false, + 0 + ); + + spinnerWheelSim = new DCMotorSim( + DCMotor.getFalcon500(Constants.Intake.NUM_SPINNER_MOTORS), + Constants.Intake.SPINNER_GEARING, + Constants.Intake.SPINNER_MOI + ); + + outPivotController = new PIDController(0, 0, 0); + inPIDController = new PIDController(0, 0, 0); + } + + public void setSetpoints( + double pivotMotorSetPoint, + double intakeMotorSetpoint, + boolean useInPID + ) { + usingInPID = useInPID; + + if (useInPID) pivotController = inPIDController; + else pivotController = outPivotController; + + pivotAppliedVoltage = pivotController.calculate( + pivotSimModel.getAngleRads(), + pivotMotorSetPoint + ); + + wheelAppliedVoltage = intakeMotorSetpoint; + + pivotSimModel.setInputVoltage(pivotAppliedVoltage); + spinnerWheelSim.setInputVoltage(intakeMotorSetpoint); + + wheelSpeedpoint = intakeMotorSetpoint; + pivotSetpoint = pivotMotorSetPoint; + } + + public void updateInputs(IntakeIOInputs inputs) { + inputs.wheelSpeed = spinnerWheelSim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.wheelAppliedVoltage = wheelAppliedVoltage; + inputs.wheelSpeedpoint = wheelSpeedpoint; + + inputs.pivotPosition = pivotSimModel.getAngleRads(); + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotSetpointError = Math.abs(pivotSimModel.getAngleRads() - pivotSetpoint); + + inputs.usingInPID = usingInPID; + + pivotSimModel.update(Constants.SIM_UPDATE_TIME); + spinnerWheelSim.update(Constants.SIM_UPDATE_TIME); + } + + public double getPosition() { + return pivotSimModel.getAngleRads(); + } + + public void stop() { + wheelAppliedVoltage = 0.0; + pivotAppliedVoltage = 0.0; + pivotSimModel.setInputVoltage(0); + spinnerWheelSim.setInputVoltage(0); + } + + public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { + outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); + inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index f80ddc2..f09ec0e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -9,81 +9,88 @@ import frc.robot.Constants; public class IntakeIOSparkMax implements IntakeIO { - private CANSparkMax pivotMotor; - public TalonFX intakeMotor; - private RelativeEncoder pivotEncoder; - PIDController outPivotController; - PIDController inPIDController; + private CANSparkMax pivotMotor; + public TalonFX intakeMotor; + private RelativeEncoder pivotEncoder; - PIDController pivotController; + PIDController outPivotController; + PIDController inPIDController; - double pivotMotorSetpoint = 0.0; - double intakeMotorSetpoint = 0.0; + PIDController pivotController; - double wheelAppliedVoltage; - double pivotAppliedVoltage; + double pivotMotorSetpoint = 0.0; + double intakeMotorSetpoint = 0.0; - double wheelSpeedpoint; - double pivotSetpoint; + double wheelAppliedVoltage; + double pivotAppliedVoltage; - boolean usingInPID; + double wheelSpeedpoint; + double pivotSetpoint; - public IntakeIOSparkMax() { - intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); - pivotMotor = new CANSparkMax(Constants.Intake.PIVOT_ID, MotorType.kBrushless); - pivotEncoder = pivotMotor.getEncoder(); + boolean usingInPID; - outPivotController = new PIDController(0, 0, 0); - inPIDController = new PIDController(0, 0, 0); + public IntakeIOSparkMax() { + intakeMotor = new TalonFX(Constants.Intake.SPINNER_ID); + pivotMotor = new CANSparkMax(Constants.Intake.PIVOT_ID, MotorType.kBrushless); + pivotEncoder = pivotMotor.getEncoder(); - pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); - pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); - } + outPivotController = new PIDController(0, 0, 0); + inPIDController = new PIDController(0, 0, 0); - public void setSetpoints( - double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useInPID) { - usingInPID = useInPID; + pivotEncoder.setPositionConversionFactor(Constants.RADIAN_CF); + pivotEncoder.setVelocityConversionFactor(Constants.RADIAN_CF); + } - if (useInPID) pivotController = inPIDController; - else pivotController = outPivotController; + public void setSetpoints( + double pivotMotorSetpoint, + double intakeMotorSetpoint, + boolean useInPID + ) { + usingInPID = useInPID; - pivotAppliedVoltage = pivotController.calculate(pivotEncoder.getPosition(), pivotMotorSetpoint); - wheelAppliedVoltage = intakeMotorSetpoint; + if (useInPID) pivotController = inPIDController; + else pivotController = outPivotController; - pivotMotor.setVoltage(pivotAppliedVoltage); - intakeMotor.setVoltage(wheelAppliedVoltage); + pivotAppliedVoltage = pivotController.calculate( + pivotEncoder.getPosition(), + pivotMotorSetpoint + ); + wheelAppliedVoltage = intakeMotorSetpoint; - wheelSpeedpoint = intakeMotorSetpoint; - pivotSetpoint = pivotMotorSetpoint; - } + pivotMotor.setVoltage(pivotAppliedVoltage); + intakeMotor.setVoltage(wheelAppliedVoltage); - public void updateInputs(IntakeIOInputs inputs) { - inputs.wheelSpeed = intakeMotor.getVelocity().getValueAsDouble(); - inputs.wheelAppliedVoltage = wheelAppliedVoltage; - inputs.wheelSpeedpoint = wheelSpeedpoint; + wheelSpeedpoint = intakeMotorSetpoint; + pivotSetpoint = pivotMotorSetpoint; + } - inputs.pivotPosition = pivotEncoder.getPosition(); - inputs.pivotAppliedVoltage = pivotAppliedVoltage; - inputs.pivotSetpoint = pivotSetpoint; - inputs.pivotSetpointError = Math.abs(pivotEncoder.getPosition() - pivotSetpoint); + public void updateInputs(IntakeIOInputs inputs) { + inputs.wheelSpeed = intakeMotor.getVelocity().getValueAsDouble(); + inputs.wheelAppliedVoltage = wheelAppliedVoltage; + inputs.wheelSpeedpoint = wheelSpeedpoint; - inputs.usingInPID = usingInPID; - } + inputs.pivotPosition = pivotEncoder.getPosition(); + inputs.pivotAppliedVoltage = pivotAppliedVoltage; + inputs.pivotSetpoint = pivotSetpoint; + inputs.pivotSetpointError = Math.abs(pivotEncoder.getPosition() - pivotSetpoint); - public double getPosition() { - return pivotEncoder.getPosition(); - } + inputs.usingInPID = usingInPID; + } - public void stop() { - wheelAppliedVoltage = 0.0; - pivotAppliedVoltage = 0.0; - pivotMotor.stopMotor(); - intakeMotor.stopMotor(); - } + public double getPosition() { + return pivotEncoder.getPosition(); + } - public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { - outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); - inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); - } + public void stop() { + wheelAppliedVoltage = 0.0; + pivotAppliedVoltage = 0.0; + pivotMotor.stopMotor(); + intakeMotor.stopMotor(); + } + + public void configurePID(PIDConstants outPIDConst, PIDConstants inPIPidConst) { + outPivotController.setPID(outPIDConst.kP, outPIDConst.kI, outPIDConst.kD); + inPIDController.setPID(inPIPidConst.kP, inPIPidConst.kI, inPIPidConst.kD); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeStates.java b/src/main/java/frc/robot/subsystems/intake/IntakeStates.java index 9c2a950..0537920 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeStates.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeStates.java @@ -4,37 +4,41 @@ import frc.robot.subsystems.*; public enum IntakeStates implements SubsystemStates { - OFF("Off", Constants.Intake.IN, Constants.Intake.OFF, true), - INTAKING("Intaking", Constants.Intake.DOWN, Constants.Intake.ON, false), - FEEDING("Feeding", Constants.Intake.IN, Constants.Intake.REVERSE, true), - OUTTAKING("Outtaking", Constants.Intake.DOWN, Constants.Intake.REVERSE, false); - - private String stateString; - private double pivotMotorSetpoint; - private double intakeMotorSetpoint; - private boolean useIn; - - IntakeStates( - String stateString, double pivotMotorSetpoint, double intakeMotorSetpoint, boolean useIn) { - this.stateString = stateString; - this.pivotMotorSetpoint = pivotMotorSetpoint; - this.intakeMotorSetpoint = intakeMotorSetpoint; - this.useIn = useIn; - } - - public String getStateString() { - return this.stateString; - } - - public double getPivotSetPoint() { - return pivotMotorSetpoint; - } - - public double getMotorSetPoint() { - return intakeMotorSetpoint; - } - - public boolean getUsingPID() { - return useIn; - } + OFF("Off", Constants.Intake.IN, Constants.Intake.OFF, true), + INTAKING("Intaking", Constants.Intake.DOWN, Constants.Intake.ON, false), + FEEDING("Feeding", Constants.Intake.IN, Constants.Intake.REVERSE, true), + OUTTAKING("Outtaking", Constants.Intake.DOWN, Constants.Intake.REVERSE, false); + + private String stateString; + private double pivotMotorSetpoint; + private double intakeMotorSetpoint; + private boolean useIn; + + IntakeStates( + String stateString, + double pivotMotorSetpoint, + double intakeMotorSetpoint, + boolean useIn + ) { + this.stateString = stateString; + this.pivotMotorSetpoint = pivotMotorSetpoint; + this.intakeMotorSetpoint = intakeMotorSetpoint; + this.useIn = useIn; + } + + public String getStateString() { + return this.stateString; + } + + public double getPivotSetPoint() { + return pivotMotorSetpoint; + } + + public double getMotorSetPoint() { + return intakeMotorSetpoint; + } + + public boolean getUsingPID() { + return useIn; + } } diff --git a/src/main/java/frc/robot/subsystems/manager/Manager.java b/src/main/java/frc/robot/subsystems/manager/Manager.java index 51ee8e7..abb210f 100644 --- a/src/main/java/frc/robot/subsystems/manager/Manager.java +++ b/src/main/java/frc/robot/subsystems/manager/Manager.java @@ -14,142 +14,141 @@ import frc.robot.util.NoteSimulator; public class Manager extends Subsystem { - Intake intakeSubsystem; - AmpBar ampBarSubsystem; - Shooter shooterSubsystem; - Drive driveSubsystem; - - public Manager() { - super("Manager", ManagerStates.IDLE); - - NoteSimulator.setDrive(driveSubsystem); - - switch (Constants.currentMode) { - case REAL: - intakeSubsystem = new Intake(new IntakeIOSparkMax()); - ampBarSubsystem = new AmpBar(new AmpBarIOReal()); - shooterSubsystem = new Shooter(new ShooterIOTalonFX()); - driveSubsystem = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOHybrid(0), - new ModuleIOHybrid(1), - new ModuleIOHybrid(2), - new ModuleIOHybrid(3)); - break; - case REPLAY: - intakeSubsystem = new Intake(new IntakeIO() {}); - ampBarSubsystem = new AmpBar(new AmpBarIO() {}); - shooterSubsystem = new Shooter(new ShooterIO() {}); - driveSubsystem = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - break; - case SIM: - intakeSubsystem = new Intake(new IntakeIOSim()); - ampBarSubsystem = new AmpBar(new AmpBarIOSim()); - shooterSubsystem = new Shooter(new ShooterIOSim()); - driveSubsystem = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - break; - default: - break; - } - - NoteSimulator.setDrive(driveSubsystem); - - // State Transitions (Nothing Automatic YET) - - /* Generally each action has a specific button, in intermediary states X will return to idle and you press + + Intake intakeSubsystem; + AmpBar ampBarSubsystem; + Shooter shooterSubsystem; + Drive driveSubsystem; + + public Manager() { + super("Manager", ManagerStates.IDLE); + NoteSimulator.setDrive(driveSubsystem); + + switch (Constants.currentMode) { + case REAL: + intakeSubsystem = new Intake(new IntakeIOSparkMax()); + ampBarSubsystem = new AmpBar(new AmpBarIOReal()); + shooterSubsystem = new Shooter(new ShooterIOTalonFX()); + driveSubsystem = new Drive( + new GyroIOPigeon2(false), + new ModuleIOHybrid(0), + new ModuleIOHybrid(1), + new ModuleIOHybrid(2), + new ModuleIOHybrid(3) + ); + break; + case REPLAY: + intakeSubsystem = new Intake(new IntakeIO() {}); + ampBarSubsystem = new AmpBar(new AmpBarIO() {}); + shooterSubsystem = new Shooter(new ShooterIO() {}); + driveSubsystem = new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {} + ); + break; + case SIM: + intakeSubsystem = new Intake(new IntakeIOSim()); + ampBarSubsystem = new AmpBar(new AmpBarIOSim()); + shooterSubsystem = new Shooter(new ShooterIOSim()); + driveSubsystem = new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim() + ); + break; + default: + break; + } + + NoteSimulator.setDrive(driveSubsystem); + + // State Transitions (Nothing Automatic YET) + + /* Generally each action has a specific button, in intermediary states X will return to idle and you press the specific button to go through the states. Right now you can go through states without them being finished which should not be the case. Some state transitions that can be automated are also not automated. Most of the functions required for the TODOs are already build into the IOs!*/ - // TODO: Automatically return to idle after scoring amp (using a timer) - // TODO: Automatically current sense notes to return to idle after intaking - // TODO: Automatically go to the shooting state after spinning up if the shooting state is - // entered from main controller input - - // Intaking (B) - addTrigger( - ManagerStates.IDLE, ManagerStates.INTAKING, () -> Constants.controller.getBButtonPressed()); - addTrigger( - ManagerStates.INTAKING, ManagerStates.IDLE, () -> Constants.controller.getBButtonPressed()); - - // Amping (Y) - addTrigger( - ManagerStates.IDLE, ManagerStates.FEED_AMP, () -> Constants.controller.getYButtonPressed()); - addTrigger( - ManagerStates.FEED_AMP, - ManagerStates.SCORE_AMP, - () -> Constants.controller.getYButtonPressed()); - addTrigger( - ManagerStates.SCORE_AMP, - ManagerStates.IDLE, - () -> Constants.controller.getYButtonPressed()); - addTrigger( - ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> Constants.controller.getXButtonPressed()); - - // Shooting (A) - addTrigger( - ManagerStates.IDLE, - ManagerStates.SPINNING_UP, - () -> Constants.controller.getAButtonPressed()); - addTrigger( - ManagerStates.IDLE, - ManagerStates.SPINNING_UP, - () -> Constants.operatorController.getAButtonPressed()); - addTrigger( - ManagerStates.SPINNING_UP, - ManagerStates.IDLE, - () -> Constants.controller.getXButtonPressed()); - addTrigger( - ManagerStates.SPINNING_UP, - ManagerStates.SHOOTING, - () -> Constants.controller.getAButtonPressed()); - addTrigger( - ManagerStates.SHOOTING, ManagerStates.IDLE, () -> Constants.controller.getAButtonPressed()); - } - - @Override - public void periodic() { - super.periodic(); - - intakeSubsystem.setState(getState().getIntakeState()); - ampBarSubsystem.setState(getState().getAmpBarState()); - shooterSubsystem.setState(getState().getShooterState()); - shooterSubsystem.setManagerState(getState()); - - intakeSubsystem.periodic(); - ampBarSubsystem.periodic(); - shooterSubsystem.periodic(); - driveSubsystem.periodic(); - - // Cancel all actions regardless of whats happening - if (Constants.operatorController.getXButtonPressed()) { - setState(ManagerStates.IDLE); - } - } - - public void stop() { - intakeSubsystem.stop(); - ampBarSubsystem.stop(); - shooterSubsystem.stop(); - driveSubsystem.stop(); - } - - @Override - protected void runState() { - NoteSimulator.update(); - NoteSimulator.logNoteInfo(); - } + // TODO: Automatically return to idle after scoring amp (using a timer) + // TODO: Automatically current sense notes to return to idle after intaking + // TODO: Automatically go to the shooting state after spinning up if the shooting state is + // entered from main controller input + + // Intaking (B) + addTrigger(ManagerStates.IDLE, ManagerStates.INTAKING, () -> + Constants.controller.getBButtonPressed() + ); + addTrigger(ManagerStates.INTAKING, ManagerStates.IDLE, () -> + Constants.controller.getBButtonPressed() + ); + + // Amping (Y) + addTrigger(ManagerStates.IDLE, ManagerStates.FEED_AMP, () -> + Constants.controller.getYButtonPressed() + ); + addTrigger(ManagerStates.FEED_AMP, ManagerStates.SCORE_AMP, () -> + Constants.controller.getYButtonPressed() + ); + addTrigger(ManagerStates.SCORE_AMP, ManagerStates.IDLE, () -> + Constants.controller.getYButtonPressed() + ); + addTrigger(ManagerStates.FEED_AMP, ManagerStates.IDLE, () -> + Constants.controller.getXButtonPressed() + ); + + // Shooting (A) + addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> + Constants.controller.getAButtonPressed() + ); + addTrigger(ManagerStates.IDLE, ManagerStates.SPINNING_UP, () -> + Constants.operatorController.getAButtonPressed() + ); + addTrigger(ManagerStates.SPINNING_UP, ManagerStates.IDLE, () -> + Constants.controller.getXButtonPressed() + ); + addTrigger(ManagerStates.SPINNING_UP, ManagerStates.SHOOTING, () -> + Constants.controller.getAButtonPressed() + ); + addTrigger(ManagerStates.SHOOTING, ManagerStates.IDLE, () -> + Constants.controller.getAButtonPressed() + ); + } + + @Override + public void periodic() { + super.periodic(); + + intakeSubsystem.setState(getState().getIntakeState()); + ampBarSubsystem.setState(getState().getAmpBarState()); + shooterSubsystem.setState(getState().getShooterState()); + shooterSubsystem.setManagerState(getState()); + + intakeSubsystem.periodic(); + ampBarSubsystem.periodic(); + shooterSubsystem.periodic(); + driveSubsystem.periodic(); + + // Cancel all actions regardless of whats happening + if (Constants.operatorController.getXButtonPressed()) { + setState(ManagerStates.IDLE); + } + } + + public void stop() { + intakeSubsystem.stop(); + ampBarSubsystem.stop(); + shooterSubsystem.stop(); + driveSubsystem.stop(); + } + + @Override + protected void runState() { + NoteSimulator.update(); + NoteSimulator.logNoteInfo(); + } } diff --git a/src/main/java/frc/robot/subsystems/manager/ManagerStates.java b/src/main/java/frc/robot/subsystems/manager/ManagerStates.java index f85b0f2..12a5114 100644 --- a/src/main/java/frc/robot/subsystems/manager/ManagerStates.java +++ b/src/main/java/frc/robot/subsystems/manager/ManagerStates.java @@ -6,44 +6,49 @@ import frc.robot.subsystems.shooter.ShooterStates; public enum ManagerStates implements SubsystemStates { - IDLE("Idle", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.OFF), - INTAKING("Intaking", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.OFF), - SPINNING_UP("Spinning Up", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.SHOOTING), - SPINNING_AND_INTAKING( - "Spin and Intake", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.SHOOTING), - SHOOTING("Shooting", IntakeStates.FEEDING, AmpBarStates.OFF, ShooterStates.SHOOTING), - FEED_AMP("Feed Amp", IntakeStates.FEEDING, AmpBarStates.FEEDING, ShooterStates.PASSING_AMP), - SCORE_AMP("Score Amp", IntakeStates.OFF, AmpBarStates.SHOOTING, ShooterStates.OFF); - - String stateString; - IntakeStates intakeState; - AmpBarStates ampBarState; - ShooterStates shooterState; - - ManagerStates( - String stateString, - IntakeStates intakeState, - AmpBarStates ampBarState, - ShooterStates shooterState) { - this.stateString = stateString; - this.intakeState = intakeState; - this.ampBarState = ampBarState; - this.shooterState = shooterState; - } - - public String getStateString() { - return this.stateString; - } - - public IntakeStates getIntakeState() { - return intakeState; - } - - public AmpBarStates getAmpBarState() { - return ampBarState; - } - - public ShooterStates getShooterState() { - return shooterState; - } + IDLE("Idle", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.OFF), + INTAKING("Intaking", IntakeStates.INTAKING, AmpBarStates.OFF, ShooterStates.OFF), + SPINNING_UP("Spinning Up", IntakeStates.OFF, AmpBarStates.OFF, ShooterStates.SHOOTING), + SPINNING_AND_INTAKING( + "Spin and Intake", + IntakeStates.INTAKING, + AmpBarStates.OFF, + ShooterStates.SHOOTING + ), + SHOOTING("Shooting", IntakeStates.FEEDING, AmpBarStates.OFF, ShooterStates.SHOOTING), + FEED_AMP("Feed Amp", IntakeStates.FEEDING, AmpBarStates.FEEDING, ShooterStates.PASSING_AMP), + SCORE_AMP("Score Amp", IntakeStates.OFF, AmpBarStates.SHOOTING, ShooterStates.OFF); + + String stateString; + IntakeStates intakeState; + AmpBarStates ampBarState; + ShooterStates shooterState; + + ManagerStates( + String stateString, + IntakeStates intakeState, + AmpBarStates ampBarState, + ShooterStates shooterState + ) { + this.stateString = stateString; + this.intakeState = intakeState; + this.ampBarState = ampBarState; + this.shooterState = shooterState; + } + + public String getStateString() { + return this.stateString; + } + + public IntakeStates getIntakeState() { + return intakeState; + } + + public AmpBarStates getAmpBarState() { + return ampBarState; + } + + public ShooterStates getShooterState() { + return shooterState; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a19f2f7..77cb7c2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -8,62 +8,65 @@ public class Shooter extends Subsystem { - private ShooterIO io; - private ManagerStates managerState; - ShooterIOInputsAutoLogged inputs; + private ShooterIO io; + private ManagerStates managerState; + ShooterIOInputsAutoLogged inputs; - public Shooter(ShooterIO io) { - super("Shooter", ShooterStates.OFF); - this.io = io; - inputs = new ShooterIOInputsAutoLogged(); + public Shooter(ShooterIO io) { + super("Shooter", ShooterStates.OFF); + this.io = io; + inputs = new ShooterIOInputsAutoLogged(); - switch (Constants.currentMode) { - case REAL: - io.configurePID( - Constants.Shooter.REAL_PID.kP, - Constants.Shooter.REAL_PID.kI, - Constants.Shooter.REAL_PID.kD); - break; - case REPLAY: - io.configurePID(0.0, 0.0, 0.0); - break; - case SIM: - io.configurePID( - Constants.Shooter.SIM_PID.kP, - Constants.Shooter.SIM_PID.kI, - Constants.Shooter.SIM_PID.kD); - break; - default: - break; - } - } + switch (Constants.currentMode) { + case REAL: + io.configurePID( + Constants.Shooter.REAL_PID.kP, + Constants.Shooter.REAL_PID.kI, + Constants.Shooter.REAL_PID.kD + ); + break; + case REPLAY: + io.configurePID(0.0, 0.0, 0.0); + break; + case SIM: + io.configurePID( + Constants.Shooter.SIM_PID.kP, + Constants.Shooter.SIM_PID.kI, + Constants.Shooter.SIM_PID.kD + ); + break; + default: + break; + } + } - @Override - protected void runState() { - io.setSpeed(getState().getMotorSpeedpoint()); + @Override + protected void runState() { + io.setSpeed(getState().getMotorSpeedpoint()); - if (managerState == ManagerStates.SHOOTING) { - NoteSimulator.launch( - io.getAverageSpeed() * Constants.Shooter.CIRCUMFRENCE_OF_SHOOTER_SPINNER); - } - } + if (managerState == ManagerStates.SHOOTING) { + NoteSimulator.launch( + io.getAverageSpeed() * Constants.Shooter.CIRCUMFRENCE_OF_SHOOTER_SPINNER + ); + } + } - public boolean nearSpeedPoint() { - return io.nearSpeedPoint(); - } + public boolean nearSpeedPoint() { + return io.nearSpeedPoint(); + } - public void stop() { - io.stop(); - } + public void stop() { + io.stop(); + } - @Override - public void periodic() { - super.periodic(); - io.updateInputs(inputs); - Logger.processInputs("Shooter", inputs); - } + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + Logger.processInputs("Shooter", inputs); + } - public void setManagerState(ManagerStates state) { - managerState = state; - } + public void setManagerState(ManagerStates state) { + managerState = state; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index c9c987d..f396916 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -3,29 +3,29 @@ import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { + @AutoLog + public static class ShooterIOInputs { - @AutoLog - public static class ShooterIOInputs { - public double rightShooterSpeed = 0.0; - public double leftShooterSpeed = 0.0; - public double leftShooterAppliedVolts = 0.0; - public double rightShooterAppliedVolts = 0.0; - public double shooterSpeedPoint = 0.0; - } + public double rightShooterSpeed = 0.0; + public double leftShooterSpeed = 0.0; + public double leftShooterAppliedVolts = 0.0; + public double rightShooterAppliedVolts = 0.0; + public double shooterSpeedPoint = 0.0; + } - public default void updateInputs(ShooterIOInputs inputs) {} + public default void updateInputs(ShooterIOInputs inputs) {} - public default void setSpeed(double rps) {} + public default void setSpeed(double rps) {} - public default void stop() {} + public default void stop() {} - public default void configurePID(double kP, double kI, double kD) {} + public default void configurePID(double kP, double kI, double kD) {} - public default boolean nearSpeedPoint() { - return false; - } + public default boolean nearSpeedPoint() { + return false; + } - public default double getAverageSpeed() { - return 0.0; - } + public default double getAverageSpeed() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index d39d390..2f9690d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -6,58 +6,61 @@ import frc.robot.Constants; public class ShooterIOSim implements ShooterIO { - FlywheelSim sim; - PIDController pid; - private double speedPoint; - private double appliedVolts; - - public ShooterIOSim() { - sim = - new FlywheelSim( - DCMotor.getFalcon500(Constants.Shooter.NUM_MOTORS), - Constants.Shooter.SHOOTER_GEARING, - Constants.Shooter.SHOOTER_MOI); - pid = new PIDController(0.0, 0.0, 0.0); - speedPoint = 0.0; - } - - @Override - public void updateInputs(ShooterIOInputs inputs) { - sim.update(Constants.SIM_UPDATE_TIME); - - inputs.leftShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.rightShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - inputs.shooterSpeedPoint = speedPoint; - inputs.leftShooterAppliedVolts = appliedVolts; - inputs.rightShooterAppliedVolts = appliedVolts; - } - - @Override - public void stop() { - appliedVolts = 0; - sim.setInputVoltage(appliedVolts); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } - - @Override - public void setSpeed(double rps) { - speedPoint = rps; - appliedVolts = pid.calculate(sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF, rps); - sim.setInputVoltage(appliedVolts); - } - - @Override - public boolean nearSpeedPoint() { - return Math.abs((sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF) - speedPoint) - > Constants.Shooter.ERROR_OF_MARGIN; - } - - @Override - public double getAverageSpeed() { - return sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; - } + + FlywheelSim sim; + PIDController pid; + private double speedPoint; + private double appliedVolts; + + public ShooterIOSim() { + sim = new FlywheelSim( + DCMotor.getFalcon500(Constants.Shooter.NUM_MOTORS), + Constants.Shooter.SHOOTER_GEARING, + Constants.Shooter.SHOOTER_MOI + ); + pid = new PIDController(0.0, 0.0, 0.0); + speedPoint = 0.0; + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + sim.update(Constants.SIM_UPDATE_TIME); + + inputs.leftShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.rightShooterSpeed = sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + inputs.shooterSpeedPoint = speedPoint; + inputs.leftShooterAppliedVolts = appliedVolts; + inputs.rightShooterAppliedVolts = appliedVolts; + } + + @Override + public void stop() { + appliedVolts = 0; + sim.setInputVoltage(appliedVolts); + } + + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } + + @Override + public void setSpeed(double rps) { + speedPoint = rps; + appliedVolts = pid.calculate(sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF, rps); + sim.setInputVoltage(appliedVolts); + } + + @Override + public boolean nearSpeedPoint() { + return ( + Math.abs((sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF) - speedPoint) > + Constants.Shooter.ERROR_OF_MARGIN + ); + } + + @Override + public double getAverageSpeed() { + return sim.getAngularVelocityRPM() / Constants.RPM_TO_RPS_CF; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index ae9bc81..a32bad8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -6,63 +6,71 @@ public class ShooterIOTalonFX implements ShooterIO { - private TalonFX leftMotor; - private TalonFX rightMotor; - private PIDController feedbackController; - private double speedPoint; - private double leftAppliedVolts; - private double rightAppliedVolts; + private TalonFX leftMotor; + private TalonFX rightMotor; + private PIDController feedbackController; + private double speedPoint; + private double leftAppliedVolts; + private double rightAppliedVolts; - public ShooterIOTalonFX() { - feedbackController = new PIDController(0, 0, 0); - leftMotor = new TalonFX(Constants.Shooter.LEFT_SHOOTER_ID); - rightMotor = new TalonFX(Constants.Shooter.RIGHT_SHOOTER_ID); - speedPoint = 0.0; - leftMotor.setInverted(false); - rightMotor.setInverted(true); - } + public ShooterIOTalonFX() { + feedbackController = new PIDController(0, 0, 0); + leftMotor = new TalonFX(Constants.Shooter.LEFT_SHOOTER_ID); + rightMotor = new TalonFX(Constants.Shooter.RIGHT_SHOOTER_ID); + speedPoint = 0.0; + leftMotor.setInverted(false); + rightMotor.setInverted(true); + } - public void updateInputs(ShooterIOInputs inputs) { - inputs.leftShooterAppliedVolts = leftAppliedVolts; - inputs.rightShooterAppliedVolts = rightAppliedVolts; - inputs.leftShooterSpeed = leftMotor.getVelocity().getValueAsDouble(); - inputs.rightShooterSpeed = rightMotor.getVelocity().getValueAsDouble(); - inputs.shooterSpeedPoint = speedPoint; - } + public void updateInputs(ShooterIOInputs inputs) { + inputs.leftShooterAppliedVolts = leftAppliedVolts; + inputs.rightShooterAppliedVolts = rightAppliedVolts; + inputs.leftShooterSpeed = leftMotor.getVelocity().getValueAsDouble(); + inputs.rightShooterSpeed = rightMotor.getVelocity().getValueAsDouble(); + inputs.shooterSpeedPoint = speedPoint; + } - public void setSpeed(double rps) { - speedPoint = rps; - leftAppliedVolts = - feedbackController.calculate(leftMotor.getRotorVelocity().getValueAsDouble(), rps); - rightAppliedVolts = - feedbackController.calculate(rightMotor.getRotorVelocity().getValueAsDouble(), rps); + public void setSpeed(double rps) { + speedPoint = rps; + leftAppliedVolts = feedbackController.calculate( + leftMotor.getRotorVelocity().getValueAsDouble(), + rps + ); + rightAppliedVolts = feedbackController.calculate( + rightMotor.getRotorVelocity().getValueAsDouble(), + rps + ); - leftMotor.setVoltage(leftAppliedVolts); - rightMotor.setVoltage(rightAppliedVolts); - } + leftMotor.setVoltage(leftAppliedVolts); + rightMotor.setVoltage(rightAppliedVolts); + } - public void stop() { - leftAppliedVolts = 0.0; - rightAppliedVolts = 0.0; - leftMotor.stopMotor(); - rightMotor.stopMotor(); - } + public void stop() { + leftAppliedVolts = 0.0; + rightAppliedVolts = 0.0; + leftMotor.stopMotor(); + rightMotor.stopMotor(); + } - public void configurePID(double kP, double kI, double kD) { - feedbackController.setPID(kP, kI, kD); - } + public void configurePID(double kP, double kI, double kD) { + feedbackController.setPID(kP, kI, kD); + } - public boolean nearSpeedPoint() { - return Math.abs(speedPoint - leftMotor.getVelocity().getValueAsDouble()) - < Constants.Shooter.ERROR_OF_MARGIN - && Math.abs(speedPoint - rightMotor.getVelocity().getValueAsDouble()) - < Constants.Shooter.ERROR_OF_MARGIN; - } + public boolean nearSpeedPoint() { + return ( + Math.abs(speedPoint - leftMotor.getVelocity().getValueAsDouble()) < + Constants.Shooter.ERROR_OF_MARGIN && + Math.abs(speedPoint - rightMotor.getVelocity().getValueAsDouble()) < + Constants.Shooter.ERROR_OF_MARGIN + ); + } - @Override - public double getAverageSpeed() { - return (leftMotor.getVelocity().getValueAsDouble() - + rightMotor.getVelocity().getValueAsDouble()) - / Constants.AVG_TWO_ITEM_F; - } + @Override + public double getAverageSpeed() { + return ( + (leftMotor.getVelocity().getValueAsDouble() + + rightMotor.getVelocity().getValueAsDouble()) / + Constants.AVG_TWO_ITEM_F + ); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java b/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java index 4a775a0..17aceeb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterStates.java @@ -4,24 +4,24 @@ import frc.robot.subsystems.SubsystemStates; public enum ShooterStates implements SubsystemStates { - OFF(Constants.Shooter.OFF, "Shooter Off"), - SHOOTING(Constants.Shooter.SHOOTING, "Shooting Full"), - PASSING_AMP(Constants.Shooter.FEEDING_AMP, "Passing To Amp"); + OFF(Constants.Shooter.OFF, "Shooter Off"), + SHOOTING(Constants.Shooter.SHOOTING, "Shooting Full"), + PASSING_AMP(Constants.Shooter.FEEDING_AMP, "Passing To Amp"); - private double speedPoint; - private String stateString; + private double speedPoint; + private String stateString; - ShooterStates(double speedPoint, String stateString) { - this.speedPoint = speedPoint; - this.stateString = stateString; - } + ShooterStates(double speedPoint, String stateString) { + this.speedPoint = speedPoint; + this.stateString = stateString; + } - public double getMotorSpeedpoint() { - return speedPoint; - } + public double getMotorSpeedpoint() { + return speedPoint; + } - @Override - public String getStateString() { - return stateString; - } + @Override + public String getStateString() { + return stateString; + } } diff --git a/src/main/java/frc/robot/util/FFConstants.java b/src/main/java/frc/robot/util/FFConstants.java index c667c27..6ab4eb2 100644 --- a/src/main/java/frc/robot/util/FFConstants.java +++ b/src/main/java/frc/robot/util/FFConstants.java @@ -1,11 +1,12 @@ package frc.robot.util; public class FFConstants { - public double kS; - public double kV; - public FFConstants(double ks, double kv) { - this.kS = ks; - this.kV = kv; - } + public double kS; + public double kV; + + public FFConstants(double ks, double kv) { + this.kS = ks; + this.kV = kv; + } } diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java index 191b54f..8694cb9 100644 --- a/src/main/java/frc/robot/util/LocalADStarAK.java +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -19,135 +19,143 @@ // https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d public class LocalADStarAK implements Pathfinder { - private final ADStarIO io = new ADStarIO(); - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - @Override - public boolean isNewPathAvailable() { - if (!Logger.hasReplaySource()) { - io.updateIsNewPathAvailable(); - } - - Logger.processInputs("LocalADStarAK", io); - - return io.isNewPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - @Override - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - if (!Logger.hasReplaySource()) { - io.updateCurrentPathPoints(constraints, goalEndState); - } - - Logger.processInputs("LocalADStarAK", io); - - if (io.currentPathPoints.isEmpty()) { - return null; - } - - return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - @Override - public void setStartPosition(Translation2d startPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setStartPosition(startPosition); - } - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - @Override - public void setGoalPosition(Translation2d goalPosition) { - if (!Logger.hasReplaySource()) { - io.adStar.setGoalPosition(goalPosition); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path to properly avoid obstacles - */ - @Override - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - if (!Logger.hasReplaySource()) { - io.adStar.setDynamicObstacles(obs, currentRobotPos); - } - } - - private static class ADStarIO implements LoggableInputs { - public LocalADStar adStar = new LocalADStar(); - public boolean isNewPathAvailable = false; - public List currentPathPoints = Collections.emptyList(); - - @Override - public void toLog(LogTable table) { - table.put("IsNewPathAvailable", isNewPathAvailable); - - double[] pointsLogged = new double[currentPathPoints.size() * 2]; - int idx = 0; - for (PathPoint point : currentPathPoints) { - pointsLogged[idx] = point.position.getX(); - pointsLogged[idx + 1] = point.position.getY(); - idx += 2; - } - - table.put("CurrentPathPoints", pointsLogged); - } - - @Override - public void fromLog(LogTable table) { - isNewPathAvailable = table.get("IsNewPathAvailable", false); - - double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); - - List pathPoints = new ArrayList<>(); - for (int i = 0; i < pointsLogged.length; i += 2) { - pathPoints.add( - new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); - } - - currentPathPoints = pathPoints; - } - - public void updateIsNewPathAvailable() { - isNewPathAvailable = adStar.isNewPathAvailable(); - } - - public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { - PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); - - if (currentPath != null) { - currentPathPoints = currentPath.getAllPathPoints(); - } else { - currentPathPoints = Collections.emptyList(); - } - } - } + + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, + Translation2d currentRobotPos + ) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null) + ); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints( + PathConstraints constraints, + GoalEndState goalEndState + ) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } } diff --git a/src/main/java/frc/robot/util/NoteSimulator.java b/src/main/java/frc/robot/util/NoteSimulator.java index 5135299..0361c1c 100644 --- a/src/main/java/frc/robot/util/NoteSimulator.java +++ b/src/main/java/frc/robot/util/NoteSimulator.java @@ -11,100 +11,111 @@ import org.littletonrobotics.junction.Logger; public class NoteSimulator { - private static Drive drive; - - private static Pose3d currentFieldPose = new Pose3d(); - private static Translation3d fieldVelocity = new Translation3d(); - private static boolean inShooter = false; - private static List noteTrajectory = new ArrayList<>(); - - public static void setDrive(Drive drivesystem) { - drive = drivesystem; - } - - public static void attachToShooter() { - inShooter = true; - noteTrajectory.clear(); - } - - public static boolean isAttached() { - return inShooter; - } - - public static List getNoteTrajectory() { - return noteTrajectory; - } - - public static void launch(double velocity) { - if (!inShooter) { - return; - } - - Logger.recordOutput("Launch Velocity", velocity); - - currentFieldPose = getFieldPose(Constants.NoteSim.SHOOTER_POSE3D); - inShooter = false; - - fieldVelocity = new Translation3d(velocity, currentFieldPose.getRotation()); - - ChassisSpeeds robotVel = drive.getChassisSpeed(); - ChassisSpeeds fieldRel = ChassisSpeeds.fromRobotRelativeSpeeds(robotVel, drive.getRotation()); - - fieldVelocity = - fieldVelocity.plus( - new Translation3d(fieldRel.vxMetersPerSecond, fieldRel.vyMetersPerSecond, 0.0)); - } - - public static Pose3d getFieldPose(Pose3d shooterPose) { - if (inShooter) { - return new Pose3d(drive.getPose()) - .transformBy(new Transform3d(shooterPose.getTranslation(), shooterPose.getRotation())); - } - - return currentFieldPose; - } - - public static void update() { - if (inShooter) { - return; - } - - Translation3d posDelta = fieldVelocity.times(Constants.NoteSim.dt); - - currentFieldPose = - new Pose3d( - currentFieldPose.getTranslation().plus(posDelta), currentFieldPose.getRotation()); - - if (currentFieldPose.getX() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getX() - >= Constants.NoteSim.FIELD_SIZE.getX() + Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getY() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getY() - >= Constants.NoteSim.FIELD_SIZE.getY() + Constants.NoteSim.OUT_OF_FIELD_MARGIN - || currentFieldPose.getZ() <= 0.0) { - fieldVelocity = new Translation3d(); - } else { - fieldVelocity = - fieldVelocity.minus(Constants.NoteSim.GRAVITY_TRANSLATION3D.times(Constants.NoteSim.dt)); - double norm = fieldVelocity.getNorm(); - - double fDrag = - (Constants.NoteSim.AIR_DENSITY - * Math.pow(norm, 2) - * Constants.NoteSim.DRAG_COEFFICIENT - * Constants.NoteSim.CROSSECTION_AREA) - / Constants.AVG_TWO_ITEM_F; - double deltaV = (Constants.NoteSim.MASS * fDrag) * Constants.NoteSim.dt; - - double t = (norm - deltaV) / norm; - fieldVelocity = fieldVelocity.times(t); - noteTrajectory.add(currentFieldPose.getTranslation()); - } - } - - public static void logNoteInfo() { - Logger.recordOutput( - "SimNoteTrajectory", NoteSimulator.getNoteTrajectory().toArray(new Translation3d[0])); - Logger.recordOutput("SimNotePose3d", getFieldPose(Constants.NoteSim.SHOOTER_POSE3D)); - } + + private static Drive drive; + + private static Pose3d currentFieldPose = new Pose3d(); + private static Translation3d fieldVelocity = new Translation3d(); + private static boolean inShooter = false; + private static List noteTrajectory = new ArrayList<>(); + + public static void setDrive(Drive drivesystem) { + drive = drivesystem; + } + + public static void attachToShooter() { + inShooter = true; + noteTrajectory.clear(); + } + + public static boolean isAttached() { + return inShooter; + } + + public static List getNoteTrajectory() { + return noteTrajectory; + } + + public static void launch(double velocity) { + if (!inShooter) { + return; + } + + Logger.recordOutput("Launch Velocity", velocity); + + currentFieldPose = getFieldPose(Constants.NoteSim.SHOOTER_POSE3D); + inShooter = false; + + fieldVelocity = new Translation3d(velocity, currentFieldPose.getRotation()); + + ChassisSpeeds robotVel = drive.getChassisSpeed(); + ChassisSpeeds fieldRel = ChassisSpeeds.fromRobotRelativeSpeeds( + robotVel, + drive.getRotation() + ); + + fieldVelocity = fieldVelocity.plus( + new Translation3d(fieldRel.vxMetersPerSecond, fieldRel.vyMetersPerSecond, 0.0) + ); + } + + public static Pose3d getFieldPose(Pose3d shooterPose) { + if (inShooter) { + return new Pose3d(drive.getPose()).transformBy( + new Transform3d(shooterPose.getTranslation(), shooterPose.getRotation()) + ); + } + + return currentFieldPose; + } + + public static void update() { + if (inShooter) { + return; + } + + Translation3d posDelta = fieldVelocity.times(Constants.NoteSim.dt); + + currentFieldPose = new Pose3d( + currentFieldPose.getTranslation().plus(posDelta), + currentFieldPose.getRotation() + ); + + if ( + currentFieldPose.getX() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getX() >= + Constants.NoteSim.FIELD_SIZE.getX() + Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getY() <= -Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getY() >= + Constants.NoteSim.FIELD_SIZE.getY() + Constants.NoteSim.OUT_OF_FIELD_MARGIN || + currentFieldPose.getZ() <= 0.0 + ) { + fieldVelocity = new Translation3d(); + } else { + fieldVelocity = fieldVelocity.minus( + Constants.NoteSim.GRAVITY_TRANSLATION3D.times(Constants.NoteSim.dt) + ); + double norm = fieldVelocity.getNorm(); + + double fDrag = + (Constants.NoteSim.AIR_DENSITY * + Math.pow(norm, 2) * + Constants.NoteSim.DRAG_COEFFICIENT * + Constants.NoteSim.CROSSECTION_AREA) / + Constants.AVG_TWO_ITEM_F; + double deltaV = (Constants.NoteSim.MASS * fDrag) * Constants.NoteSim.dt; + + double t = (norm - deltaV) / norm; + fieldVelocity = fieldVelocity.times(t); + noteTrajectory.add(currentFieldPose.getTranslation()); + } + } + + public static void logNoteInfo() { + Logger.recordOutput( + "SimNoteTrajectory", + NoteSimulator.getNoteTrajectory().toArray(new Translation3d[0]) + ); + Logger.recordOutput("SimNotePose3d", getFieldPose(Constants.NoteSim.SHOOTER_POSE3D)); + } }