From 49449b6987ff8569bca846ef705e533533b2847e Mon Sep 17 00:00:00 2001 From: BR88C Date: Wed, 16 Oct 2024 14:05:30 -0400 Subject: [PATCH] Initial commit --- .github/workflows/dashboard.yml | 111 ++- .github/workflows/robot-code.yml | 9 +- .gitignore | 3 +- .vscode/extensions.json | 3 +- .vscode/settings.json | 55 +- advantagescope.json | 658 ---------------- build.gradle | 21 +- dashboard/package.json | 59 +- dashboard/src-tauri/Cargo.toml | 36 +- dashboard/src-tauri/build.rs | 2 +- dashboard/src-tauri/capabilities/default.json | 10 + dashboard/src-tauri/src/lib.rs | 7 + dashboard/src-tauri/src/main.rs | 23 +- dashboard/src-tauri/tauri.conf.json | 92 +-- dashboard/src/App.svelte | 2 - dashboard/src/main.ts | 2 +- dashboard/src/ntStores.ts | 21 - dashboard/src/tabs/DriverView.svelte | 150 +--- dashboard/src/tabs/Tunables.svelte | 106 --- dashboard/tsconfig.json | 22 +- dashboard/vite.config.ts | 2 +- epilogue/.gitignore | 243 ++++++ epilogue/README.md | 1 + epilogue/build.gradle | 12 + epilogue/buildSrc/build.gradle | 3 + .../groovy/epilogue.java-conventions.gradle | 45 ++ epilogue/epilogue-processor/build.gradle | 14 + .../processor/AnnotationProcessor.java | 418 ++++++++++ .../epilogue/processor/ArrayHandler.java | 77 ++ .../epilogue/processor/CollectionHandler.java | 61 ++ .../processor/ConfiguredLoggerHandler.java | 45 ++ .../epilogue/processor/ElementHandler.java | 137 ++++ .../first/epilogue/processor/EnumHandler.java | 32 + .../epilogue/processor/EpilogueGenerator.java | 170 +++++ .../epilogue/processor/LoggableHandler.java | 46 ++ .../epilogue/processor/LoggerGenerator.java | 236 ++++++ .../epilogue/processor/MeasureHandler.java | 32 + .../epilogue/processor/PrimitiveHandler.java | 42 + .../epilogue/processor/SendableHandler.java | 51 ++ .../first/epilogue/processor/StringUtils.java | 112 +++ .../epilogue/processor/StructHandler.java | 52 ++ .../epilogue/processor/SupplierHandler.java | 62 ++ epilogue/epilogue-runtime/build.gradle | 7 + .../wpi/first/epilogue/CustomLoggerFor.java | 30 + .../first/epilogue/EpilogueConfiguration.java | 44 ++ .../java/edu/wpi/first/epilogue/Logged.java | 92 +++ .../edu/wpi/first/epilogue/NotLogged.java | 19 + .../epilogue/logging/ClassSpecificLogger.java | 117 +++ .../first/epilogue/logging/DataLogger.java | 228 ++++++ .../first/epilogue/logging/FileLogger.java | 144 ++++ .../first/epilogue/logging/LazyLogger.java | 241 ++++++ .../logging/LogBackedSendableBuilder.java | 207 +++++ .../first/epilogue/logging/MultiLogger.java | 135 ++++ .../first/epilogue/logging/NTDataLogger.java | 153 ++++ .../first/epilogue/logging/NullLogger.java | 63 ++ .../wpi/first/epilogue/logging/SubLogger.java | 116 +++ .../epilogue/logging/errors/CrashOnError.java | 23 + .../epilogue/logging/errors/ErrorHandler.java | 61 ++ .../epilogue/logging/errors/ErrorPrinter.java | 21 + .../logging/errors/LoggerDisabler.java | 71 ++ epilogue/gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43462 bytes .../gradle/wrapper/gradle-wrapper.properties | 7 + epilogue/gradlew | 249 ++++++ epilogue/gradlew.bat | 92 +++ epilogue/settings.gradle | 3 + gradle/wrapper/gradle-wrapper.jar | Bin 43462 -> 43583 bytes gradle/wrapper/gradle-wrapper.properties | 2 +- gradlew | 7 +- gradlew.bat | 22 +- src/main/java/com/choreo/lib/Choreo.java | 74 +- .../com/choreo/lib/ChoreoTrajectoryState.java | 12 +- .../java/org/team340/lib/GRRDashboard.java | 413 ---------- .../java/org/team340/lib/GRRSubsystem.java | 492 ------------ .../org/team340/lib/HardwareSendables.java | 719 ------------------ .../team340/lib/controller/Controller.java | 387 ++++++++++ .../team340/lib/controller/Controller2.java | 470 ------------ .../lib/controller/Controller2Config.java | 147 ---- .../lib/controller/ControllerConfig.java | 49 ++ .../lib/controller/ControllerLogger.java | 18 + .../lib/controller/JoystickProfile.java | 56 -- .../lib/controller/JoystickProfiler.java | 107 --- .../team340/lib/dashboard/GRRDashboard.java | 180 +++++ .../org/team340/lib/dashboard/Tunable.java | 254 +++++++ .../team340/lib/logging/ADIS16470Logger.java | 28 + .../team340/lib/logging/CANcoderLogger.java | 22 + .../lib/logging/DigitalInputLogger.java | 19 + .../team340/lib/logging/EncoderLogger.java | 20 + .../team340/lib/logging/Pigeon2Logger.java | 27 + .../lib/logging/RelativeEncoderLogger.java | 20 + .../logging/SparkAbsoluteEncoderLogger.java | 20 + .../team340/lib/logging/SparkFlexLogger.java | 24 + .../lib/logging/SparkLimitSwitchLogger.java | 19 + .../team340/lib/logging/SparkMaxLogger.java | 24 + .../lib/logging/SparkPIDControllerLogger.java | 19 + .../logging/SparkRelativeEncoderLogger.java | 20 + .../team340/lib/logging/TalonFXLogger.java | 27 + .../org/team340/lib/swerve/SwerveAPI.java | 581 ++++++++++++++ .../team340/lib/swerve/SwerveAPILogger.java | 55 ++ .../org/team340/lib/swerve/SwerveBase.java | 683 ----------------- .../org/team340/lib/swerve/SwerveModule.java | 269 ++++--- .../org/team340/lib/swerve/SwerveState.java | 84 ++ .../team340/lib/swerve/SwerveTunables.java | 97 +++ .../lib/swerve/config/SwerveConfig.java | 703 +++++------------ .../lib/swerve/config/SwerveModuleConfig.java | 273 ++----- .../swerve/hardware/SwerveBaseHardware.java | 50 ++ .../lib/swerve/hardware/SwerveEncoders.java | 319 ++++++++ .../lib/swerve/hardware/SwerveIMUs.java | 257 +++++++ .../lib/swerve/hardware/SwerveMotors.java | 548 +++++++++++++ .../hardware/encoders/SwerveEncoder.java | 30 - .../encoders/vendors/SwerveCANcoder.java | 47 -- .../encoders/vendors/SwerveSparkEncoder.java | 37 - .../lib/swerve/hardware/imu/SwerveIMU.java | 44 -- .../lib/swerve/hardware/imu/SwerveIMUSim.java | 44 -- .../hardware/imu/vendors/SwerveADIS16470.java | 43 -- .../hardware/imu/vendors/SwervePigeon2.java | 57 -- .../swerve/hardware/motors/SwerveMotor.java | 63 -- .../motors/vendors/SwerveSparkFlex.java | 145 ---- .../motors/vendors/SwerveSparkMax.java | 145 ---- .../motors/vendors/SwerveTalonFX.java | 125 --- .../lib/swerve/util/SwerveConversions.java | 21 - .../lib/swerve/util/SwerveOdometryThread.java | 124 --- .../lib/swerve/util/SwerveRatelimiter.java | 457 ----------- .../lib/swerve/util/SwerveVisualizer.java | 194 ----- .../java/org/team340/lib/util/Alliance.java | 29 +- .../{commands => util}/CommandBuilder.java | 22 +- .../org/team340/lib/util/GRRSubsystem.java | 25 + src/main/java/org/team340/lib/util/Math2.java | 282 +++---- .../java/org/team340/lib/util/Mutable.java | 16 +- .../java/org/team340/lib/util/Polar2d.java | 27 - .../java/org/team340/lib/util/Profiler.java | 206 +++++ .../org/team340/lib/util/SendableFactory.java | 38 - src/main/java/org/team340/lib/util/Sleep.java | 51 +- .../java/org/team340/lib/util/StringUtil.java | 37 +- .../org/team340/lib/util/TriggerLockout.java | 26 - .../lib/util/config/FeedForwardConfig.java | 43 -- .../team340/lib/util/config/PIDConfig.java | 34 - .../lib/util/config/rev/RevConfigBase.java | 144 ---- .../util/config/rev/RevConfigRegistry.java | 141 ---- .../team340/lib/util/ctre/PhoenixUtil.java | 58 ++ .../rev/RelativeEncoderConfig.java | 37 +- .../team340/lib/util/rev/RevConfigBase.java | 76 ++ .../lib/util/rev/RevConfigRegistry.java | 150 ++++ .../rev/SparkAbsoluteEncoderConfig.java | 37 +- .../{config => }/rev/SparkFlexConfig.java | 115 ++- .../rev/SparkLimitSwitchConfig.java | 33 +- .../util/{config => }/rev/SparkMaxConfig.java | 111 ++- .../rev/SparkPIDControllerConfig.java | 81 +- .../java/org/team340/robot/Constants.java | 576 +++----------- src/main/java/org/team340/robot/Robot.java | 59 +- .../org/team340/robot/RobotContainer.java | 208 ++--- .../org/team340/robot/commands/Autos.java | 121 --- .../org/team340/robot/commands/Routines.java | 204 ++--- .../team340/robot/commands/SystemsCheck.java | 24 - .../org/team340/robot/subsystems/Climber.java | 125 --- .../org/team340/robot/subsystems/Feeder.java | 174 ++--- .../org/team340/robot/subsystems/Intake.java | 336 +++----- .../org/team340/robot/subsystems/Lights.java | 141 ---- .../org/team340/robot/subsystems/Pivot.java | 306 ++++---- .../org/team340/robot/subsystems/Shooter.java | 432 ++++------- .../org/team340/robot/subsystems/Swerve.java | 719 +++++------------- vendordeps/Phoenix5.json | 151 ---- vendordeps/Phoenix6.json | 48 +- vendordeps/REVLib.json | 10 +- vendordeps/URCL.json | 65 ++ vendordeps/photonlib.json | 10 +- 165 files changed, 9860 insertions(+), 10269 deletions(-) delete mode 100644 advantagescope.json create mode 100644 dashboard/src-tauri/capabilities/default.json create mode 100644 dashboard/src-tauri/src/lib.rs delete mode 100644 dashboard/src/tabs/Tunables.svelte create mode 100644 epilogue/.gitignore create mode 100644 epilogue/README.md create mode 100644 epilogue/build.gradle create mode 100644 epilogue/buildSrc/build.gradle create mode 100644 epilogue/buildSrc/src/main/groovy/epilogue.java-conventions.gradle create mode 100644 epilogue/epilogue-processor/build.gradle create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EpilogueGenerator.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java create mode 100644 epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java create mode 100644 epilogue/epilogue-runtime/build.gradle create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/CustomLoggerFor.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/EpilogueConfiguration.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/Logged.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/NotLogged.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/DataLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LogBackedSendableBuilder.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTDataLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/SubLogger.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/CrashOnError.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorHandler.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorPrinter.java create mode 100644 epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/LoggerDisabler.java create mode 100644 epilogue/gradle/wrapper/gradle-wrapper.jar create mode 100644 epilogue/gradle/wrapper/gradle-wrapper.properties create mode 100644 epilogue/gradlew create mode 100644 epilogue/gradlew.bat create mode 100644 epilogue/settings.gradle delete mode 100644 src/main/java/org/team340/lib/GRRDashboard.java delete mode 100644 src/main/java/org/team340/lib/GRRSubsystem.java delete mode 100644 src/main/java/org/team340/lib/HardwareSendables.java create mode 100644 src/main/java/org/team340/lib/controller/Controller.java delete mode 100644 src/main/java/org/team340/lib/controller/Controller2.java delete mode 100644 src/main/java/org/team340/lib/controller/Controller2Config.java create mode 100644 src/main/java/org/team340/lib/controller/ControllerConfig.java create mode 100644 src/main/java/org/team340/lib/controller/ControllerLogger.java delete mode 100644 src/main/java/org/team340/lib/controller/JoystickProfile.java delete mode 100644 src/main/java/org/team340/lib/controller/JoystickProfiler.java create mode 100644 src/main/java/org/team340/lib/dashboard/GRRDashboard.java create mode 100644 src/main/java/org/team340/lib/dashboard/Tunable.java create mode 100644 src/main/java/org/team340/lib/logging/ADIS16470Logger.java create mode 100644 src/main/java/org/team340/lib/logging/CANcoderLogger.java create mode 100644 src/main/java/org/team340/lib/logging/DigitalInputLogger.java create mode 100644 src/main/java/org/team340/lib/logging/EncoderLogger.java create mode 100644 src/main/java/org/team340/lib/logging/Pigeon2Logger.java create mode 100644 src/main/java/org/team340/lib/logging/RelativeEncoderLogger.java create mode 100644 src/main/java/org/team340/lib/logging/SparkAbsoluteEncoderLogger.java create mode 100644 src/main/java/org/team340/lib/logging/SparkFlexLogger.java create mode 100644 src/main/java/org/team340/lib/logging/SparkLimitSwitchLogger.java create mode 100644 src/main/java/org/team340/lib/logging/SparkMaxLogger.java create mode 100644 src/main/java/org/team340/lib/logging/SparkPIDControllerLogger.java create mode 100644 src/main/java/org/team340/lib/logging/SparkRelativeEncoderLogger.java create mode 100644 src/main/java/org/team340/lib/logging/TalonFXLogger.java create mode 100644 src/main/java/org/team340/lib/swerve/SwerveAPI.java create mode 100644 src/main/java/org/team340/lib/swerve/SwerveAPILogger.java delete mode 100644 src/main/java/org/team340/lib/swerve/SwerveBase.java create mode 100644 src/main/java/org/team340/lib/swerve/SwerveState.java create mode 100644 src/main/java/org/team340/lib/swerve/SwerveTunables.java create mode 100644 src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java create mode 100644 src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java create mode 100644 src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java create mode 100644 src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveSparkEncoder.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMUSim.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwerveADIS16470.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java delete mode 100644 src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java delete mode 100644 src/main/java/org/team340/lib/swerve/util/SwerveConversions.java delete mode 100644 src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java delete mode 100644 src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java delete mode 100644 src/main/java/org/team340/lib/swerve/util/SwerveVisualizer.java rename src/main/java/org/team340/lib/{commands => util}/CommandBuilder.java (78%) create mode 100644 src/main/java/org/team340/lib/util/GRRSubsystem.java delete mode 100644 src/main/java/org/team340/lib/util/Polar2d.java create mode 100644 src/main/java/org/team340/lib/util/Profiler.java delete mode 100644 src/main/java/org/team340/lib/util/SendableFactory.java delete mode 100644 src/main/java/org/team340/lib/util/TriggerLockout.java delete mode 100644 src/main/java/org/team340/lib/util/config/FeedForwardConfig.java delete mode 100644 src/main/java/org/team340/lib/util/config/PIDConfig.java delete mode 100644 src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java delete mode 100644 src/main/java/org/team340/lib/util/config/rev/RevConfigRegistry.java create mode 100644 src/main/java/org/team340/lib/util/ctre/PhoenixUtil.java rename src/main/java/org/team340/lib/util/{config => }/rev/RelativeEncoderConfig.java (77%) create mode 100644 src/main/java/org/team340/lib/util/rev/RevConfigBase.java create mode 100644 src/main/java/org/team340/lib/util/rev/RevConfigRegistry.java rename src/main/java/org/team340/lib/util/{config => }/rev/SparkAbsoluteEncoderConfig.java (76%) rename src/main/java/org/team340/lib/util/{config => }/rev/SparkFlexConfig.java (84%) rename src/main/java/org/team340/lib/util/{config => }/rev/SparkLimitSwitchConfig.java (52%) rename src/main/java/org/team340/lib/util/{config => }/rev/SparkMaxConfig.java (85%) rename src/main/java/org/team340/lib/util/{config => }/rev/SparkPIDControllerConfig.java (88%) delete mode 100644 src/main/java/org/team340/robot/commands/Autos.java delete mode 100644 src/main/java/org/team340/robot/commands/SystemsCheck.java delete mode 100644 src/main/java/org/team340/robot/subsystems/Climber.java delete mode 100644 src/main/java/org/team340/robot/subsystems/Lights.java delete mode 100644 vendordeps/Phoenix5.json create mode 100644 vendordeps/URCL.json diff --git a/.github/workflows/dashboard.yml b/.github/workflows/dashboard.yml index 0b175e4..ce71121 100644 --- a/.github/workflows/dashboard.yml +++ b/.github/workflows/dashboard.yml @@ -1,9 +1,12 @@ name: Dashboard on: [pull_request, push] +env: + NODE_VERSION: 20 + jobs: format: - name: Format + name: Check Format runs-on: ubuntu-latest steps: @@ -15,7 +18,7 @@ jobs: - name: Setup Node uses: actions/setup-node@v4 with: - node-version: 20 + node-version: ${{ env.NODE_VERSION }} registry-url: https://registry.npmjs.org/ - name: Install Node Modules @@ -27,13 +30,34 @@ jobs: working-directory: dashboard build: - name: Build + env: + MACOSX_DEPLOYMENT_TARGET: 13.3 strategy: + fail-fast: false matrix: - node: [20] - platform: [windows-latest, macos-latest, ubuntu-latest] + include: + - artifact: Windows-x86_64 + platform: windows-2022 + build-flags: --target=x86_64-pc-windows-msvc + + - artifact: Windows-arm64 + platform: windows-2022 + build-flags: --target=aarch64-pc-windows-msvc + + - artifact: macOS-x86_64 + platform: macOS-14 + build-flags: --target=x86_64-apple-darwin + + - artifact: macOS-arm64 + platform: macOS-14 + build-flags: --target=aarch64-apple-darwin + + - artifact: Linux-x86_64 + platform: ubuntu-22.04 + build-flags: --target=x86_64-unknown-linux-gnu + name: "Build ${{ matrix.artifact }}" runs-on: ${{ matrix.platform }} steps: @@ -45,22 +69,85 @@ jobs: - name: Setup Node uses: actions/setup-node@v4 with: - node-version: ${{ matrix.node }} + node-version: ${{ env.NODE_VERSION }} registry-url: https://registry.npmjs.org/ - name: Setup Rust uses: dtolnay/rust-toolchain@stable - name: Install System Dependencies (Ubuntu Only) - if: matrix.platform == 'ubuntu-latest' + if: startsWith(matrix.platform, 'ubuntu') run: | - sudo apt-get update - sudo apt-get install -y libgtk-3-dev libwebkit2gtk-4.0-dev libappindicator3-dev librsvg2-dev patchelf + sudo apt-get update -q + sudo apt-get install -y \ + libwebkit2gtk-4.1-dev \ + build-essential \ + curl \ + wget \ + file \ + libxdo-dev \ + libssl-dev \ + libayatana-appindicator3-dev \ + librsvg2-dev - name: Install Node Modules run: npm i --include=dev working-directory: dashboard - - name: Run Build - run: npm run build:tauri - working-directory: dashboard + - name: Install Windows aarch64 Rust compiler (Windows-arm64 Only) + if: matrix.artifact == 'Windows-arm64' + run: rustup target install aarch64-pc-windows-msvc + + - name: Install macOS x86_64 Rust compiler (macOS-x86_64 Only) + if: matrix.artifact == 'macOS-x86_64' + run: rustup target add x86_64-apple-darwin + + - name: Setup Xcode (macOS Only) + if: startsWith(matrix.platform, 'macOS') + run: sudo xcode-select -switch /Applications/Xcode_15.3.app + + - name: Run Tauri Build + uses: tauri-apps/tauri-action@v0 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + projectPath: dashboard + args: ${{ matrix.build-flags }} + + - name: Upload bundle (Windows-x86_54) + if: matrix.artifact == 'Windows-x86_64' + uses: actions/upload-artifact@v4 + with: + name: GRRDashboard-${{ matrix.artifact }} + path: dashboard/src-tauri/target/x86_64-pc-windows-msvc/release/bundle/nsis/*.exe + + - name: Upload bundle (Windows-arm64) + if: matrix.artifact == 'Windows-arm64' + uses: actions/upload-artifact@v4 + with: + name: GRRDashboard-${{ matrix.artifact }} + path: dashboard/src-tauri/target/aarch64-pc-windows-msvc/release/bundle/nsis/*.exe + + - name: Upload bundle (macOS-x86_64) + if: matrix.artifact == 'macOS-x86_64' + uses: actions/upload-artifact@v4 + with: + name: GRRDashboard-${{ matrix.artifact }} + path: dashboard/src-tauri/target/x86_64-apple-darwin/release/bundle/dmg/*.dmg + + - name: Upload bundle (macOS-arm64) + if: matrix.artifact == 'macOS-arm64' + uses: actions/upload-artifact@v4 + with: + name: GRRDashboard-${{ matrix.artifact }} + path: dashboard/src-tauri/target/aarch64-apple-darwin/release/bundle/dmg/*.dmg + + - name: Upload bundle (Linux-x86_64) + if: matrix.artifact == 'Linux-x86_64' + uses: actions/upload-artifact@v4 + with: + name: GRRDashboard-${{ matrix.artifact }} + path: | + dashboard/src-tauri/target/x86_64-unknown-linux-gnu/release/bundle/appimage/*.AppImage + dashboard/src-tauri/target/x86_64-unknown-linux-gnu/release/bundle/deb/*.deb + dashboard/src-tauri/target/x86_64-unknown-linux-gnu/release/bundle/rpm/*.rpm diff --git a/.github/workflows/robot-code.yml b/.github/workflows/robot-code.yml index 690122a..2abd919 100644 --- a/.github/workflows/robot-code.yml +++ b/.github/workflows/robot-code.yml @@ -1,9 +1,12 @@ name: Robot Code on: [pull_request, push] +env: + NODE_VERSION: 20 + jobs: format: - name: Format + name: Check Format runs-on: ubuntu-latest container: wpilib/ubuntu-base:22.04 @@ -19,7 +22,7 @@ jobs: - name: Setup Node uses: actions/setup-node@v4 with: - node-version: 20 + node-version: ${{ env.NODE_VERSION }} registry-url: https://registry.npmjs.org/ - name: Grant execute permission for gradlew @@ -45,7 +48,7 @@ jobs: - name: Setup Node uses: actions/setup-node@v4 with: - node-version: 20 + node-version: ${{ env.NODE_VERSION }} registry-url: https://registry.npmjs.org/ - name: Grant execute permission for gradlew diff --git a/.gitignore b/.gitignore index 61f68c2..a22a671 100644 --- a/.gitignore +++ b/.gitignore @@ -180,8 +180,9 @@ logs/ ctre_sim/ # Dashboard -dashboard/dist/ +dashboard/build/ dashboard/node_modules/ dashboard/src-tauri/target/ +dashboard/src-tauri/gen/schemas/ dashboard/src-tauri/Cargo.lock dashboard/package-lock.json diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 35ddc89..052966a 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -19,7 +19,6 @@ "vscjava.vscode-java-test", "wpilibsuite.vscode-wpilib", "asciidoctor.asciidoctor-vscode", - "eamodio.gitlens", - "GitHub.vscode-pull-request-github" + "tauri-apps.tauri-vscode" ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index fffde08..acbfbc0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,7 +1,28 @@ { "java.configuration.updateBuildConfiguration": "automatic", "java.debug.settings.onBuildFailureProceed": true, + "java.inlayHints.parameterNames.enabled": "literals", + "java.inlayHints.parameterNames.exclusions": [ + "edu.wpi.first.math.util.Units.*", + "(key, value)", + "(initialValue)", + "(magnitude)", + "(value)" + ], "java.server.launchMode": "Standard", + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "svelte.enable-ts-plugin": true, "files.exclude": { "**/.git": true, "**/.svn": true, @@ -15,61 +36,52 @@ "**/.factorypath": true, "**/*~": true }, - "java.test.config": [ - { - "name": "WPIlibUnitTests", - "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], - "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , - "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" - } - }, - ], - "java.test.defaultConfig": "WPIlibUnitTests", "cSpell.words": [ "Accl", "Accum", "ADIS", "Bezier", "Botpose", - "Blacklight", - "Blacklights", "Brushless", "CANandCoder", + "Canbus", "CANcoder", "Checkstyle", "Choreo", - "ChoreoLib", "Cnfg", "CTRE", "Deadband", + "Deadbands", "Deadzone", + "Decel", "DeltAng", "DeltVel", "Desaturate", "Desaturated", + "Desaturating", "Desaturation", "Devs", "Discretization", "Discretize", - "Dont", + "Discretizing", "DTheta", "Falsi", "Feedforward", + "Fullscreen", "Gradlew", "Grav", "GRRDashboard", - "GSON", "Holonomic", + "Intaking", "Integ", - "Interp", "Interpolatable", + "Itor", "JoystickProfiles", "Keepalive", "Lerp", "Motorcontrol", "Msgpack", + "Mult", "NetworkTables", "NTURI", "Odometry", @@ -79,6 +91,8 @@ "Protobuf", "Pubuid", "Quasistatic", + "Ratelimit", + "Ratelimited", "Ratelimiter", "Ratelimits", "Regula", @@ -87,21 +101,22 @@ "RoboRIO", "Sendables", "Setpoint", - "setpoints", "Subuid", "TalonFX", "TalonSRX", "Tauri", "Teleop", + "Timesync", "Topicsonly", "Traj", - "Trajs", "Tunables", "Unannounce", "Unsub", "Unsubscriber", "Unsubscribers", + "URCL", "Vbat", + "Vmax", "WPIBlue", "WPILib", "WPILibJ", diff --git a/advantagescope.json b/advantagescope.json deleted file mode 100644 index ca70f0c..0000000 --- a/advantagescope.json +++ /dev/null @@ -1,658 +0,0 @@ -{ - "version": "3.0.2", - "layout": [ - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Robot/voltage", - "color": "#2b66a2", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "NT:/GRRDashboard/Robot/enabled", - "color": "#e5b31b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Details/command", - "color": "#af2437", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Feeder/Details/command", - "color": "#80588e", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Details/command", - "color": "#e48b32", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Lights/Details/command", - "color": "#aacaee", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Pivot/Details/command", - "color": "#c0b487", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Details/command", - "color": "#858584", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Details/command", - "color": "#3b875a", - "show": true - }, - { - "key": "NT:/GRRDashboard/Autos/default", - "color": "#5d4f92", - "show": true - }, - { - "key": "NT:/GRRDashboard/Autos/selected", - "color": "#a64b6b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Autos/active", - "color": "#eb987e", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Details/readErrors", - "color": "#d993aa", - "show": true - } - ] - } - }, - "title": "Voltage" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-2/powerUsage", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-3/powerUsage", - "color": "#e5b31b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-4/powerUsage", - "color": "#af2437", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-5/powerUsage", - "color": "#80588e", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-6/powerUsage", - "color": "#e48b32", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-7/powerUsage", - "color": "#aacaee", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-8/powerUsage", - "color": "#c0b487", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Hardware/CAN-9/powerUsage", - "color": "#858584", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Hardware/CAN-20/powerUsage", - "color": "#3b875a", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Hardware/CAN-21/powerUsage", - "color": "#d993aa", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Hardware/CAN-22/powerUsage", - "color": "#eb987e", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Hardware/CAN-23/powerUsage", - "color": "#5d4f92", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Pivot/Hardware/CAN-30/powerUsage", - "color": "#a64b6b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Feeder/Hardware/CAN-31/powerUsage", - "color": "#dbd345", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Hardware/CAN-32/powerUsage", - "color": "#7e331f", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Hardware/CAN-33/powerUsage", - "color": "#96b637", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Hardware/CAN-40/powerUsage", - "color": "#5f4528", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Hardware/CAN-41/powerUsage", - "color": "#d36134", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Power Usage" - }, - { - "type": 6, - "fields": [], - "listFields": [ - [ - { - "type": "Vision Target", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/visionTargets", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Green Ghost", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Details/speaker", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ], - [ - { - "type": "Robot", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/robot", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Trajectory", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/trajectory", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Yellow Ghost", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/trajectoryTarget", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Yellow Ghost", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/target", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Green Ghost", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/visionMeasurements", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ] - ], - "options": { - "field": "2024 Field", - "alliance": "auto", - "robot": "Duck Bot", - "unitDistance": "meters", - "unitRotation": "radians", - "cameraIndex": -1, - "fov": 50 - }, - "configHidden": false, - "title": "Field" - }, - { - "type": 5, - "fields": [], - "listFields": [ - [ - { - "type": "Robot", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/robot", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Trajectory", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/trajectory", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Ghost", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/trajectoryTarget", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "type": "Ghost", - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/target", - "sourceTypeIndex": 0, - "sourceType": 5 - } - ] - ], - "options": { - "game": "2024 Field", - "unitDistance": "meters", - "unitRotation": "radians", - "origin": "right", - "size": 0.8382, - "allianceBumpers": "auto", - "allianceOrigin": "blue", - "orientation": "blue left, red right" - }, - "configHidden": false, - "title": "Odometry" - }, - { - "type": 9, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/moduleStates", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/desiredModuleStates", - "sourceTypeIndex": 0, - "sourceType": 5 - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/robot/2", - "sourceTypeIndex": 0, - "sourceType": 2 - } - ], - "listFields": [], - "options": { - "maxSpeed": 5, - "rotationUnits": "radians", - "arrangement": "0,3,1,2", - "sizeLeftRight": 0.65, - "sizeFrontBack": 0.65, - "forwardDirection": "up" - }, - "configHidden": false, - "title": "Swerve" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/moduleStates/1", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/desiredModuleStates/1", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "NT:/GRRDashboard/Robot/enabled", - "color": "#af2437", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Front Left Velocity" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": "angle", - "factor": 1, - "from": "radians", - "to": "degrees" - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/moduleStates/0", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Swerve/Visualizations/desiredModuleStates/0", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "NT:/GRRDashboard/Robot/enabled", - "color": "#af2437", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Front Left Heading" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": "angle", - "factor": 1, - "from": "radians", - "to": "degrees" - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Hardware/CAN-20-AbsoluteEncoder/position", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Details/armTarget", - "color": "#e5b31b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Intake/Details/armMaintain", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Intake" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": "angle", - "factor": 1, - "from": "radians", - "to": "degrees" - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Pivot/Hardware/CAN-30/position", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Pivot/Details/target", - "color": "#e5b31b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Pivot/Details/maintain", - "color": "#af2437", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [] - } - }, - "title": "Pivot" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Details/leftTarget", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Hardware/CAN-32/velocity", - "color": "#e5b31b", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Details/rightTarget", - "color": "#af2437", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Hardware/CAN-33/velocity", - "color": "#80588e", - "show": true - } - ] - }, - "discrete": { - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Details/leftPIDActive", - "color": "#e48b32", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Details/rightPIDActive", - "color": "#aacaee", - "show": true - } - ] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Hardware/CAN-32/current", - "color": "#c0b487", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Shooter/Hardware/CAN-33/current", - "color": "#858584", - "show": true - } - ] - } - }, - "title": "Shooter" - }, - { - "type": 1, - "legends": { - "left": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Hardware/CAN-40/output", - "color": "#2b66a2", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Hardware/CAN-41/output", - "color": "#e5b31b", - "show": true - } - ] - }, - "discrete": { - "fields": [] - }, - "right": { - "lockedRange": null, - "unitConversion": { - "type": null, - "factor": 1 - }, - "fields": [ - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Hardware/CAN-40/current", - "color": "#af2437", - "show": true - }, - { - "key": "NT:/GRRDashboard/Subsystems/Climber/Hardware/CAN-41/current", - "color": "#80588e", - "show": true - } - ] - } - }, - "title": "Climb" - }, - { - "type": 8, - "fields": [], - "listFields": [], - "options": { - "ids": [ - 0, - 1, - 2 - ], - "layouts": [ - "Xbox Controller (White)", - "Xbox Controller (Blue)", - "None" - ] - }, - "configHidden": false, - "title": "Controllers" - } - ] -} diff --git a/build.gradle b/build.gradle index 1b79653..cac24ac 100644 --- a/build.gradle +++ b/build.gradle @@ -1,9 +1,11 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" - id "com.diffplug.spotless" version "6.23.3" + id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "com.diffplug.spotless" version "6.25.0" } +repositories { mavenLocal() } + java { sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 @@ -51,6 +53,10 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { + implementation 'edu.wpi.first.epilogue:epilogue-runtime:0.0.0' + annotationProcessor 'edu.wpi.first.epilogue:epilogue-processor:0.0.0' + annotationProcessor 'edu.wpi.first.epilogue:epilogue-runtime:0.0.0' + implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -86,8 +92,15 @@ spotless { endWithNewline() removeUnusedImports() trimTrailingWhitespace() - prettier(['prettier': '2.8.8', 'prettier-plugin-java': '2.2.0']) - .config(['parser': 'java', tabWidth: 4, useTabs: false, printWidth: 140 ]) + prettier(['prettier': '3.3.3', 'prettier-plugin-java': '2.6.4']) + .config([ + 'parser': 'java', + 'plugins': ['prettier-plugin-java'], + printWidth: 120, + tabWidth: 4, + useTabs: false, + trailingComma: "none" + ]) } } diff --git a/dashboard/package.json b/dashboard/package.json index b7e09a5..aadf0b3 100644 --- a/dashboard/package.json +++ b/dashboard/package.json @@ -1,30 +1,33 @@ { - "name": "grrdashboard", - "type": "module", - "scripts": { - "build": "vite build --emptyOutDir", - "build:tauri": "tauri build", - "check": "svelte-check --tsconfig ./tsconfig.json", - "dev": "vite", - "dev:tauri": "tauri dev", - "format": "prettier --write --plugin prettier-plugin-svelte ./src/", - "format:check": "prettier --check --plugin prettier-plugin-svelte ./src/", - "preview": "vite preview", - "tauri": "tauri" - }, - "devDependencies": { - "@sveltejs/vite-plugin-svelte": "^3.0.1", - "@tauri-apps/cli": "^1.5.9", - "@tsconfig/svelte": "^5.0.2", - "chart.js": "^4.4.1", - "chartjs-plugin-zoom": "^2.0.1", - "prettier": "^3.2.4", - "prettier-plugin-svelte": "^3.1.2", - "svelte": "^4.2.9", - "svelte-check": "^3.6.3", - "tslib": "^2.6.2", - "typescript": "^5.3.3", - "vite": "^5.0.12" - }, - "private": true + "name": "grrdashboard", + "version": "2025.0.0", + "description": "", + "type": "module", + "scripts": { + "build": "vite build --emptyOutDir", + "check": "svelte-check --tsconfig ./tsconfig.json", + "dev": "vite", + "format": "prettier --write --plugin prettier-plugin-svelte ./src/", + "format:check": "prettier --check --plugin prettier-plugin-svelte ./src/", + "preview": "vite preview", + "tauri": "tauri" + }, + "license": "MIT", + "dependencies": { + "@tauri-apps/api": ">=2.0.0-rc.0", + "@tauri-apps/plugin-shell": ">=2.0.0-rc.0" + }, + "devDependencies": { + "@sveltejs/adapter-static": "^3.0.5", + "@sveltejs/vite-plugin-svelte": "^3.1.2", + "@tauri-apps/cli": ">=2.0.0-rc.0", + "@tsconfig/svelte": "^5.0.4", + "prettier": "^3.3.3", + "prettier-plugin-svelte": "^3.2.6", + "svelte": "^4.2.19", + "svelte-check": "^4.0.2", + "tslib": "^2.7.0", + "typescript": "^5.6.2", + "vite": "^5.4.7" + } } diff --git a/dashboard/src-tauri/Cargo.toml b/dashboard/src-tauri/Cargo.toml index 864bd12..be94c78 100644 --- a/dashboard/src-tauri/Cargo.toml +++ b/dashboard/src-tauri/Cargo.toml @@ -1,29 +1,23 @@ [package] -name = "app" -version = "0.1.0" -description = "A Tauri App" -authors = ["you"] -license = "" -repository = "" -default-run = "app" +name = "grrdashboard" +version = "2025.0.0" edition = "2021" -rust-version = "1.60" +description = "A custom built dashboard by Team 340 for the FIRST Robotics Competition" +repository = "https://github.com/Greater-Rochester-Robotics/GRRBase.git" +homepage = "https://team340.org" +authors = ["Team 340"] +license = "MIT" -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[lib] +name = "grrdashboard_lib" +crate-type = ["lib", "cdylib", "staticlib"] [build-dependencies] -tauri-build = { version = "1.4.0", features = [] } +tauri-build = { version = "2.0.0-rc", features = [] } [dependencies] -portpicker = "0.1.1" -serde_json = "1.0" -serde = { version = "1.0", features = ["derive"] } -tauri = { version = "1.4.0", features = [] } -tauri-plugin-localhost = "0.1.0" -tauri-plugin-positioner = "1.0.4" +tauri = { version = "2.0.0-rc", features = [] } +tauri-plugin-shell = "2.0.0-rc" +serde = { version = "1", features = ["derive"] } +serde_json = "1" -[features] -# this feature is used for production builds or when `devPath` points to the filesystem and the built-in dev server is disabled. -# If you use cargo directly instead of tauri's cli you can use this feature flag to switch between tauri's `dev` and `build` modes. -# DO NOT REMOVE!! -custom-protocol = [ "tauri/custom-protocol" ] diff --git a/dashboard/src-tauri/build.rs b/dashboard/src-tauri/build.rs index 795b9b7..d860e1e 100644 --- a/dashboard/src-tauri/build.rs +++ b/dashboard/src-tauri/build.rs @@ -1,3 +1,3 @@ fn main() { - tauri_build::build() + tauri_build::build() } diff --git a/dashboard/src-tauri/capabilities/default.json b/dashboard/src-tauri/capabilities/default.json new file mode 100644 index 0000000..3bb4cc4 --- /dev/null +++ b/dashboard/src-tauri/capabilities/default.json @@ -0,0 +1,10 @@ +{ + "$schema": "../gen/schemas/desktop-schema.json", + "identifier": "default", + "description": "Capability for the main window", + "windows": ["main"], + "permissions": [ + "core:default", + "shell:allow-open" + ] +} diff --git a/dashboard/src-tauri/src/lib.rs b/dashboard/src-tauri/src/lib.rs new file mode 100644 index 0000000..e89e1c4 --- /dev/null +++ b/dashboard/src-tauri/src/lib.rs @@ -0,0 +1,7 @@ +#[cfg_attr(mobile, tauri::mobile_entry_point)] +pub fn run() { + tauri::Builder::default() + .plugin(tauri_plugin_shell::init()) + .run(tauri::generate_context!()) + .expect("error while running tauri application"); +} diff --git a/dashboard/src-tauri/src/main.rs b/dashboard/src-tauri/src/main.rs index 5020efb..b72f801 100644 --- a/dashboard/src-tauri/src/main.rs +++ b/dashboard/src-tauri/src/main.rs @@ -1,25 +1,6 @@ +// Prevents additional console window on Windows in release, DO NOT REMOVE!! #![cfg_attr(not(debug_assertions), windows_subsystem = "windows")] -use tauri::Manager; -use tauri::{utils::config::AppUrl, WindowUrl}; -use tauri_plugin_positioner::{WindowExt, Position}; - fn main() { - let port = portpicker::pick_unused_port().expect("failed to find unused port"); - - let mut context = tauri::generate_context!(); - let url = format!("http://localhost:{}", port).parse().unwrap(); - let window_url = WindowUrl::External(url); - - context.config_mut().build.dist_dir = AppUrl::Url(window_url.clone()); - - tauri::Builder::default() - .plugin(tauri_plugin_localhost::Builder::new(port).build()) - .setup(|app| { - let win = app.get_window("main").unwrap(); - let _ = win.move_window(Position::TopCenter); - Ok(()) - }) - .run(context) - .expect("error while running tauri application"); + grrdashboard_lib::run() } diff --git a/dashboard/src-tauri/tauri.conf.json b/dashboard/src-tauri/tauri.conf.json index 31b7bcf..f4e3f48 100644 --- a/dashboard/src-tauri/tauri.conf.json +++ b/dashboard/src-tauri/tauri.conf.json @@ -1,60 +1,38 @@ { - "$schema": "../node_modules/@tauri-apps/cli/schema.json", - "build": { - "beforeBuildCommand": "npm run build", - "beforeDevCommand": "npm run dev", - "devPath": "http://localhost:5173", - "distDir": "../dist" - }, - "package": { - "productName": "GRRDashboard", - "version": "0.0.0" - }, - "tauri": { - "allowlist": { - "all": false - }, - "bundle": { - "active": false, - "category": "DeveloperTool", - "copyright": "", - "deb": { - "depends": [] - }, - "externalBin": [], - "icon": ["icons/32x32.png", "icons/128x128.png", "icons/128x128@2x.png", "icons/icon.icns", "icons/icon.ico"], - "identifier": "org.team340.grrdashboard", - "longDescription": "", - "macOS": { - "entitlements": null, - "exceptionDomain": "", - "frameworks": [], - "providerShortName": null, - "signingIdentity": null - }, - "resources": [], - "shortDescription": "", - "targets": "all", - "windows": { - "certificateThumbprint": null, - "digestAlgorithm": "sha256", - "timestampUrl": "" - } - }, - "security": { - "csp": null - }, - "updater": { - "active": false - }, - "windows": [ - { - "title": "GRRDashboard", - "fullscreen": false, - "width": 1920, - "height": 800, - "resizable": true - } - ] + "$schema": "https://schema.tauri.app/config/2.0.0-rc", + "productName": "GRRDashboard", + "version": "2025.0.0", + "identifier": "org.team340.grrdashboard", + "build": { + "beforeDevCommand": "npm run dev", + "devUrl": "http://localhost:5173", + "beforeBuildCommand": "npm run build", + "frontendDist": "../build" + }, + "app": { + "windows": [ + { + "title": "GRRDashboard", + "fullscreen": false, + "width": 1920, + "height": 800, + "resizable": true } + ], + "security": { + "csp": null + } + }, + "bundle": { + "active": true, + "targets": ["appimage", "deb", "dmg", "nsis", "rpm"], + "shortDescription": "GRRDashboard", + "icon": [ + "icons/32x32.png", + "icons/128x128.png", + "icons/128x128@2x.png", + "icons/icon.icns", + "icons/icon.ico" + ] + } } diff --git a/dashboard/src/App.svelte b/dashboard/src/App.svelte index 3f92375..0c2fca4 100644 --- a/dashboard/src/App.svelte +++ b/dashboard/src/App.svelte @@ -6,7 +6,6 @@ import AutoSelection from "./tabs/AutoSelection.svelte"; import DriverView from "./tabs/DriverView.svelte"; - import Tunables from "./tabs/Tunables.svelte"; import { NTConnected } from "./ntStores"; import { NTSvelteClientState } from "./lib/NTSvelte"; @@ -18,7 +17,6 @@ const tabs = { "Driver View": DriverView, "Auto Selection": AutoSelection, - Tunables: Tunables, }; // Helpers for tab selection. diff --git a/dashboard/src/main.ts b/dashboard/src/main.ts index d9391db..3bce418 100644 --- a/dashboard/src/main.ts +++ b/dashboard/src/main.ts @@ -1,5 +1,5 @@ import App from "./App.svelte"; -const app = new App({ target: document.getElementById(`app`) }); +const app = new App({ target: document.getElementById(`app`) as any }); export default app; diff --git a/dashboard/src/ntStores.ts b/dashboard/src/ntStores.ts index 33cee81..f78df61 100644 --- a/dashboard/src/ntStores.ts +++ b/dashboard/src/ntStores.ts @@ -17,25 +17,4 @@ export const AutosActive = nt.subscribe(`/GRRDashboard/Autos/active`, `` export const AutosOptions = nt.subscribe(`/GRRDashboard/Autos/options`, []); export const AutosSelected = nt.publish(`/GRRDashboard/Autos/selected`, `string`, ``); -export const FacingSpeaker = nt.subscribe(`/GRRDashboard/Subsystems/Swerve/Details/facingSpeaker`, false); -export const PivotAtPosition = nt.subscribe(`/GRRDashboard/Subsystems/Pivot/Details/atPosition`, false); -export const AtSpeed = nt.subscribe(`/GRRDashboard/Subsystems/Shooter/Details/atSpeed`, false); -export const HasNote = nt.subscribe(`/GRRDashboard/Subsystems/Pivot/Details/hasNote`, false); - -export const RobotPosition = nt.subscribe(`/GRRDashboard/Subsystems/Swerve/Visualizations/robot`, [0, 0, 0]); - -export const TunableNoteVelocity = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableNoteVelocity`, `double`, 5.6); -export const TunableNormFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableNormFudge`, `double`, 0.49); -export const TunableStrafeFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableStrafeFudge`, `double`, 0.85); -export const TunableSpinCompensation = nt.publish( - `/GRRDashboard/Subsystems/Swerve/Details/tunableSpinCompensation`, - `double`, - -2 * (Math.PI / 180), -); -export const TunableDistanceFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableDistanceFudge`, `double`, 0.0); -export const TunableSpeakerXFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableSpeakerXFudge`, `double`, 0.0); -export const TunableSpeakerYFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableSpeakerYFudge`, `double`, 0.0); -export const TunableAmpXFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableAmpXFudge`, `double`, 0.0); -export const TunableAmpYFudge = nt.publish(`/GRRDashboard/Subsystems/Swerve/Details/tunableAmpYFudge`, `double`, 0.0); - nt.connect(); diff --git a/dashboard/src/tabs/DriverView.svelte b/dashboard/src/tabs/DriverView.svelte index 1ee29e1..ce20372 100644 --- a/dashboard/src/tabs/DriverView.svelte +++ b/dashboard/src/tabs/DriverView.svelte @@ -1,156 +1,12 @@
-
-
-
-
-

Has Note

-
-
-

Facing Speaker

-
-
- -
-

Shoot!

-
- -
-
-

Pivot At Position

-
-
-

At Speed

-
-
-
- -
- field - - - - - - - -
-
+

Nothing to see here!

diff --git a/dashboard/src/tabs/Tunables.svelte b/dashboard/src/tabs/Tunables.svelte deleted file mode 100644 index 1bbdbed..0000000 --- a/dashboard/src/tabs/Tunables.svelte +++ /dev/null @@ -1,106 +0,0 @@ - - -
-
-
-

Note Velocity:

- {$TunableNoteVelocity} -

-

Norm Fudge:

- {$TunableNormFudge} -

-

Strafe Fudge:

- {$TunableStrafeFudge} -

-

Spin Compensation:

- {$TunableSpinCompensation} -

-

Distance Fudge:

- {$TunableDistanceFudge} -

-

Speaker X:

- {$TunableSpeakerXFudge} -

-

Speaker Y:

- {$TunableSpeakerYFudge} -

-

Amp X:

- {$TunableAmpXFudge} -

-

Amp Y:

- {$TunableAmpYFudge} -
-
- - diff --git a/dashboard/tsconfig.json b/dashboard/tsconfig.json index be3a172..3b6833a 100644 --- a/dashboard/tsconfig.json +++ b/dashboard/tsconfig.json @@ -1,13 +1,13 @@ { - "extends": "@tsconfig/svelte/tsconfig.json", - "compilerOptions": { - "strict": true, - "target": "ESNext", - "useDefineForClassFields": true, - "module": "ESNext", - "checkJs": true, - "isolatedModules": true, - }, - "include": ["src/**/*.d.ts", "src/**/*.ts", "src/**/*.js", "src/**/*.svelte"], - "references": [{ "path": "./tsconfig.node.json" }], + "extends": "@tsconfig/svelte/tsconfig.json", + "compilerOptions": { + "strict": true, + "target": "ESNext", + "useDefineForClassFields": true, + "module": "ESNext", + "checkJs": true, + "isolatedModules": true, + }, + "include": ["src/**/*.d.ts", "src/**/*.ts", "src/**/*.js", "src/**/*.svelte"], + "references": [{ "path": "./tsconfig.node.json" }], } diff --git a/dashboard/vite.config.ts b/dashboard/vite.config.ts index 3bbabaa..eeb0bb2 100644 --- a/dashboard/vite.config.ts +++ b/dashboard/vite.config.ts @@ -5,7 +5,7 @@ import { svelte } from "@sveltejs/vite-plugin-svelte"; export default defineConfig({ build: { minify: !process.env.TAURI_DEBUG ? "esbuild" : false, - outDir: `./dist`, + outDir: `./build`, sourcemap: !!process.env.TAURI_DEBUG, target: process.env.TAURI_PLATFORM == "windows" ? "chrome105" : "safari13", }, diff --git a/epilogue/.gitignore b/epilogue/.gitignore new file mode 100644 index 0000000..edea361 --- /dev/null +++ b/epilogue/.gitignore @@ -0,0 +1,243 @@ +# WPIlib Specific + +dependency-reduced-pom.xml +doxygen.log +build*/ +!buildSrc/ + +simgui-ds.json +simgui-window.json +simgui.json + +networktables.json + +# Created by the jenkins test script +test-reports + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Created by http://www.gitignore.io + +### Linux ### +*~ + +# KDE directory preferences +.directory + + +### Windows ### +# Windows image file caches +Thumbs.db +ehthumbs.db + +# Folder config file +Desktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msm +*.msp + + +### OSX ### +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + + +# Thumbnails +._* + +# Files that might appear on external disk +.Spotlight-V100 +.Trashes + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + + +### Java ### +*.class + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.ear + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + + +### C++ ### +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Compiled Dynamic libraries +*.dylib +*.dll + +# Fortran module files +*.mod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + + +### Maven ### +target/ +pom.xml.tag +pom.xml.releaseBackup +pom.xml.versionsBackup +pom.xml.next +release.properties + + +### CMake ### +CMakeCache.txt +CMakeFiles +cmake_install.cmake +install_manifest.txt + + +### Gradle ### +.gradle +build/ + +# Ignore Gradle GUI config +gradle-app.setting + + +### Vagrant ### +.vagrant/ + + +### Eclipse ### +*.pydevproject +.metadata +.gradle +bin/ +tmp/ +*.tmp +*.bak +*.swp +*~.nib +local.properties +.settings/ +.loadpath + +### Python ### +*.pyc +__pycache__ + +# Gradle wrapper +!gradle-wrapper.jar + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + +# sbteclipse plugin +.target + +# TeXlipse plugin +.texlipse + +#catkin stuff +package.xml + +# Doxygen stuff +NO + +# Simulation folder stuff +!simulation/install_resources/* + +# VSCode +.vscode/ + +#classpaths and projects +.project +.classpath + +#Visual Studio +# User-specific files +*.suo +*.user +*.userosscache +*.sln.docstates +CMakeSettings.json + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.cachefile +*.VC.db +*.VC.VC.opendb +*.VC.db-shm +*.VC.db-wal + +*.sln +*.vcxproj +*.vcxproj.filters + +# Visual Studio 2015 cache/options directory +.vs/ + +# compile_commands +compile_commands.json + +# clang configuration and clangd cache +.clang +.clangd/ +.cache/ + +imgui.ini + +# Bazel +/.ijwb/ +/bazel-* +user.bazelrc +coverage_report/ diff --git a/epilogue/README.md b/epilogue/README.md new file mode 100644 index 0000000..c354d7f --- /dev/null +++ b/epilogue/README.md @@ -0,0 +1 @@ +Run `./gradlew publishToMavenLocal` diff --git a/epilogue/build.gradle b/epilogue/build.gradle new file mode 100644 index 0000000..a29ded0 --- /dev/null +++ b/epilogue/build.gradle @@ -0,0 +1,12 @@ +buildscript { + repositories { + maven { + url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn' + } + } +} + +plugins { + id 'edu.wpi.first.GradleRIO' version '2024.3.2' + id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2' +} diff --git a/epilogue/buildSrc/build.gradle b/epilogue/buildSrc/build.gradle new file mode 100644 index 0000000..6784052 --- /dev/null +++ b/epilogue/buildSrc/build.gradle @@ -0,0 +1,3 @@ +plugins { + id 'groovy-gradle-plugin' +} diff --git a/epilogue/buildSrc/src/main/groovy/epilogue.java-conventions.gradle b/epilogue/buildSrc/src/main/groovy/epilogue.java-conventions.gradle new file mode 100644 index 0000000..3132e08 --- /dev/null +++ b/epilogue/buildSrc/src/main/groovy/epilogue.java-conventions.gradle @@ -0,0 +1,45 @@ +plugins { + id 'java-library' + id 'maven-publish' + id 'edu.wpi.first.GradleRIO' + id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' +} + +if (project.hasProperty('releaseMode')) { + wpilibRepositories.addAllReleaseRepositories(project) +} else { + wpilibRepositories.addAllDevelopmentRepositories(project) +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 + + withSourcesJar() + withJavadocJar() +} + +dependencies { + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +group = 'edu.wpi.first.epilogue' +version = '0.0.0' + +publishing { + publications { + maven(MavenPublication) { + groupId = group + artifactId = project.name + version = version + + from components.java + } + } +} diff --git a/epilogue/epilogue-processor/build.gradle b/epilogue/epilogue-processor/build.gradle new file mode 100644 index 0000000..b4570b7 --- /dev/null +++ b/epilogue/epilogue-processor/build.gradle @@ -0,0 +1,14 @@ +plugins { + id 'epilogue.java-conventions' +} + +dependencies { + implementation(project(':epilogue-runtime')) + + api wpi.java.deps.wpilib() + api 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:+' + + implementation 'com.google.auto.service:auto-service:1.1.1' + annotationProcessor 'com.google.auto.service:auto-service:1.1.1' + testImplementation 'com.google.testing.compile:compile-testing:+' +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java new file mode 100644 index 0000000..4d66719 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/AnnotationProcessor.java @@ -0,0 +1,418 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import com.google.auto.service.AutoService; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Set; +import javax.annotation.processing.AbstractProcessor; +import javax.annotation.processing.Processor; +import javax.annotation.processing.RoundEnvironment; +import javax.annotation.processing.SupportedAnnotationTypes; +import javax.annotation.processing.SupportedSourceVersion; +import javax.lang.model.SourceVersion; +import javax.lang.model.element.AnnotationMirror; +import javax.lang.model.element.AnnotationValue; +import javax.lang.model.element.Element; +import javax.lang.model.element.ElementKind; +import javax.lang.model.element.ExecutableElement; +import javax.lang.model.element.Modifier; +import javax.lang.model.element.TypeElement; +import javax.lang.model.element.VariableElement; +import javax.lang.model.type.DeclaredType; +import javax.lang.model.type.NoType; +import javax.lang.model.type.TypeKind; +import javax.lang.model.type.TypeMirror; +import javax.tools.Diagnostic; + +@SupportedAnnotationTypes({ "edu.wpi.first.epilogue.CustomLoggerFor", "edu.wpi.first.epilogue.Logged" }) +@SupportedSourceVersion(SourceVersion.RELEASE_17) +@AutoService(Processor.class) +public class AnnotationProcessor extends AbstractProcessor { + + private static final String kCustomLoggerFqn = "edu.wpi.first.epilogue.CustomLoggerFor"; + private static final String kClassSpecificLoggerFqn = "edu.wpi.first.epilogue.logging.ClassSpecificLogger"; + private static final String kLoggedFqn = "edu.wpi.first.epilogue.Logged"; + + private EpilogueGenerator m_epiloguerGenerator; + private LoggerGenerator m_loggerGenerator; + private List m_handlers; + + @Override + public boolean process(Set annotations, RoundEnvironment roundEnv) { + if (annotations.isEmpty()) { + // Nothing to do, don't claim + return false; + } + + Map customLoggers = new HashMap<>(); + + annotations + .stream() + .filter(ann -> kCustomLoggerFqn.contentEquals(ann.getQualifiedName())) + .findAny() + .ifPresent(customLogger -> { + customLoggers.putAll(processCustomLoggers(roundEnv, customLogger)); + }); + + roundEnv + .getRootElements() + .stream() + .filter(e -> + processingEnv + .getTypeUtils() + .isAssignable( + e.asType(), + processingEnv + .getTypeUtils() + .erasure(processingEnv.getElementUtils().getTypeElement(kClassSpecificLoggerFqn).asType()) + ) + ) + .filter(e -> e.getAnnotation(CustomLoggerFor.class) == null) + .forEach(e -> { + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "Custom logger classes should have a @CustomLoggerFor annotation", + e + ); + }); + + // Handlers are declared in order of priority. If an element could be logged in more than one + // way (eg a class implements both Sendable and StructSerializable), the order of the handlers + // in this list will determine how it gets logged. + m_handlers = List.of( + new LoggableHandler(processingEnv), // prioritize epilogue logging over Sendable + new ConfiguredLoggerHandler(processingEnv, customLoggers), // then customized logging configs + new ArrayHandler(processingEnv), + new CollectionHandler(processingEnv), + new EnumHandler(processingEnv), + new MeasureHandler(processingEnv), + new PrimitiveHandler(processingEnv), + new SupplierHandler(processingEnv), + new StructHandler(processingEnv), // prioritize struct over sendable + new SendableHandler(processingEnv) + ); + + m_epiloguerGenerator = new EpilogueGenerator(processingEnv, customLoggers); + m_loggerGenerator = new LoggerGenerator(processingEnv, m_handlers); + + annotations + .stream() + .filter(ann -> kLoggedFqn.contentEquals(ann.getQualifiedName())) + .findAny() + .ifPresent(epilogue -> { + processEpilogue(roundEnv, epilogue); + }); + + return false; + } + + private boolean validateFields(Set annotatedElements) { + var fields = annotatedElements + .stream() + .filter(e -> e instanceof VariableElement) + .map(e -> (VariableElement) e) + .toList(); + + boolean valid = true; + + for (VariableElement field : fields) { + // Field is explicitly tagged + // And is not opted out of + if (field.getAnnotation(NotLogged.class) == null && isNotLoggable(field, field.asType())) { + // And is not of a loggable type + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "[EPILOGUE] You have opted in to logging on this field, " + + "but it is not a loggable data type!", + field + ); + valid = false; + } + } + return valid; + } + + private boolean validateMethods(Set annotatedElements) { + var methods = annotatedElements + .stream() + .filter(e -> e instanceof ExecutableElement) + .map(e -> (ExecutableElement) e) + .toList(); + + boolean valid = true; + + for (ExecutableElement method : methods) { + // Field is explicitly tagged + if (method.getAnnotation(NotLogged.class) == null) { + // And is not opted out of + if (isNotLoggable(method, method.getReturnType())) { + // And is not of a loggable type + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "[EPILOGUE] You have opted in to logging on this method, " + + "but it does not return a loggable data type!", + method + ); + valid = false; + } + + if (!method.getModifiers().contains(Modifier.PUBLIC)) { + // Only public methods can be logged + + processingEnv + .getMessager() + .printMessage(Diagnostic.Kind.ERROR, "[EPILOGUE] Logged methods must be public", method); + + valid = false; + } + + if (method.getModifiers().contains(Modifier.STATIC)) { + processingEnv + .getMessager() + .printMessage(Diagnostic.Kind.ERROR, "[EPILOGUE] Logged methods cannot be static", method); + + valid = false; + } + + if (method.getReturnType().getKind() == TypeKind.NONE) { + processingEnv + .getMessager() + .printMessage(Diagnostic.Kind.ERROR, "[EPILOGUE] Logged methods cannot be void", method); + + valid = false; + } + + if (!method.getParameters().isEmpty()) { + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "[EPILOGUE] Logged methods cannot accept arguments", + method + ); + + valid = false; + } + } + } + return valid; + } + + /** + * Checks if a type is not loggable. + * + * @param type the type to check + */ + private boolean isNotLoggable(Element element, TypeMirror type) { + if (type instanceof NoType) { + // e.g. void, cannot log + return true; + } + + boolean loggable = m_handlers.stream().anyMatch(h -> h.isLoggable(element)); + + if (loggable) { + return false; + } + + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.NOTE, + "[EPILOGUE] Excluded from logs because " + type + " is not a loggable data type", + element + ); + return true; + } + + @SuppressWarnings("unchecked") + private Map processCustomLoggers( + RoundEnvironment roundEnv, + TypeElement customLoggerAnnotation + ) { + // map logged type to its custom logger, eg + // { Point.class => CustomPointLogger.class } + var customLoggers = new HashMap(); + + var annotatedElements = roundEnv.getElementsAnnotatedWith(customLoggerAnnotation); + + var loggerSuperClass = processingEnv + .getElementUtils() + .getTypeElement("edu.wpi.first.epilogue.logging.ClassSpecificLogger"); + + for (Element annotatedElement : annotatedElements) { + List targetTypes = List.of(); + for (AnnotationMirror annotationMirror : annotatedElement.getAnnotationMirrors()) { + for (var entry : annotationMirror.getElementValues().entrySet()) { + if ("value".equals(entry.getKey().getSimpleName().toString())) { + targetTypes = (List) entry.getValue().getValue(); + } + } + } + + boolean hasPublicNoArgConstructor = annotatedElement + .getEnclosedElements() + .stream() + .anyMatch( + enclosedElement -> + enclosedElement instanceof ExecutableElement exe && + exe.getKind() == ElementKind.CONSTRUCTOR && + exe.getModifiers().contains(Modifier.PUBLIC) && + exe.getParameters().isEmpty() + ); + + if (!hasPublicNoArgConstructor) { + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "Logger classes must have a public no-argument constructor", + annotatedElement + ); + continue; + } + + for (AnnotationValue value : targetTypes) { + var targetType = (DeclaredType) value.getValue(); + var reflectedTarget = targetType.asElement(); + + // eg ClassSpecificLogger + var requiredSuperClass = processingEnv + .getTypeUtils() + .getDeclaredType( + loggerSuperClass, + processingEnv.getTypeUtils().getWildcardType(null, reflectedTarget.asType()) + ); + + if (customLoggers.containsKey(targetType)) { + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "Multiple custom loggers detected for type " + targetType, + annotatedElement + ); + continue; + } + + if (!processingEnv.getTypeUtils().isAssignable(annotatedElement.asType(), requiredSuperClass)) { + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "Not a subclass of ClassSpecificLogger<" + targetType + ">", + annotatedElement + ); + continue; + } + + customLoggers.put(targetType, (DeclaredType) annotatedElement.asType()); + } + } + + return customLoggers; + } + + private void processEpilogue(RoundEnvironment roundEnv, TypeElement epilogueAnnotation) { + var annotatedElements = roundEnv.getElementsAnnotatedWith(epilogueAnnotation); + + List loggerClassNames = new ArrayList<>(); + var mainRobotClasses = new ArrayList(); + + // Used to check for a main robot class + var robotBaseClass = processingEnv + .getElementUtils() + .getTypeElement("edu.wpi.first.wpilibj.TimedRobot") + .asType(); + + boolean validFields = validateFields(annotatedElements); + boolean validMethods = validateMethods(annotatedElements); + + if (!(validFields && validMethods)) { + // Generate nothing and bail + return; + } + + var classes = annotatedElements + .stream() + .filter(e -> e instanceof TypeElement) + .map(e -> (TypeElement) e) + .toList(); + for (TypeElement clazz : classes) { + try { + warnOfNonLoggableElements(clazz); + m_loggerGenerator.writeLoggerFile(clazz); + + if (processingEnv.getTypeUtils().isAssignable(clazz.getSuperclass(), robotBaseClass)) { + mainRobotClasses.add(clazz); + } + + loggerClassNames.add(StringUtils.loggerClassName(clazz)); + } catch (IOException e) { + processingEnv + .getMessager() + .printMessage( + Diagnostic.Kind.ERROR, + "Could not write logger file for " + clazz.getQualifiedName(), + clazz + ); + e.printStackTrace(System.err); + } + } + + // Sort alphabetically + mainRobotClasses.sort(Comparator.comparing(c -> c.getSimpleName().toString())); + m_epiloguerGenerator.writeEpilogueFile(loggerClassNames, mainRobotClasses); + } + + private void warnOfNonLoggableElements(TypeElement clazz) { + var config = clazz.getAnnotation(Logged.class); + if (config.strategy() == Logged.Strategy.OPT_IN) { + // field and method validations will have already checked everything + return; + } + + for (Element element : clazz.getEnclosedElements()) { + if (element.getAnnotation(NotLogged.class) != null) { + // Explicitly opted out from, don't need to check + continue; + } + + if (element.getModifiers().contains(Modifier.STATIC)) { + // static elements are never logged + continue; + } + + if (element instanceof VariableElement v) { + // isNotLoggable will internally print a warning message + isNotLoggable(v, v.asType()); + } + + if ( + element instanceof ExecutableElement exe && + exe.getModifiers().contains(Modifier.PUBLIC) && + exe.getParameters().isEmpty() + ) { + // isNotLoggable will internally print a warning message + isNotLoggable(exe, exe.getReturnType()); + } + } + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java new file mode 100644 index 0000000..0573f53 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ArrayHandler.java @@ -0,0 +1,77 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.ArrayType; +import javax.lang.model.type.PrimitiveType; +import javax.lang.model.type.TypeMirror; + +/** + * Arrays of bytes, ints, flats, doubles, booleans, Strings, and struct-serializable objects can be + * logged. No other array types - including multidimensional arrays - are loggable. + */ +public class ArrayHandler extends ElementHandler { + + private final StructHandler m_structHandler; + private final TypeMirror m_javaLangString; + + protected ArrayHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + // use a struct handler for managing struct arrays + m_structHandler = new StructHandler(processingEnv); + + m_javaLangString = lookupTypeElement(processingEnv, "java.lang.String").asType(); + } + + @Override + public boolean isLoggable(Element element) { + return dataType(element) instanceof ArrayType arr && isLoggableComponentType(arr.getComponentType()); + } + + /** + * Checks if an array containing elements of the given type can be logged. + * + * @param type the data type to check + * @return true if an array like {@code type[]} can be logged, false otherwise + */ + public boolean isLoggableComponentType(TypeMirror type) { + if (type instanceof PrimitiveType primitive) { + return switch (primitive.getKind()) { + case BYTE, INT, LONG, FLOAT, DOUBLE, BOOLEAN -> true; + default -> false; + }; + } + + return ( + m_structHandler.isLoggableType(type) || m_processingEnv.getTypeUtils().isAssignable(type, m_javaLangString) + ); + } + + @Override + public String logInvocation(Element element) { + var dataType = dataType(element); + + // known to be an array type (assuming isLoggable is checked first); this is a safe cast + var componentType = ((ArrayType) dataType).getComponentType(); + + if (m_structHandler.isLoggableType(componentType)) { + // Struct arrays need to pass in the struct serializer + return ( + "dataLogger.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element) + + ", " + + m_structHandler.structAccess(componentType) + + ")" + ); + } else { + // Primitive or string array + return "dataLogger.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + } + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java new file mode 100644 index 0000000..390deb1 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/CollectionHandler.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.DeclaredType; +import javax.lang.model.type.TypeMirror; + +/** + * Collections of strings and structs are loggable. Collections of boxed primitive types are not. + */ +public class CollectionHandler extends ElementHandler { + + private final ArrayHandler m_arrayHandler; + private final TypeMirror m_collectionType; + private final StructHandler m_structHandler; + + protected CollectionHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + m_arrayHandler = new ArrayHandler(processingEnv); + m_collectionType = processingEnv.getElementUtils().getTypeElement("java.util.Collection").asType(); + m_structHandler = new StructHandler(processingEnv); + } + + @Override + public boolean isLoggable(Element element) { + var dataType = dataType(element); + + return ( + m_processingEnv + .getTypeUtils() + .isAssignable(dataType, m_processingEnv.getTypeUtils().erasure(m_collectionType)) && + dataType instanceof DeclaredType decl && + decl.getTypeArguments().size() == 1 && + m_arrayHandler.isLoggableComponentType(decl.getTypeArguments().get(0)) + ); + } + + @Override + public String logInvocation(Element element) { + var dataType = dataType(element); + var componentType = ((DeclaredType) dataType).getTypeArguments().get(0); + + if (m_structHandler.isLoggableType(componentType)) { + return ( + "dataLogger.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element) + + ", " + + m_structHandler.structAccess(componentType) + + ")" + ); + } else { + return "dataLogger.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + } + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java new file mode 100644 index 0000000..fb79561 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ConfiguredLoggerHandler.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import java.util.Map; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.DeclaredType; +import javax.lang.model.type.TypeMirror; + +public class ConfiguredLoggerHandler extends ElementHandler { + + private final Map m_customLoggers; + + protected ConfiguredLoggerHandler( + ProcessingEnvironment processingEnv, + Map customLoggers + ) { + super(processingEnv); + this.m_customLoggers = customLoggers; + } + + @Override + public boolean isLoggable(Element element) { + return m_customLoggers.containsKey(dataType(element)); + } + + @Override + public String logInvocation(Element element) { + var dataType = dataType(element); + var loggerType = m_customLoggers.get(dataType); + + return ( + "Epilogue." + + StringUtils.lowerCamelCase(loggerType.asElement().getSimpleName()) + + ".tryUpdate(dataLogger.getSubLogger(\"" + + loggedName(element) + + "\"), " + + elementAccess(element) + + ", Epilogue.getConfig().errorHandler)" + ); + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java new file mode 100644 index 0000000..7628745 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/ElementHandler.java @@ -0,0 +1,137 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.element.ExecutableElement; +import javax.lang.model.element.Modifier; +import javax.lang.model.element.TypeElement; +import javax.lang.model.element.VariableElement; +import javax.lang.model.type.TypeMirror; + +/** + * Handles logging of fields or methods. An element that passes the {@link #isLoggable(Element)} + * check guarantees that {@link #logInvocation(Element)} will generate a code snippet that will log + * that element. Some subclasses may return {@code null} for the invocation to signal that the + * element should not be logged, but still be considered loggable for the purposes of error + * messaging during the compilation phase. + */ +public abstract class ElementHandler { + + protected final ProcessingEnvironment m_processingEnv; + + /** + * Instantiates the handler. + * + * @param processingEnv the processing environment, used to look up type information + */ + protected ElementHandler(ProcessingEnvironment processingEnv) { + this.m_processingEnv = processingEnv; + } + + protected static TypeElement lookupTypeElement(ProcessingEnvironment processingEnv, String name) { + return processingEnv.getElementUtils().getTypeElement(name); + } + + /** + * Gets the type of data that would be logged by a field or method. + * + * @param element the field or method element to check + * @return the logged datatype + */ + protected TypeMirror dataType(Element element) { + if (element instanceof VariableElement field) { + return field.asType(); + } else if (element instanceof ExecutableElement method) { + return method.getReturnType(); + } else { + throw new IllegalStateException("Unexpected" + element.getClass().getName()); + } + } + + /** + * Gets the name of a field or method as it would appear in logs. + * + * @param element the field or method element to check + * @return the name specified in the {@link Logged @Logged} annotation on the element, if present; + * otherwise, the field or method's name with no modifications + */ + public String loggedName(Element element) { + var elementName = element.getSimpleName().toString(); + var config = element.getAnnotation(Logged.class); + + if (config != null && !config.name().isBlank()) { + return config.name(); + } else { + return elementName; + } + } + + /** + * Generates the code snippet to use to access a field or method on a logged object. Private + * fields are accessed via {@link java.lang.invoke.VarHandle VarHandles} and private methods are + * accessed via {@link java.lang.invoke.MethodHandle MethodHandles} (note that this requires the + * logger file to generate those fields). Because the generated logger files are in the same + * package as the logged type, package-private, protected, and public fields and methods are + * always accessible using normal field reads and method calls. Values returned by {@code + * VarHandle} and {@code MethodHandle} invocations will be cast to the appropriate data type. + * + * @param element the element to generate the access for + * @return the generated access snippet + */ + public String elementAccess(Element element) { + if (element instanceof VariableElement field) { + return fieldAccess(field); + } else if (element instanceof ExecutableElement method) { + return methodAccess(method); + } else { + throw new IllegalStateException("Unexpected" + element.getClass().getName()); + } + } + + private static String fieldAccess(VariableElement field) { + if (field.getModifiers().contains(Modifier.PRIVATE)) { + // (com.example.Foo) $fooField.get(object) + return "(" + field.asType() + ") $" + field.getSimpleName() + ".get(object)"; + } else { + // object.fooField + return "object." + field.getSimpleName(); + } + } + + private static String methodAccess(ExecutableElement method) { + if (method.getModifiers().contains(Modifier.PRIVATE)) { + // (com.example.Foo) _getFoo.invoke(object) + // NOTE: Currently, only public methods are logged, so this branch will not be used + return "(" + method.getReturnType() + ") _" + method.getSimpleName() + ".invoke(object)"; + } else { + // object.getFoo() + return "object." + method.getSimpleName() + "()"; + } + } + + /** + * Checks if a field or method can be logged by this handler. + * + * @param element the field or method element to check + * @return true if the element can be logged, false if not + */ + public abstract boolean isLoggable(Element element); + + /** + * Generates a code snippet to place in a generated logger file to log the value of a field or + * method. Log invocations are placed in a generated implementation of {@link + * ClassSpecificLogger#update(DataLogger, Object)}, with access to the data logger and logged + * object passed to the method call. + * + * @param element the field or method element to generate the logger call for + * @return the generated log invocation + */ + public abstract String logInvocation(Element element); +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java new file mode 100644 index 0000000..cd47c75 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EnumHandler.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.TypeMirror; + +public class EnumHandler extends ElementHandler { + + private final TypeMirror m_javaLangEnum; + + protected EnumHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + // raw type java.lang.Enum + m_javaLangEnum = processingEnv + .getTypeUtils() + .erasure(processingEnv.getElementUtils().getTypeElement("java.lang.Enum").asType()); + } + + @Override + public boolean isLoggable(Element element) { + return m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_javaLangEnum); + } + + @Override + public String logInvocation(Element element) { + return "dataLogger.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EpilogueGenerator.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EpilogueGenerator.java new file mode 100644 index 0000000..baba403 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EpilogueGenerator.java @@ -0,0 +1,170 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import edu.wpi.first.epilogue.EpilogueConfiguration; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.Collection; +import java.util.List; +import java.util.Map; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.TypeElement; +import javax.lang.model.type.DeclaredType; +import javax.lang.model.type.TypeMirror; + +/** + * Generates the {@code Epilogue} file used as the main entry point to logging with Epilogue in a + * robot program. {@code Epilogue} has instances of every generated logger class, a {@link + * EpilogueConfiguration config} object, and (if the main robot class inherits from {@link + * edu.wpi.first.wpilibj.TimedRobot TimedRobot}) a {@code bind()} method to automatically add a + * periodic logging call to the robot. + */ +public class EpilogueGenerator { + + private final ProcessingEnvironment m_processingEnv; + private final Map m_customLoggers; + + public EpilogueGenerator(ProcessingEnvironment processingEnv, Map customLoggers) { + this.m_processingEnv = processingEnv; + this.m_customLoggers = customLoggers; + } + + /** + * Creates the Epilogue file, which is the main entry point for users to set up and interact with + * the generated loggers. + * + * @param loggerClassNames the names of the generated logger classes. Each of these will be + * instantiated in a public static field on the Epilogue class. + * @param mainRobotClasses the main robot classes. May be empty. Used to generate a {@code bind()} + * method to add a callback hook to a TimedRobot to log itself. + */ + @SuppressWarnings("checkstyle:LineLength") // Source code templates exceed the line length limit + public void writeEpilogueFile(List loggerClassNames, Collection mainRobotClasses) { + try { + var centralStore = m_processingEnv.getFiler().createSourceFile("edu.wpi.first.epilogue.Epilogue"); + + try (var out = new PrintWriter(centralStore.openOutputStream())) { + out.println("package edu.wpi.first.epilogue;"); + out.println(); + + loggerClassNames + .stream() + .sorted() + .forEach(name -> { + if (!name.contains(".")) { + // Logger is in the global namespace, don't need to import + return; + } + + out.println("import " + name + ";"); + }); + m_customLoggers + .values() + .stream() + .distinct() + .forEach(loggerType -> { + var name = loggerType.asElement().toString(); + if (!name.contains(".")) { + // Logger is in the global namespace, don't need to import + return; + } + out.println("import " + name + ";"); + }); + out.println(); + + out.println("public final class Epilogue {"); + out.println(" private static final EpilogueConfiguration config = new EpilogueConfiguration();"); + out.println(); + + loggerClassNames.forEach(name -> { + String simple = StringUtils.simpleName(name); + + // public static final FooLogger fooLogger = new FooLogger(); + out.print(" public static final "); + out.print(simple); + out.print(" "); + out.print(StringUtils.lowerCamelCase(simple)); + out.print(" = new "); + out.print(simple); + out.println("();"); + }); + m_customLoggers + .values() + .stream() + .distinct() + .forEach(loggerType -> { + var loggerTypeName = loggerType.asElement().getSimpleName(); + out.println( + " public static final " + + loggerTypeName + + " " + + StringUtils.lowerCamelCase(loggerTypeName) + + " = new " + + loggerTypeName + + "();" + ); + }); + out.println(); + + out.println( + """ + public static void configure(java.util.function.Consumer configurator) { + configurator.accept(config); + } + + public static EpilogueConfiguration getConfig() { + return config; + } + """ + ); + + out.println( + """ + /** + * Checks if data associated with a given importance level should be logged. + */ + public static boolean shouldLog(Logged.Importance importance) { + return importance.compareTo(config.minimumImportance) >= 0; + } + """.stripTrailing() + ); + + // Only generate a binding if the robot class is a TimedRobot + if (!mainRobotClasses.isEmpty()) { + for (TypeElement mainRobotClass : mainRobotClasses) { + String robotClassName = mainRobotClass.getQualifiedName().toString(); + + out.println(); + out.print( + """ + /** + * Updates Epilogue. This must be called periodically in order for Epilogue to record + * new values. Alternatively, {@code bind()} can be used to update at an offset from + * the main robot loop. + */ + """ + ); + out.println(" public static void update(" + robotClassName + " robot) {"); + out.println(" long start = System.nanoTime();"); + out.println( + " " + + StringUtils.loggerFieldName(mainRobotClass) + + ".tryUpdate(config.dataLogger.getSubLogger(config.root), robot, config.errorHandler);" + ); + out.println( + " config.dataLogger.log(\"Epilogue/Stats/Last Run\", (System.nanoTime() - start) / 1e6);" + ); + out.println(" }"); + } + } + + out.println("}"); + } + } catch (IOException e) { + throw new RuntimeException(e); + } + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java new file mode 100644 index 0000000..648e14d --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggableHandler.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import edu.wpi.first.epilogue.Logged; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.DeclaredType; +import javax.lang.model.type.TypeMirror; + +/** Handles logging for types annotated with the {@link Logged @Logged} annotation. */ +public class LoggableHandler extends ElementHandler { + + protected LoggableHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + } + + @Override + public boolean isLoggable(Element element) { + var dataType = dataType(element); + return ( + dataType.getAnnotation(Logged.class) != null || + (dataType instanceof DeclaredType decl && decl.asElement().getAnnotation(Logged.class) != null) + ); + } + + @Override + public String logInvocation(Element element) { + TypeMirror dataType = dataType(element); + var reflectedType = m_processingEnv + .getElementUtils() + .getTypeElement(m_processingEnv.getTypeUtils().erasure(dataType).toString()); + + return ( + "Epilogue." + + StringUtils.loggerFieldName(reflectedType) + + ".tryUpdate(dataLogger.getSubLogger(\"" + + loggedName(element) + + "\"), " + + elementAccess(element) + + ", Epilogue.getConfig().errorHandler)" + ); + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java new file mode 100644 index 0000000..f3a67aa --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/LoggerGenerator.java @@ -0,0 +1,236 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import static java.util.stream.Collectors.groupingBy; +import static java.util.stream.Collectors.toList; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import java.io.IOException; +import java.io.PrintWriter; +import java.util.EnumMap; +import java.util.List; +import java.util.function.Predicate; +import java.util.stream.Stream; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.element.ExecutableElement; +import javax.lang.model.element.Modifier; +import javax.lang.model.element.TypeElement; +import javax.lang.model.element.VariableElement; + +/** Generates logger class files for {@link Logged @Logged}-annotated classes. */ +public class LoggerGenerator { + + private final ProcessingEnvironment m_processingEnv; + private final List m_handlers; + + public LoggerGenerator(ProcessingEnvironment processingEnv, List handlers) { + this.m_processingEnv = processingEnv; + this.m_handlers = handlers; + } + + private static boolean isNotSkipped(Element e) { + return e.getAnnotation(NotLogged.class) == null; + } + + /** + * Generates the logger class used to handle data objects of the given type. The generated logger + * class will subclass from {@link edu.wpi.first.epilogue.logging.ClassSpecificLogger} and + * implement the {@code update()} method to populate a data log with information from an instance + * of the data type. + * + * @param clazz the data type that the logger should support. + * @throws IOException if the file could not be written + */ + public void writeLoggerFile(TypeElement clazz) throws IOException { + var config = clazz.getAnnotation(Logged.class); + boolean requireExplicitOptIn = config.strategy() == Logged.Strategy.OPT_IN; + + Predicate notSkipped = LoggerGenerator::isNotSkipped; + Predicate optedIn = e -> !requireExplicitOptIn || e.getAnnotation(Logged.class) != null; + + var fieldsToLog = clazz + .getEnclosedElements() + .stream() + .filter(e -> e instanceof VariableElement) + .map(e -> (VariableElement) e) + .filter(notSkipped) + .filter(optedIn) + .filter(e -> !e.getModifiers().contains(Modifier.STATIC)) + .filter(this::isLoggable) + .toList(); + + var methodsToLog = clazz + .getEnclosedElements() + .stream() + .filter(e -> e instanceof ExecutableElement) + .map(e -> (ExecutableElement) e) + .filter(notSkipped) + .filter(optedIn) + .filter(e -> !e.getModifiers().contains(Modifier.STATIC)) + .filter(e -> e.getModifiers().contains(Modifier.PUBLIC)) + .filter(e -> e.getParameters().isEmpty()) + .filter(e -> e.getReceiverType() != null) + .filter(this::isLoggable) + .toList(); + + writeLoggerFile(clazz.getQualifiedName().toString(), config, fieldsToLog, methodsToLog); + } + + private void writeLoggerFile( + String className, + Logged classConfig, + List loggableFields, + List loggableMethods + ) throws IOException { + String packageName = null; + int lastDot = className.lastIndexOf('.'); + if (lastDot > 0) { + packageName = className.substring(0, lastDot); + } + + String simpleClassName = StringUtils.simpleName(className); + String loggerClassName = className + "Logger"; + String loggerSimpleClassName = loggerClassName.substring(lastDot + 1); + + // Use the name on the class config to set the generated logger names + // This helps to avoid naming conflicts + if (!classConfig.name().isBlank()) { + loggerSimpleClassName = StringUtils.capitalize(classConfig.name().replaceAll(" ", "")) + "Logger"; + if (lastDot > 0) { + loggerClassName = packageName + "." + loggerSimpleClassName; + } else { + loggerClassName = loggerSimpleClassName; + } + } + + var loggerFile = m_processingEnv.getFiler().createSourceFile(loggerClassName); + + var privateFields = loggableFields.stream().filter(e -> e.getModifiers().contains(Modifier.PRIVATE)).toList(); + boolean requiresVarHandles = !privateFields.isEmpty(); + + try (var out = new PrintWriter(loggerFile.openWriter())) { + if (packageName != null) { + // package com.example; + out.println("package " + packageName + ";"); + out.println(); + } + + out.println("import edu.wpi.first.epilogue.Logged;"); + out.println("import edu.wpi.first.epilogue.Epilogue;"); + out.println("import edu.wpi.first.epilogue.logging.ClassSpecificLogger;"); + out.println("import edu.wpi.first.epilogue.logging.DataLogger;"); + if (requiresVarHandles) { + out.println("import java.lang.invoke.MethodHandles;"); + out.println("import java.lang.invoke.VarHandle;"); + } + out.println(); + + // public class FooLogger implements ClassSpecificLogger { + out.println( + "public class " + loggerSimpleClassName + " extends ClassSpecificLogger<" + simpleClassName + "> {" + ); + + if (requiresVarHandles) { + for (var privateField : privateFields) { + // This field needs a VarHandle to access. + // Cache it in the class to avoid lookups + out.println(" private static final VarHandle $" + privateField.getSimpleName() + ";"); + } + out.println(); + + var clazz = simpleClassName + ".class"; + + out.println(" static {"); + out.println(" try {"); + out.println(" var lookup = MethodHandles.privateLookupIn(" + clazz + ", MethodHandles.lookup());"); + + for (var privateField : privateFields) { + var fieldName = privateField.getSimpleName(); + out.println( + " $" + + fieldName + + " = lookup.findVarHandle(" + + clazz + + ", \"" + + fieldName + + "\", " + + m_processingEnv.getTypeUtils().erasure(privateField.asType()) + + ".class);" + ); + } + + out.println(" } catch (ReflectiveOperationException e) {"); + out.println( + " throw new RuntimeException(" + + "\"[EPILOGUE] Could not load private fields for logging!\", e);" + ); + out.println(" }"); + out.println(" }"); + out.println(); + } + + out.println(" public " + loggerSimpleClassName + "() {"); + out.println(" super(" + simpleClassName + ".class);"); + out.println(" }"); + out.println(); + + // @Override + // public void update(DataLogger dataLogger, Foo object) { + out.println(" @Override"); + out.println(" public void update(DataLogger dataLogger, " + simpleClassName + " object) {"); + + // Build a map of importance levels to the fields logged at those levels + // e.g. { DEBUG: [fieldA, fieldB], INFO: [fieldC], CRITICAL: [fieldD, fieldE, fieldF] } + var loggedElementsByImportance = Stream.concat(loggableFields.stream(), loggableMethods.stream()).collect( + groupingBy( + element -> { + var config = element.getAnnotation(Logged.class); + if (config == null) { + // No configuration on this element, fall back to the class-level + // configuration + return classConfig.importance(); + } else { + return config.importance(); + } + }, + () -> new EnumMap<>(Logged.Importance.class), // EnumMap for consistent ordering + toList() + ) + ); + + loggedElementsByImportance.forEach((importance, elements) -> { + out.println(" if (Epilogue.shouldLog(Logged.Importance." + importance.name() + ")) {"); + + for (var loggableElement : elements) { + // findFirst for prioritization + var handler = m_handlers.stream().filter(h -> h.isLoggable(loggableElement)).findFirst(); + + handler.ifPresent(h -> { + // May be null if the handler consumes the element but does not actually want it + // to be logged. For example, the sendable handler consumes all sendable types + // but does not log commands or subsystems, to prevent excessive warnings about + // unloggable commands. + var logInvocation = h.logInvocation(loggableElement); + if (logInvocation != null) { + out.println(logInvocation.indent(6).stripTrailing() + ";"); + } + }); + } + + out.println(" }"); + }); + + out.println(" }"); + out.println("}"); + } + } + + private boolean isLoggable(Element element) { + return m_handlers.stream().anyMatch(h -> h.isLoggable(element)); + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java new file mode 100644 index 0000000..72df0e1 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/MeasureHandler.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.TypeMirror; + +public class MeasureHandler extends ElementHandler { + + private final TypeMirror m_measure; + + protected MeasureHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + m_measure = processingEnv + .getTypeUtils() + .erasure(processingEnv.getElementUtils().getTypeElement("edu.wpi.first.units.Measure").asType()); + } + + @Override + public boolean isLoggable(Element element) { + return m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_measure); + } + + @Override + public String logInvocation(Element element) { + // DataLogger has builtin support for logging measures + return "dataLogger.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java new file mode 100644 index 0000000..b0094e1 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/PrimitiveHandler.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import static javax.lang.model.type.TypeKind.BOOLEAN; +import static javax.lang.model.type.TypeKind.BYTE; +import static javax.lang.model.type.TypeKind.CHAR; +import static javax.lang.model.type.TypeKind.DOUBLE; +import static javax.lang.model.type.TypeKind.FLOAT; +import static javax.lang.model.type.TypeKind.INT; +import static javax.lang.model.type.TypeKind.LONG; +import static javax.lang.model.type.TypeKind.SHORT; + +import java.util.Set; +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.TypeMirror; + +public class PrimitiveHandler extends ElementHandler { + + private final TypeMirror m_javaLangString; + + protected PrimitiveHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + m_javaLangString = processingEnv.getElementUtils().getTypeElement("java.lang.String").asType(); + } + + @Override + public boolean isLoggable(Element element) { + return ( + m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_javaLangString) || + Set.of(BYTE, CHAR, SHORT, INT, LONG, FLOAT, DOUBLE, BOOLEAN).contains(dataType(element).getKind()) + ); + } + + @Override + public String logInvocation(Element element) { + return "dataLogger.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java new file mode 100644 index 0000000..a5e0ba6 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SendableHandler.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.TypeMirror; + +public class SendableHandler extends ElementHandler { + + private final TypeMirror m_sendableType; + private final TypeMirror m_commandType; + private final TypeMirror m_subsystemType; + + protected SendableHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + m_sendableType = lookupTypeElement(processingEnv, "edu.wpi.first.util.sendable.Sendable").asType(); + m_commandType = lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.Command").asType(); + m_subsystemType = lookupTypeElement(processingEnv, "edu.wpi.first.wpilibj2.command.SubsystemBase").asType(); + } + + @Override + public boolean isLoggable(Element element) { + var dataType = dataType(element); + + // Accept any sendable type. However, the log invocation will return null + // for sendable types that should not be logged (commands, subsystems) + return m_processingEnv.getTypeUtils().isAssignable(dataType, m_sendableType); + } + + @Override + public String logInvocation(Element element) { + var dataType = dataType(element); + + if ( + m_processingEnv.getTypeUtils().isAssignable(dataType, m_commandType) || + m_processingEnv.getTypeUtils().isAssignable(dataType, m_subsystemType) + ) { + // Do not log commands or subsystems via their sendable implementations + // We accept all sendable objects to prevent them from being reported as not loggable, + // but their sendable implementations do not include helpful information. + // Users are free to provide custom logging implementations for commands, and tag their + // subsystems with @Logged to log their contents automatically + return null; + } + + return "logSendable(dataLogger.getSubLogger(\"" + loggedName(element) + "\"), " + elementAccess(element) + ")"; + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java new file mode 100644 index 0000000..6b8542e --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StringUtils.java @@ -0,0 +1,112 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import edu.wpi.first.epilogue.Logged; +import javax.lang.model.element.TypeElement; + +public final class StringUtils { + + private StringUtils() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Gets the simple name of a fully qualified class. + * + * @param fqn the fully qualified class name + * @return the simple class name, without a package specifier + */ + public static String simpleName(String fqn) { + return fqn.substring(fqn.lastIndexOf('.') + 1); + } + + /** + * Capitalizes a string. The first character will be capitalized, and the rest of the string will + * be left as is. + * + * @param str the string to capitalize + * @return the capitalized string + */ + public static String capitalize(CharSequence str) { + return Character.toUpperCase(str.charAt(0)) + str.subSequence(1, str.length()).toString(); + } + + /** + * Converts a string to a lower camel-cased version. This requires the input string to only + * consist of alphanumeric characters, without underscores, spaces, or any other special + * character. + * + * @param str the string to convert + * @return the lower camel-case version of the string + */ + public static String lowerCamelCase(CharSequence str) { + StringBuilder builder = new StringBuilder(str.length()); + + int i = 0; + for ( + ; + i < str.length() && + (i == 0 || + i == str.length() - 1 || + (Character.isUpperCase(str.charAt(i)) && Character.isUpperCase(str.charAt(i + 1)))); + i++ + ) { + builder.append(Character.toLowerCase(str.charAt(i))); + } + + builder.append(str.subSequence(i, str.length())); + return builder.toString(); + } + + /** + * Gets the name of the field used to hold a logger for data of the given type. + * + * @param clazz the data type that the logger supports + * @return the logger field name + */ + public static String loggerFieldName(TypeElement clazz) { + return lowerCamelCase(simpleName(loggerClassName(clazz))); + } + + /** + * Gets the name of the generated class used to log data of the given type. This will be + * fully-qualified class name, such as {@code "frc.robot.MyRobotLogger"}. + * + * @param clazz the data type that the logger supports + * @return the logger class name + */ + public static String loggerClassName(TypeElement clazz) { + var config = clazz.getAnnotation(Logged.class); + var className = clazz.getQualifiedName().toString(); + + String packageName; + int lastDot = className.lastIndexOf('.'); + if (lastDot <= 0) { + packageName = null; + } else { + packageName = className.substring(0, lastDot); + } + + String loggerClassName; + + // Use the name on the class config to set the generated logger names + // This helps to avoid naming conflicts + if (config.name().isBlank()) { + loggerClassName = className + "Logger"; + } else { + String cleaned = config.name().replaceAll(" ", ""); + + var loggerSimpleClassName = StringUtils.capitalize(cleaned) + "Logger"; + if (packageName != null) { + loggerClassName = packageName + "." + loggerSimpleClassName; + } else { + loggerClassName = loggerSimpleClassName; + } + } + + return loggerClassName; + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java new file mode 100644 index 0000000..d487c22 --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/StructHandler.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.TypeMirror; +import javax.lang.model.util.Types; + +public class StructHandler extends ElementHandler { + + private final TypeMirror m_serializable; + private final Types m_typeUtils; + + protected StructHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + m_serializable = processingEnv + .getElementUtils() + .getTypeElement("edu.wpi.first.util.struct.StructSerializable") + .asType(); + m_typeUtils = processingEnv.getTypeUtils(); + } + + @Override + public boolean isLoggable(Element element) { + return m_typeUtils.isAssignable(dataType(element), m_serializable); + } + + public boolean isLoggableType(TypeMirror type) { + return m_typeUtils.isAssignable(type, m_serializable); + } + + public String structAccess(TypeMirror serializableType) { + var className = m_typeUtils.erasure(serializableType).toString(); + return className + ".struct"; + } + + @Override + public String logInvocation(Element element) { + return ( + "dataLogger.log(\"" + + loggedName(element) + + "\", " + + elementAccess(element) + + ", " + + structAccess(dataType(element)) + + ")" + ); + } +} diff --git a/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java new file mode 100644 index 0000000..b4f2b8d --- /dev/null +++ b/epilogue/epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/SupplierHandler.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.processor; + +import javax.annotation.processing.ProcessingEnvironment; +import javax.lang.model.element.Element; +import javax.lang.model.type.TypeMirror; + +public class SupplierHandler extends ElementHandler { + + private final TypeMirror m_booleanSupplier; + private final TypeMirror m_intSupplier; + private final TypeMirror m_longSupplier; + private final TypeMirror m_doubleSupplier; + + protected SupplierHandler(ProcessingEnvironment processingEnv) { + super(processingEnv); + m_booleanSupplier = processingEnv + .getElementUtils() + .getTypeElement("java.util.function.BooleanSupplier") + .asType(); + m_intSupplier = processingEnv.getElementUtils().getTypeElement("java.util.function.IntSupplier").asType(); + m_longSupplier = processingEnv.getElementUtils().getTypeElement("java.util.function.LongSupplier").asType(); + m_doubleSupplier = processingEnv.getElementUtils().getTypeElement("java.util.function.DoubleSupplier").asType(); + } + + @Override + public boolean isLoggable(Element element) { + return ( + m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_booleanSupplier) || + m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_intSupplier) || + m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_longSupplier) || + m_processingEnv.getTypeUtils().isAssignable(dataType(element), m_doubleSupplier) + ); + } + + @Override + public String logInvocation(Element element) { + return "dataLogger.log(\"" + loggedName(element) + "\", " + elementAccess(element) + ")"; + } + + @Override + public String elementAccess(Element element) { + var typeUtils = m_processingEnv.getTypeUtils(); + var dataType = dataType(element); + String base = super.elementAccess(element); + + if (typeUtils.isAssignable(dataType, m_booleanSupplier)) { + return base + ".getAsBoolean()"; + } else if (typeUtils.isAssignable(dataType, m_intSupplier)) { + return base + ".getAsInt()"; + } else if (typeUtils.isAssignable(dataType, m_longSupplier)) { + return base + ".getAsLong()"; + } else if (typeUtils.isAssignable(dataType, m_doubleSupplier)) { + return base + ".getAsDouble()"; + } else { + throw new IllegalArgumentException("Element type is unsupported: " + dataType); + } + } +} diff --git a/epilogue/epilogue-runtime/build.gradle b/epilogue/epilogue-runtime/build.gradle new file mode 100644 index 0000000..859d594 --- /dev/null +++ b/epilogue/epilogue-runtime/build.gradle @@ -0,0 +1,7 @@ +plugins { + id 'epilogue.java-conventions' +} + +dependencies { + api wpi.java.deps.wpilib() // ntcore, wpiutil (datalog, struct), wpiunits +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/CustomLoggerFor.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/CustomLoggerFor.java new file mode 100644 index 0000000..f0a1db5 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/CustomLoggerFor.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * Placed on a subclass of {@code ClassSpecificLogger}. Epilogue will detect it at compile time and + * allow logging of data types compatible with the logger. + * + * 

+ *   {@literal @}CustomLoggerFor(VendorMotorType.class)
+ *    class ExampleMotorLogger extends ClassSpecificLogger<VendorMotorType> { }
+ *
+ */ +@Retention(RetentionPolicy.RUNTIME) +@Target(ElementType.TYPE) +public @interface CustomLoggerFor { + /** + * The class or classes of objects able to be logged with the annotated logger. + * + * @return the supported data types + */ + Class[] value(); +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/EpilogueConfiguration.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/EpilogueConfiguration.java new file mode 100644 index 0000000..fc9fa8b --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/EpilogueConfiguration.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue; + +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.epilogue.logging.NTDataLogger; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.epilogue.logging.errors.ErrorPrinter; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * A configuration object to be used by the generated {@code Epilogue} class to customize its + * behavior. + */ +@SuppressWarnings("checkstyle:MemberName") +public class EpilogueConfiguration { + + /** + * The data logger implementation for Epilogue to use. By default, this will log data directly to + * NetworkTables. NetworkTable data can be mirrored to a log file on disk by calling {@code + * DataLogManager.start()} in your {@code robotInit} method. + */ + public DataLogger dataLogger = new NTDataLogger(NetworkTableInstance.getDefault()); + + /** + * The minimum importance level of data to be logged. Defaults to debug, which logs data of all + * importance levels. Any data tagged with an importance level lower than this will not be logged. + */ + public Logged.Importance minimumImportance = Logged.Importance.DEBUG; + + /** + * The error handler for loggers to use if they encounter an error while logging. Defaults to + * printing an error to the standard output. + */ + public ErrorHandler errorHandler = new ErrorPrinter(); + + /** + * The root identifier to use for all logged data. Defaults to "Robot", but can be changed to any + * string. + */ + public String root = "Robot"; +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/Logged.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/Logged.java new file mode 100644 index 0000000..7e29ac6 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/Logged.java @@ -0,0 +1,92 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * Place this annotation on a class to automatically log every field and every public accessor + * method (methods with no arguments and return a loggable data type). Use {@link #strategy()} to + * flag a class as logging everything it can, except for those elements tagged with + * {@code @Logged(importance = NONE)}; or for logging only specific items also tagged with + * {@code @Logged}. + * + *

Logged fields may have any access modifier. Logged methods must be public; non-public methods + * will be ignored. + * + *

Epilogue can log all primitive types, arrays of primitive types (except char and short), + * Strings, arrays of Strings, sendable objects, objects with a struct serializer, and arrays of + * objects with struct serializers. + */ +@Retention(RetentionPolicy.RUNTIME) +@Target({ ElementType.FIELD, ElementType.METHOD, ElementType.TYPE }) +public @interface Logged { + /** + * The name for the annotated element to be logged as. Does nothing on class-level annotations. + * Fields and methods will default to be logged using their in-code names; use this attribute to + * set it to something custom. + * + *

If the annotation is placed on a class, the specified name will not change logged data + * (since that uses the names of the specific usages of the class in fields and methods); however, + * it will be used to set the names of the generated logger that Logged will use to log instances + * of the class. This can be used to avoid name conflicts if you have multiple classes with the + * same name, but in different packages, and want to be able to log both. + * + * @return the name to use to log the field or method under; or the name of the generated + * class-specific logger + */ + String name() default ""; + + /** Opt-in or opt-out strategies for logging. */ + enum Strategy { + /** + * Log everything except for those elements explicitly opted out of with the skip = true + * attribute. This is the default behavior. + */ + OPT_OUT, + + /** Log only fields and methods tagged with an {@link Logged} annotation. */ + OPT_IN + } + + /** + * The strategy to use for logging. Only has an effect on annotations on class or interface + * declarations. + * + * @return the strategy to use to determine which fields and methods in the class to log + */ + Strategy strategy() default Strategy.OPT_OUT; + + /** + * Data importance. Can be used at the class level to set the default importance for all data + * points in the class, and can be used on individual fields and methods to set a specific + * importance level overriding the class-level default. + */ + enum Importance { + /** Debug information. Useful for low-level information like raw sensor values. */ + DEBUG, + + /** + * Informational data. Useful for higher-level information like pose estimates or subsystem + * state. + */ + INFO, + + /** Critical data that should always be present in logs. */ + CRITICAL + } + + /** + * The importance of the annotated data. If placed on a class or interface, this will be the + * default importance of all data within that class; this can be overridden on a per-element basis + * by annotating fields and methods with their own {@code @Logged(importance = ...)} annotation. + * + * @return the importance of the annotated element + */ + Importance importance() default Importance.DEBUG; +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/NotLogged.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/NotLogged.java new file mode 100644 index 0000000..49b38e9 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/NotLogged.java @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * A field or method annotated as {@code @NotLogged} will be ignored by Epilogue when determining + * the data to log. + */ +@Target({ ElementType.FIELD, ElementType.METHOD }) +@Retention(RetentionPolicy.RUNTIME) +public @interface NotLogged { +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java new file mode 100644 index 0000000..c7f4d88 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/ClassSpecificLogger.java @@ -0,0 +1,117 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import java.util.LinkedHashMap; +import java.util.Map; + +/** + * Base class for class-specific generated loggers. Loggers are generated at compile time by the + * Epilogue annotation processor and are used at runtime for zero-overhead data logging. Users may + * also declare custom loggers, annotated with {@link CustomLoggerFor @CustomLoggerFor}, for + * Epilogue to pull in during compile time to use for logging third party types. + * + * @param the type of data supported by the logger + */ +@SuppressWarnings("unused") // Used by generated subclasses +public abstract class ClassSpecificLogger { + + private final Class m_clazz; + // TODO: This will hold onto Sendables that are otherwise no longer referenced by a robot program. + // Determine if that's a concern + // Linked hashmap to maintain insert order + private final Map m_sendables = new LinkedHashMap<>(); + + @SuppressWarnings("PMD.RedundantFieldInitializer") + private boolean m_disabled = false; + + /** + * Instantiates the logger. + * + * @param clazz the Java class of objects that can be logged + */ + protected ClassSpecificLogger(Class clazz) { + this.m_clazz = clazz; + } + + /** + * Updates an object's fields in a data log. + * + * @param dataLogger the logger to update + * @param object the object to update in the log + */ + protected abstract void update(DataLogger dataLogger, T object); + + /** + * Attempts to update the data log. Will do nothing if the logger is {@link #disable() disabled}. + * + * @param dataLogger the logger to log data to + * @param object the data object to log + * @param errorHandler the handler to use if logging raised an exception + */ + @SuppressWarnings("PMD.AvoidCatchingGenericException") + public final void tryUpdate(DataLogger dataLogger, T object, ErrorHandler errorHandler) { + if (m_disabled) { + return; + } + + try { + update(dataLogger, object); + } catch (Exception e) { + errorHandler.handle(e, this); + } + } + + /** + * Checks if this logger has been disabled. + * + * @return true if this logger has been disabled by {@link #disable()}, false if not + */ + public final boolean isDisabled() { + return m_disabled; + } + + /** Disables this logger. Any log calls made while disabled will be ignored. */ + public final void disable() { + m_disabled = true; + } + + /** Reenables this logger after being disabled. Has no effect if the logger is already enabled. */ + public final void reenable() { + m_disabled = false; + } + + /** + * Gets the type of the data this logger accepts. + * + * @return the logged data type + */ + public final Class getLoggedType() { + return m_clazz; + } + + /** + * Logs a sendable type. + * + * @param dataLogger the logger to log data into + * @param sendable the sendable object to log + */ + protected void logSendable(DataLogger dataLogger, Sendable sendable) { + if (sendable == null) { + return; + } + + var builder = m_sendables.computeIfAbsent(sendable, s -> { + var b = new LogBackedSendableBuilder(dataLogger); + s.initSendable(b); + return b; + }); + builder.update(); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/DataLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/DataLogger.java new file mode 100644 index 0000000..06cd6f1 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/DataLogger.java @@ -0,0 +1,228 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import edu.wpi.first.util.struct.Struct; +import java.util.Collection; + +/** A data logger is a generic interface for logging discrete data points. */ +public interface DataLogger { + /** + * Creates a data logger that logs to multiple backends at once. Data reads will still only occur + * once; data is passed to all composed loggers at once. + * + * @param loggers the loggers to compose together + * @return the multi logger + */ + static DataLogger multi(DataLogger... loggers) { + return new MultiLogger(loggers); + } + + /** + * Creates a lazy version of this logger. A lazy logger will only log data to a field when its + * value changes, which can help keep file size and bandwidth usage in check. However, there is an + * additional CPU and memory overhead associated with tracking the current value of every logged + * entry. The most surefire way to reduce CPU and memory usage associated with logging is to log + * fewer things - which can be done by opting out of logging unnecessary data or increasing the + * minimum logged importance level in the Epilogue configuration. + * + * @return the lazy logger + */ + default DataLogger lazy() { + return new LazyLogger(this); + } + + /** + * Gets a logger that can be used to log nested data underneath a specific path. + * + * @param path the path to use for logging nested data under + * @return the sub logger + */ + DataLogger getSubLogger(String path); + + /** + * Logs a 32-bit integer data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, int value); + + /** + * Logs a 64-bit integer data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, long value); + + /** + * Logs a 32-bit floating point data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, float value); + + /** + * Logs a 64-bit floating point data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, double value); + + /** + * Logs a boolean data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, boolean value); + + /** + * Logs a raw byte array data point. NOTE: serializable data should be logged + * using {@link #log(String, Object, Struct)}. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, byte[] value); + + /** + * Logs a 32-bit integer array data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, int[] value); + + /** + * Logs a 64-bit integer array data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, long[] value); + + /** + * Logs a 32-bit floating point array data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, float[] value); + + /** + * Logs a 64-bit floating point array data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, double[] value); + + /** + * Logs a boolean array data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, boolean[] value); + + /** + * Logs a text data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, String value); + + /** + * Logs a string array data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + void log(String identifier, String[] value); + + /** + * Logs a collection of strings data point. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + */ + default void log(String identifier, Collection value) { + log(identifier, value.toArray(String[]::new)); + } + + /** + * Logs a struct-serializable object. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + * @param struct the struct to use to serialize the data + * @param the serializable type + */ + void log(String identifier, S value, Struct struct); + + /** + * Logs an array of struct-serializable objects. + * + * @param identifier the identifier of the data point + * @param value the value of the data point + * @param struct the struct to use to serialize the objects + * @param the serializable type + */ + void log(String identifier, S[] value, Struct struct); + + /** + * Logs a collection of struct-serializable objects. + * + * @param identifier the identifier of the data + * @param value the collection of objects to log + * @param struct the struct to use to serialize the objects + * @param the serializable type + */ + default void log(String identifier, Collection value, Struct struct) { + @SuppressWarnings("unchecked") + S[] array = (S[]) value.toArray(Object[]::new); + log(identifier, array, struct); + } + + /** + * Logs a measurement's value in terms of its base unit. + * + * @param identifier the identifier of the data field + * @param value the new value of the data field + */ + default void log(String identifier, Measure value) { + log(identifier, value.baseUnitMagnitude()); + } + + /** + * Logs a measurement's value in terms of another unit. + * + * @param identifier the identifier of the data field + * @param value the new value of the data field + * @param unit the unit to log the measurement in + * @param the dimension of the unit + */ + default > void log(String identifier, Measure value, U unit) { + log(identifier, value.in(unit)); + } + + /** + * Logs an enum value. The value will appear as a string entry using the name of the enum. + * + * @param identifier the identifier of the data field + * @param value the new value of the data field + */ + default void log(String identifier, Enum value) { + log(identifier, value.name()); + } + // TODO: Add default methods to support common no-struct no-sendable types like joysticks? +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileLogger.java new file mode 100644 index 0000000..93de587 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/FileLogger.java @@ -0,0 +1,144 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.util.datalog.BooleanArrayLogEntry; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DataLogEntry; +import edu.wpi.first.util.datalog.DoubleArrayLogEntry; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.FloatArrayLogEntry; +import edu.wpi.first.util.datalog.FloatLogEntry; +import edu.wpi.first.util.datalog.IntegerArrayLogEntry; +import edu.wpi.first.util.datalog.IntegerLogEntry; +import edu.wpi.first.util.datalog.RawLogEntry; +import edu.wpi.first.util.datalog.StringArrayLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.datalog.StructArrayLogEntry; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.util.struct.Struct; +import java.util.HashMap; +import java.util.Map; +import java.util.function.BiFunction; + +/** A data logger implementation that saves information to a WPILib {@link DataLog} file on disk. */ +public class FileLogger implements DataLogger { + + private final DataLog m_dataLog; + private final Map m_entries = new HashMap<>(); + private final Map m_subLoggers = new HashMap<>(); + + /** + * Creates a new file logger. + * + * @param dataLog the data log to save data to + */ + public FileLogger(DataLog dataLog) { + this.m_dataLog = requireNonNullParam(dataLog, "dataLog", "FileLogger"); + } + + @Override + public DataLogger getSubLogger(String path) { + return m_subLoggers.computeIfAbsent(path, k -> new SubLogger(k, this)); + } + + @SuppressWarnings("unchecked") + private E getEntry(String identifier, BiFunction ctor) { + if (m_entries.get(identifier) != null) { + return (E) m_entries.get(identifier); + } + + var entry = ctor.apply(m_dataLog, identifier); + m_entries.put(identifier, entry); + return entry; + } + + @Override + public void log(String identifier, int value) { + getEntry(identifier, IntegerLogEntry::new).append(value); + } + + @Override + public void log(String identifier, long value) { + getEntry(identifier, IntegerLogEntry::new).append(value); + } + + @Override + public void log(String identifier, float value) { + getEntry(identifier, FloatLogEntry::new).append(value); + } + + @Override + public void log(String identifier, double value) { + getEntry(identifier, DoubleLogEntry::new).append(value); + } + + @Override + public void log(String identifier, boolean value) { + getEntry(identifier, BooleanLogEntry::new).append(value); + } + + @Override + public void log(String identifier, byte[] value) { + getEntry(identifier, RawLogEntry::new).append(value); + } + + @Override + @SuppressWarnings("PMD.UnnecessaryCastRule") + public void log(String identifier, int[] value) { + long[] widened = new long[value.length]; + for (int i = 0; i < value.length; i++) { + widened[i] = (long) value[i]; + } + getEntry(identifier, IntegerArrayLogEntry::new).append(widened); + } + + @Override + public void log(String identifier, long[] value) { + getEntry(identifier, IntegerArrayLogEntry::new).append(value); + } + + @Override + public void log(String identifier, float[] value) { + getEntry(identifier, FloatArrayLogEntry::new).append(value); + } + + @Override + public void log(String identifier, double[] value) { + getEntry(identifier, DoubleArrayLogEntry::new).append(value); + } + + @Override + public void log(String identifier, boolean[] value) { + getEntry(identifier, BooleanArrayLogEntry::new).append(value); + } + + @Override + public void log(String identifier, String value) { + getEntry(identifier, StringLogEntry::new).append(value); + } + + @Override + public void log(String identifier, String[] value) { + getEntry(identifier, StringArrayLogEntry::new).append(value); + } + + @Override + @SuppressWarnings("unchecked") + public void log(String identifier, S value, Struct struct) { + m_dataLog.addSchema(struct); + getEntry(identifier, (log, k) -> StructLogEntry.create(log, k, struct)).append(value); + } + + @Override + @SuppressWarnings("unchecked") + public void log(String identifier, S[] value, Struct struct) { + m_dataLog.addSchema(struct); + getEntry(identifier, (log, k) -> StructArrayLogEntry.create(log, k, struct)).append(value); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyLogger.java new file mode 100644 index 0000000..6627ff5 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LazyLogger.java @@ -0,0 +1,241 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.util.struct.Struct; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.Objects; + +/** + * A data logger implementation that only logs data when it changes. Useful for keeping bandwidth + * and file sizes down. However, because it still needs to check that data has changed, it cannot + * avoid expensive sensor reads. + */ +public class LazyLogger implements DataLogger { + + private final DataLogger m_logger; + + // Keep a record of the most recent value written to each entry + // Note that this may duplicate a lot of data, and will box primitives. + private final Map m_previousValues = new HashMap<>(); + private final Map m_subLoggers = new HashMap<>(); + + /** + * Creates a new lazy logger wrapper around another logger. + * + * @param logger the logger to delegate to + */ + public LazyLogger(DataLogger logger) { + this.m_logger = logger; + } + + @Override + public DataLogger lazy() { + // Already lazy, don't need to wrap it again + return this; + } + + @Override + public DataLogger getSubLogger(String path) { + return m_subLoggers.computeIfAbsent(path, k -> new SubLogger(k, this)); + } + + @Override + public void log(String identifier, int value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof Integer oldValue && oldValue == value) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, long value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof Long oldValue && oldValue == value) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, float value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof Float oldValue && oldValue == value) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, double value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof Double oldValue && oldValue == value) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, boolean value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof Boolean oldValue && oldValue == value) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, byte[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof byte[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, int[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof int[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, long[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof long[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, float[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof float[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, double[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof double[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, boolean[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof boolean[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, String value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof String oldValue && oldValue.equals(value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, String[] value) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof String[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value); + } + + @Override + public void log(String identifier, S value, Struct struct) { + var previous = m_previousValues.get(identifier); + + if (Objects.equals(previous, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value, struct); + } + + @Override + public void log(String identifier, S[] value, Struct struct) { + var previous = m_previousValues.get(identifier); + + if (previous instanceof Object[] oldValue && Arrays.equals(oldValue, value)) { + // no change + return; + } + + m_previousValues.put(identifier, value); + m_logger.log(identifier, value, struct); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LogBackedSendableBuilder.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LogBackedSendableBuilder.java new file mode 100644 index 0000000..3f3d9bb --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/LogBackedSendableBuilder.java @@ -0,0 +1,207 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.util.function.BooleanConsumer; +import edu.wpi.first.util.function.FloatConsumer; +import edu.wpi.first.util.function.FloatSupplier; +import edu.wpi.first.util.sendable.SendableBuilder; +import java.util.ArrayList; +import java.util.Collection; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.LongConsumer; +import java.util.function.LongSupplier; +import java.util.function.Supplier; + +/** A sendable builder implementation that sends data to a {@link DataLogger}. */ +@SuppressWarnings("PMD.CouplingBetweenObjects") // most methods simply delegate to the logger +public class LogBackedSendableBuilder implements SendableBuilder { + + private final DataLogger m_logger; + private final Collection m_updates = new ArrayList<>(); + + /** + * Creates a new sendable builder that delegates writes to an underlying data logger. + * + * @param logger the data logger to write the sendable data to + */ + public LogBackedSendableBuilder(DataLogger logger) { + this.m_logger = logger; + } + + @Override + public void setSmartDashboardType(String type) { + m_logger.log(".type", type); + } + + @Override + public void setActuator(boolean value) { + // ignore + } + + @Override + public void setSafeState(Runnable func) { + // ignore + } + + @Override + public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) { + m_updates.add(() -> m_logger.log(key, getter.getAsBoolean())); + } + + @Override + public void publishConstBoolean(String key, boolean value) { + m_logger.log(key, value); + } + + @Override + public void addIntegerProperty(String key, LongSupplier getter, LongConsumer setter) { + m_updates.add(() -> m_logger.log(key, getter.getAsLong())); + } + + @Override + public void publishConstInteger(String key, long value) { + m_logger.log(key, value); + } + + @Override + public void addFloatProperty(String key, FloatSupplier getter, FloatConsumer setter) { + m_updates.add(() -> m_logger.log(key, getter.getAsFloat())); + } + + @Override + public void publishConstFloat(String key, float value) { + m_logger.log(key, value); + } + + @Override + public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter) { + m_updates.add(() -> m_logger.log(key, getter.getAsDouble())); + } + + @Override + public void publishConstDouble(String key, double value) { + m_logger.log(key, value); + } + + @Override + public void addStringProperty(String key, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstString(String key, String value) { + m_logger.log(key, value); + } + + @Override + public void addBooleanArrayProperty(String key, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstBooleanArray(String key, boolean[] value) { + m_logger.log(key, value); + } + + @Override + public void addIntegerArrayProperty(String key, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstIntegerArray(String key, long[] value) { + m_logger.log(key, value); + } + + @Override + public void addFloatArrayProperty(String key, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstFloatArray(String key, float[] value) { + m_logger.log(key, value); + } + + @Override + public void addDoubleArrayProperty(String key, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstDoubleArray(String key, double[] value) { + m_logger.log(key, value); + } + + @Override + public void addStringArrayProperty(String key, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstStringArray(String key, String[] value) { + m_logger.log(key, value); + } + + @Override + public void addRawProperty(String key, String typeString, Supplier getter, Consumer setter) { + if (getter != null) { + m_updates.add(() -> m_logger.log(key, getter.get())); + } + } + + @Override + public void publishConstRaw(String key, String typeString, byte[] value) { + m_logger.log(key, value); + } + + @Override + public BackendKind getBackendKind() { + return BackendKind.kUnknown; + } + + @Override + public boolean isPublished() { + return true; + } + + @Override + public void update() { + for (Runnable update : m_updates) { + update.run(); + } + } + + @Override + public void clearProperties() { + m_updates.clear(); + } + + @Override + public void addCloseable(AutoCloseable closeable) { + // Ignore + } + + @Override + public void close() throws Exception { + clearProperties(); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiLogger.java new file mode 100644 index 0000000..dd980b3 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/MultiLogger.java @@ -0,0 +1,135 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.util.struct.Struct; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * A data logger implementation that delegates to other loggers. Helpful for simultaneous logging to + * multiple data stores at once. + */ +public class MultiLogger implements DataLogger { + + private final List m_loggers; + private final Map m_subLoggers = new HashMap<>(); + + // Use DataLogger.multi(...) instead of instantiation directly + MultiLogger(DataLogger... loggers) { + this.m_loggers = List.of(loggers); + } + + @Override + public DataLogger getSubLogger(String path) { + return m_subLoggers.computeIfAbsent(path, k -> new SubLogger(k, this)); + } + + @Override + public void log(String identifier, int value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, long value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, float value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, double value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, boolean value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, byte[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, int[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, long[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, float[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, double[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, boolean[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, String value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, String[] value) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value); + } + } + + @Override + public void log(String identifier, S value, Struct struct) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value, struct); + } + } + + @Override + public void log(String identifier, S[] value, Struct struct) { + for (DataLogger logger : m_loggers) { + logger.log(identifier, value, struct); + } + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTDataLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTDataLogger.java new file mode 100644 index 0000000..4cccf1b --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NTDataLogger.java @@ -0,0 +1,153 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.networktables.BooleanArrayPublisher; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.FloatArrayPublisher; +import edu.wpi.first.networktables.FloatPublisher; +import edu.wpi.first.networktables.IntegerArrayPublisher; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.Publisher; +import edu.wpi.first.networktables.RawPublisher; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.util.struct.Struct; +import java.util.HashMap; +import java.util.Map; + +/** + * A data logger implementation that sends data over network tables. Be careful when using this, + * since sending too much data may cause bandwidth or CPU starvation. + */ +public class NTDataLogger implements DataLogger { + + private final NetworkTableInstance m_nt; + + private final Map m_publishers = new HashMap<>(); + private final Map m_subLoggers = new HashMap<>(); + + /** + * Creates a data logger that sends information to NetworkTables. + * + * @param nt the NetworkTable instance to use to send data to + */ + public NTDataLogger(NetworkTableInstance nt) { + this.m_nt = nt; + } + + @Override + public DataLogger getSubLogger(String path) { + return m_subLoggers.computeIfAbsent(path, k -> new SubLogger(k, this)); + } + + @Override + public void log(String identifier, int value) { + ((IntegerPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerTopic(k).publish())).set( + value + ); + } + + @Override + public void log(String identifier, long value) { + ((IntegerPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerTopic(k).publish())).set( + value + ); + } + + @Override + public void log(String identifier, float value) { + ((FloatPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getFloatTopic(k).publish())).set(value); + } + + @Override + public void log(String identifier, double value) { + ((DoublePublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getDoubleTopic(k).publish())).set(value); + } + + @Override + public void log(String identifier, boolean value) { + ((BooleanPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getBooleanTopic(k).publish())).set( + value + ); + } + + @Override + public void log(String identifier, byte[] value) { + ((RawPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getRawTopic(k).publish("raw"))).set(value); + } + + @Override + @SuppressWarnings("PMD.UnnecessaryCastRule") + public void log(String identifier, int[] value) { + // NT backend only supports int64[], so we have to manually widen to 64 bits before sending + long[] widened = new long[value.length]; + + for (int i = 0; i < value.length; i++) { + widened[i] = (long) value[i]; + } + + ((IntegerArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerArrayTopic(k).publish() + )).set(widened); + } + + @Override + public void log(String identifier, long[] value) { + ((IntegerArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getIntegerArrayTopic(k).publish() + )).set(value); + } + + @Override + public void log(String identifier, float[] value) { + ((FloatArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getFloatArrayTopic(k).publish())).set( + value + ); + } + + @Override + public void log(String identifier, double[] value) { + ((DoubleArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getDoubleArrayTopic(k).publish() + )).set(value); + } + + @Override + public void log(String identifier, boolean[] value) { + ((BooleanArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getBooleanArrayTopic(k).publish() + )).set(value); + } + + @Override + public void log(String identifier, String value) { + ((StringPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getStringTopic(k).publish())).set(value); + } + + @Override + public void log(String identifier, String[] value) { + ((StringArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getStringArrayTopic(k).publish() + )).set(value); + } + + @Override + @SuppressWarnings("unchecked") + public void log(String identifier, S value, Struct struct) { + m_nt.addSchema(struct); + ((StructPublisher) m_publishers.computeIfAbsent(identifier, k -> m_nt.getStructTopic(k, struct).publish() + )).set(value); + } + + @Override + @SuppressWarnings("unchecked") + public void log(String identifier, S[] value, Struct struct) { + m_nt.addSchema(struct); + ((StructArrayPublisher) m_publishers.computeIfAbsent(identifier, k -> + m_nt.getStructArrayTopic(k, struct).publish() + )).set(value); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullLogger.java new file mode 100644 index 0000000..c5b2648 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/NullLogger.java @@ -0,0 +1,63 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.util.struct.Struct; + +/** Null data logger implementation that logs nothing. */ +public class NullLogger implements DataLogger { + + @Override + public DataLogger getSubLogger(String path) { + // Since a sublogger would still log nothing and has no state, we can just return the same + // null-logging implementation + return this; + } + + @Override + public void log(String identifier, int value) {} + + @Override + public void log(String identifier, long value) {} + + @Override + public void log(String identifier, float value) {} + + @Override + public void log(String identifier, double value) {} + + @Override + public void log(String identifier, boolean value) {} + + @Override + public void log(String identifier, byte[] value) {} + + @Override + public void log(String identifier, int[] value) {} + + @Override + public void log(String identifier, long[] value) {} + + @Override + public void log(String identifier, float[] value) {} + + @Override + public void log(String identifier, double[] value) {} + + @Override + public void log(String identifier, boolean[] value) {} + + @Override + public void log(String identifier, String value) {} + + @Override + public void log(String identifier, String[] value) {} + + @Override + public void log(String identifier, S value, Struct struct) {} + + @Override + public void log(String identifier, S[] value, Struct struct) {} +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/SubLogger.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/SubLogger.java new file mode 100644 index 0000000..03165e4 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/SubLogger.java @@ -0,0 +1,116 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging; + +import edu.wpi.first.util.struct.Struct; +import java.util.HashMap; +import java.util.Map; + +/** + * A data logger that logs to an underlying logger, prepending all logged data with a specific + * prefix. Useful for logging nested data structures. + */ +public class SubLogger implements DataLogger { + + private final String m_prefix; + private final DataLogger m_impl; + private final Map m_subLoggers = new HashMap<>(); + + /** + * Creates a new sublogger underneath another logger. + * + * @param prefix the prefix to append to all data logged in the sublogger + * @param impl the data logger to log to + */ + public SubLogger(String prefix, DataLogger impl) { + // Add a trailing slash if not already present + if (prefix.endsWith("/")) { + this.m_prefix = prefix; + } else { + this.m_prefix = prefix + "/"; + } + this.m_impl = impl; + } + + @Override + public DataLogger getSubLogger(String path) { + return m_subLoggers.computeIfAbsent(path, k -> new SubLogger(k, this)); + } + + @Override + public void log(String identifier, int value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, long value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, float value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, double value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, boolean value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, byte[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, int[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, long[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, float[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, double[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, boolean[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, String value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, String[] value) { + m_impl.log(m_prefix + identifier, value); + } + + @Override + public void log(String identifier, S value, Struct struct) { + m_impl.log(m_prefix + identifier, value, struct); + } + + @Override + public void log(String identifier, S[] value, Struct struct) { + m_impl.log(m_prefix + identifier, value, struct); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/CrashOnError.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/CrashOnError.java new file mode 100644 index 0000000..7a76d94 --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/CrashOnError.java @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging.errors; + +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + +/** + * An error handler implementation that will throw an exception if logging raised an exception. This + * is useful when running code in simulation or in JUnit tests to quickly identify errors in your + * code. + */ +public class CrashOnError implements ErrorHandler { + + @Override + public void handle(Throwable exception, ClassSpecificLogger logger) { + throw new RuntimeException( + "[EPILOGUE] An error occurred while logging an instance of " + logger.getLoggedType().getName(), + exception + ); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorHandler.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorHandler.java new file mode 100644 index 0000000..782c4dc --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorHandler.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging.errors; + +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + +/** + * An error handler is used by the Logged framework to catch and process any errors that occur + * during the logging process. Different handlers can be used in different operating modes, such as + * crashing in simulation to identify errors before they make it to a robot, or automatically + * disabling loggers if they encounter too many errors on the field to let the robot keep running + * while playing a match. + */ +@FunctionalInterface +public interface ErrorHandler { + /** + * Handles an exception that arose while logging. + * + * @param exception the exception that occurred + * @param logger the logger that was being processed that caused the error to occur + */ + void handle(Throwable exception, ClassSpecificLogger logger); + + /** + * Creates an error handler that will immediately re-throw an exception and cause robot code to + * exit. This is particularly useful when running in simulation or JUnit tests to identify errors + * quickly and safely. + * + * @return the error handler + */ + static ErrorHandler crashOnError() { + return new CrashOnError(); + } + + /** + * Creates an error handler that will print error messages to the console output, but otherwise + * allow logging to continue in the future. This can be helpful when errors occur only rarely and + * you don't want your robot program to crash or disable future logging. + * + * @return the error handler + */ + static ErrorHandler printErrorMessages() { + return new ErrorPrinter(); + } + + /** + * Creates an error handler that will automatically disable a logger if it encounters too many + * errors. Only the error-prone logger(s) will be disabled; loggers that have not encountered any + * errors, or encountered fewer than the limit, will continue to be used. Disabled loggers can be + * reset by calling {@link LoggerDisabler#reset()} on the handler. + * + * @param maximumPermissibleErrors the maximum number of errors that a logger is permitted to + * encounter before being disabled. + * @return the error handler + */ + static LoggerDisabler disabling(int maximumPermissibleErrors) { + return LoggerDisabler.forLimit(maximumPermissibleErrors); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorPrinter.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorPrinter.java new file mode 100644 index 0000000..d00f2cb --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/ErrorPrinter.java @@ -0,0 +1,21 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging.errors; + +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; + +/** An error handler implementation that prints error information to the console. */ +public class ErrorPrinter implements ErrorHandler { + + @Override + public void handle(Throwable exception, ClassSpecificLogger logger) { + System.err.println( + "[EPILOGUE] An error occurred while logging an instance of " + + logger.getLoggedType().getName() + + ": " + + exception.getMessage() + ); + } +} diff --git a/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/LoggerDisabler.java b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/LoggerDisabler.java new file mode 100644 index 0000000..b45ea5e --- /dev/null +++ b/epilogue/epilogue-runtime/src/main/java/edu/wpi/first/epilogue/logging/errors/LoggerDisabler.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.epilogue.logging.errors; + +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import java.util.HashMap; +import java.util.Map; + +/** + * An error handler that disables loggers after too many exceptions are raised. Useful when playing + * in matches, where data logging is less important than reliable control. Setting the threshold to + * ≤0 will cause any logger that encounters an exception whilst logging to immediately be disabled. + * Setting to higher values means your program is more tolerant of errors, but takes longer to + * disable, and therefore may have more sets of partial or incomplete data and may have more CPU + * overhead due to the cost of throwing exceptions. + */ +public class LoggerDisabler implements ErrorHandler { + + private final int m_threshold; + private final Map, Integer> m_errorCounts = new HashMap<>(); + + /** + * Creates a new logger-disabling error handler. + * + * @param threshold how many errors any one logger is allowed to encounter before it is disabled. + */ + public LoggerDisabler(int threshold) { + this.m_threshold = threshold; + } + + /** + * Creates a disabler that kicks in after a logger raises more than {@code threshold} exceptions. + * + * @param threshold the threshold value for the maximum number of exceptions loggers are permitted + * to encounter before they are disabled + * @return the disabler + */ + public static LoggerDisabler forLimit(int threshold) { + return new LoggerDisabler(threshold); + } + + @Override + public void handle(Throwable exception, ClassSpecificLogger logger) { + var errorCount = m_errorCounts.getOrDefault(logger, 0) + 1; + m_errorCounts.put(logger, errorCount); + + if (errorCount > m_threshold) { + logger.disable(); + System.err.println( + "[EPILOGUE] Too many errors detected in " + + logger.getClass().getName() + + " (maximum allowed: " + + m_threshold + + "). The most recent error follows:" + ); + System.err.println(exception.getMessage()); + exception.printStackTrace(System.err); + } + } + + /** Resets all error counts and reenables all loggers. */ + public void reset() { + for (var logger : m_errorCounts.keySet()) { + // Safe. This is a no-op on loggers that are already enabled + logger.reenable(); + } + m_errorCounts.clear(); + } +} diff --git a/epilogue/gradle/wrapper/gradle-wrapper.jar b/epilogue/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..d64cd4917707c1f8861d8cb53dd15194d4248596 GIT binary patch literal 43462 zcma&NWl&^owk(X(xVyW%ySuwf;qI=D6|RlDJ2cR^yEKh!@I- zp9QeisK*rlxC>+~7Dk4IxIRsKBHqdR9b3+fyL=ynHmIDe&|>O*VlvO+%z5;9Z$|DJ zb4dO}-R=MKr^6EKJiOrJdLnCJn>np?~vU-1sSFgPu;pthGwf}bG z(1db%xwr#x)r+`4AGu$j7~u2MpVs3VpLp|mx&;>`0p0vH6kF+D2CY0fVdQOZ@h;A` z{infNyvmFUiu*XG}RNMNwXrbec_*a3N=2zJ|Wh5z* z5rAX$JJR{#zP>KY**>xHTuw?|-Rg|o24V)74HcfVT;WtQHXlE+_4iPE8QE#DUm%x0 zEKr75ur~W%w#-My3Tj`hH6EuEW+8K-^5P62$7Sc5OK+22qj&Pd1;)1#4tKihi=~8C zHiQSst0cpri6%OeaR`PY>HH_;CPaRNty%WTm4{wDK8V6gCZlG@U3$~JQZ;HPvDJcT1V{ z?>H@13MJcCNe#5z+MecYNi@VT5|&UiN1D4ATT+%M+h4c$t;C#UAs3O_q=GxK0}8%8 z8J(_M9bayxN}69ex4dzM_P3oh@ZGREjVvn%%r7=xjkqxJP4kj}5tlf;QosR=%4L5y zWhgejO=vao5oX%mOHbhJ8V+SG&K5dABn6!WiKl{|oPkq(9z8l&Mm%(=qGcFzI=eLu zWc_oCLyf;hVlB@dnwY98?75B20=n$>u3b|NB28H0u-6Rpl((%KWEBOfElVWJx+5yg z#SGqwza7f}$z;n~g%4HDU{;V{gXIhft*q2=4zSezGK~nBgu9-Q*rZ#2f=Q}i2|qOp z!!y4p)4o=LVUNhlkp#JL{tfkhXNbB=Ox>M=n6soptJw-IDI|_$is2w}(XY>a=H52d z3zE$tjPUhWWS+5h=KVH&uqQS=$v3nRs&p$%11b%5qtF}S2#Pc`IiyBIF4%A!;AVoI zXU8-Rpv!DQNcF~(qQnyyMy=-AN~U>#&X1j5BLDP{?K!%h!;hfJI>$mdLSvktEr*89 zdJHvby^$xEX0^l9g$xW-d?J;L0#(`UT~zpL&*cEh$L|HPAu=P8`OQZV!-}l`noSp_ zQ-1$q$R-gDL)?6YaM!=8H=QGW$NT2SeZlb8PKJdc=F-cT@j7Xags+Pr*jPtlHFnf- zh?q<6;)27IdPc^Wdy-mX%2s84C1xZq9Xms+==F4);O`VUASmu3(RlgE#0+#giLh-& zcxm3_e}n4{%|X zJp{G_j+%`j_q5}k{eW&TlP}J2wtZ2^<^E(O)4OQX8FDp6RJq!F{(6eHWSD3=f~(h} zJXCf7=r<16X{pHkm%yzYI_=VDP&9bmI1*)YXZeB}F? z(%QsB5fo*FUZxK$oX~X^69;x~j7ms8xlzpt-T15e9}$4T-pC z6PFg@;B-j|Ywajpe4~bk#S6(fO^|mm1hKOPfA%8-_iGCfICE|=P_~e;Wz6my&)h_~ zkv&_xSAw7AZ%ThYF(4jADW4vg=oEdJGVOs>FqamoL3Np8>?!W#!R-0%2Bg4h?kz5I zKV-rKN2n(vUL%D<4oj@|`eJ>0i#TmYBtYmfla;c!ATW%;xGQ0*TW@PTlGG><@dxUI zg>+3SiGdZ%?5N=8uoLA|$4isK$aJ%i{hECP$bK{J#0W2gQ3YEa zZQ50Stn6hqdfxJ*9#NuSLwKFCUGk@c=(igyVL;;2^wi4o30YXSIb2g_ud$ zgpCr@H0qWtk2hK8Q|&wx)}4+hTYlf;$a4#oUM=V@Cw#!$(nOFFpZ;0lc!qd=c$S}Z zGGI-0jg~S~cgVT=4Vo)b)|4phjStD49*EqC)IPwyeKBLcN;Wu@Aeph;emROAwJ-0< z_#>wVm$)ygH|qyxZaet&(Vf%pVdnvKWJn9`%DAxj3ot;v>S$I}jJ$FLBF*~iZ!ZXE zkvui&p}fI0Y=IDX)mm0@tAd|fEHl~J&K}ZX(Mm3cm1UAuwJ42+AO5@HwYfDH7ipIc zmI;1J;J@+aCNG1M`Btf>YT>~c&3j~Qi@Py5JT6;zjx$cvOQW@3oQ>|}GH?TW-E z1R;q^QFjm5W~7f}c3Ww|awg1BAJ^slEV~Pk`Kd`PS$7;SqJZNj->it4DW2l15}xP6 zoCl$kyEF%yJni0(L!Z&14m!1urXh6Btj_5JYt1{#+H8w?5QI%% zo-$KYWNMJVH?Hh@1n7OSu~QhSswL8x0=$<8QG_zepi_`y_79=nK=_ZP_`Em2UI*tyQoB+r{1QYZCpb?2OrgUw#oRH$?^Tj!Req>XiE#~B|~ z+%HB;=ic+R@px4Ld8mwpY;W^A%8%l8$@B@1m5n`TlKI6bz2mp*^^^1mK$COW$HOfp zUGTz-cN9?BGEp}5A!mDFjaiWa2_J2Iq8qj0mXzk; z66JBKRP{p%wN7XobR0YjhAuW9T1Gw3FDvR5dWJ8ElNYF94eF3ebu+QwKjtvVu4L zI9ip#mQ@4uqVdkl-TUQMb^XBJVLW(-$s;Nq;@5gr4`UfLgF$adIhd?rHOa%D);whv z=;krPp~@I+-Z|r#s3yCH+c1US?dnm+C*)r{m+86sTJusLdNu^sqLrfWed^ndHXH`m zd3#cOe3>w-ga(Dus_^ppG9AC>Iq{y%%CK+Cro_sqLCs{VLuK=dev>OL1dis4(PQ5R zcz)>DjEkfV+MO;~>VUlYF00SgfUo~@(&9$Iy2|G0T9BSP?&T22>K46D zL*~j#yJ?)^*%J3!16f)@Y2Z^kS*BzwfAQ7K96rFRIh>#$*$_Io;z>ux@}G98!fWR@ zGTFxv4r~v)Gsd|pF91*-eaZ3Qw1MH$K^7JhWIdX%o$2kCbvGDXy)a?@8T&1dY4`;L z4Kn+f%SSFWE_rpEpL9bnlmYq`D!6F%di<&Hh=+!VI~j)2mfil03T#jJ_s?}VV0_hp z7T9bWxc>Jm2Z0WMU?`Z$xE74Gu~%s{mW!d4uvKCx@WD+gPUQ zV0vQS(Ig++z=EHN)BR44*EDSWIyT~R4$FcF*VEY*8@l=218Q05D2$|fXKFhRgBIEE zdDFB}1dKkoO^7}{5crKX!p?dZWNz$m>1icsXG2N+((x0OIST9Zo^DW_tytvlwXGpn zs8?pJXjEG;T@qrZi%#h93?FP$!&P4JA(&H61tqQi=opRzNpm zkrG}$^t9&XduK*Qa1?355wd8G2CI6QEh@Ua>AsD;7oRUNLPb76m4HG3K?)wF~IyS3`fXuNM>${?wmB zpVz;?6_(Fiadfd{vUCBM*_kt$+F3J+IojI;9L(gc9n3{sEZyzR9o!_mOwFC#tQ{Q~ zP3-`#uK#tP3Q7~Q;4H|wjZHO8h7e4IuBxl&vz2w~D8)w=Wtg31zpZhz%+kzSzL*dV zwp@{WU4i;hJ7c2f1O;7Mz6qRKeASoIv0_bV=i@NMG*l<#+;INk-^`5w@}Dj~;k=|}qM1vq_P z|GpBGe_IKq|LNy9SJhKOQ$c=5L{Dv|Q_lZl=-ky*BFBJLW9&y_C|!vyM~rQx=!vun z?rZJQB5t}Dctmui5i31C_;_}CEn}_W%>oSXtt>@kE1=JW*4*v4tPp;O6 zmAk{)m!)}34pTWg8{i>($%NQ(Tl;QC@J@FfBoc%Gr&m560^kgSfodAFrIjF}aIw)X zoXZ`@IsMkc8_=w%-7`D6Y4e*CG8k%Ud=GXhsTR50jUnm+R*0A(O3UKFg0`K;qp1bl z7``HN=?39ic_kR|^R^~w-*pa?Vj#7|e9F1iRx{GN2?wK!xR1GW!qa=~pjJb-#u1K8 zeR?Y2i-pt}yJq;SCiVHODIvQJX|ZJaT8nO+(?HXbLefulKKgM^B(UIO1r+S=7;kLJ zcH}1J=Px2jsh3Tec&v8Jcbng8;V-`#*UHt?hB(pmOipKwf3Lz8rG$heEB30Sg*2rx zV<|KN86$soN(I!BwO`1n^^uF2*x&vJ$2d$>+`(romzHP|)K_KkO6Hc>_dwMW-M(#S zK(~SiXT1@fvc#U+?|?PniDRm01)f^#55;nhM|wi?oG>yBsa?~?^xTU|fX-R(sTA+5 zaq}-8Tx7zrOy#3*JLIIVsBmHYLdD}!0NP!+ITW+Thn0)8SS!$@)HXwB3tY!fMxc#1 zMp3H?q3eD?u&Njx4;KQ5G>32+GRp1Ee5qMO0lZjaRRu&{W<&~DoJNGkcYF<5(Ab+J zgO>VhBl{okDPn78<%&e2mR{jwVCz5Og;*Z;;3%VvoGo_;HaGLWYF7q#jDX=Z#Ml`H z858YVV$%J|e<1n`%6Vsvq7GmnAV0wW4$5qQ3uR@1i>tW{xrl|ExywIc?fNgYlA?C5 zh$ezAFb5{rQu6i7BSS5*J-|9DQ{6^BVQ{b*lq`xS@RyrsJN?-t=MTMPY;WYeKBCNg z^2|pN!Q^WPJuuO4!|P@jzt&tY1Y8d%FNK5xK(!@`jO2aEA*4 zkO6b|UVBipci?){-Ke=+1;mGlND8)6+P;8sq}UXw2hn;fc7nM>g}GSMWu&v&fqh

iViYT=fZ(|3Ox^$aWPp4a8h24tD<|8-!aK0lHgL$N7Efw}J zVIB!7=T$U`ao1?upi5V4Et*-lTG0XvExbf!ya{cua==$WJyVG(CmA6Of*8E@DSE%L z`V^$qz&RU$7G5mg;8;=#`@rRG`-uS18$0WPN@!v2d{H2sOqP|!(cQ@ zUHo!d>>yFArLPf1q`uBvY32miqShLT1B@gDL4XoVTK&@owOoD)OIHXrYK-a1d$B{v zF^}8D3Y^g%^cnvScOSJR5QNH+BI%d|;J;wWM3~l>${fb8DNPg)wrf|GBP8p%LNGN# z3EaIiItgwtGgT&iYCFy9-LG}bMI|4LdmmJt@V@% zb6B)1kc=T)(|L@0;wr<>=?r04N;E&ef+7C^`wPWtyQe(*pD1pI_&XHy|0gIGHMekd zF_*M4yi6J&Z4LQj65)S zXwdM{SwUo%3SbPwFsHgqF@V|6afT|R6?&S;lw=8% z3}@9B=#JI3@B*#4s!O))~z zc>2_4Q_#&+5V`GFd?88^;c1i7;Vv_I*qt!_Yx*n=;rj!82rrR2rQ8u5(Ejlo{15P% zs~!{%XJ>FmJ})H^I9bn^Re&38H{xA!0l3^89k(oU;bZWXM@kn$#aoS&Y4l^-WEn-fH39Jb9lA%s*WsKJQl?n9B7_~P z-XM&WL7Z!PcoF6_D>V@$CvUIEy=+Z&0kt{szMk=f1|M+r*a43^$$B^MidrT0J;RI` z(?f!O<8UZkm$_Ny$Hth1J#^4ni+im8M9mr&k|3cIgwvjAgjH z8`N&h25xV#v*d$qBX5jkI|xOhQn!>IYZK7l5#^P4M&twe9&Ey@@GxYMxBZq2e7?`q z$~Szs0!g{2fGcp9PZEt|rdQ6bhAgpcLHPz?f-vB?$dc*!9OL?Q8mn7->bFD2Si60* z!O%y)fCdMSV|lkF9w%x~J*A&srMyYY3{=&$}H zGQ4VG_?$2X(0|vT0{=;W$~icCI{b6W{B!Q8xdGhF|D{25G_5_+%s(46lhvNLkik~R z>nr(&C#5wwOzJZQo9m|U<;&Wk!_#q|V>fsmj1g<6%hB{jGoNUPjgJslld>xmODzGjYc?7JSuA?A_QzjDw5AsRgi@Y|Z0{F{!1=!NES-#*f^s4l0Hu zz468))2IY5dmD9pa*(yT5{EyP^G>@ZWumealS-*WeRcZ}B%gxq{MiJ|RyX-^C1V=0 z@iKdrGi1jTe8Ya^x7yyH$kBNvM4R~`fbPq$BzHum-3Zo8C6=KW@||>zsA8-Y9uV5V z#oq-f5L5}V<&wF4@X@<3^C%ptp6+Ce)~hGl`kwj)bsAjmo_GU^r940Z-|`<)oGnh7 zFF0Tde3>ui?8Yj{sF-Z@)yQd~CGZ*w-6p2U<8}JO-sRsVI5dBji`01W8A&3$?}lxBaC&vn0E$c5tW* zX>5(zzZ=qn&!J~KdsPl;P@bmA-Pr8T*)eh_+Dv5=Ma|XSle6t(k8qcgNyar{*ReQ8 zTXwi=8vr>!3Ywr+BhggHDw8ke==NTQVMCK`$69fhzEFB*4+H9LIvdt-#IbhZvpS}} zO3lz;P?zr0*0$%-Rq_y^k(?I{Mk}h@w}cZpMUp|ucs55bcloL2)($u%mXQw({Wzc~ z;6nu5MkjP)0C(@%6Q_I_vsWrfhl7Zpoxw#WoE~r&GOSCz;_ro6i(^hM>I$8y>`!wW z*U^@?B!MMmb89I}2(hcE4zN2G^kwyWCZp5JG>$Ez7zP~D=J^LMjSM)27_0B_X^C(M z`fFT+%DcKlu?^)FCK>QzSnV%IsXVcUFhFdBP!6~se&xxrIxsvySAWu++IrH;FbcY$ z2DWTvSBRfLwdhr0nMx+URA$j3i7_*6BWv#DXfym?ZRDcX9C?cY9sD3q)uBDR3uWg= z(lUIzB)G$Hr!){>E{s4Dew+tb9kvToZp-1&c?y2wn@Z~(VBhqz`cB;{E4(P3N2*nJ z_>~g@;UF2iG{Kt(<1PyePTKahF8<)pozZ*xH~U-kfoAayCwJViIrnqwqO}7{0pHw$ zs2Kx?s#vQr7XZ264>5RNKSL8|Ty^=PsIx^}QqOOcfpGUU4tRkUc|kc7-!Ae6!+B{o~7nFpm3|G5^=0#Bnm6`V}oSQlrX(u%OWnC zoLPy&Q;1Jui&7ST0~#+}I^&?vcE*t47~Xq#YwvA^6^} z`WkC)$AkNub|t@S!$8CBlwbV~?yp&@9h{D|3z-vJXgzRC5^nYm+PyPcgRzAnEi6Q^gslXYRv4nycsy-SJu?lMps-? zV`U*#WnFsdPLL)Q$AmD|0`UaC4ND07+&UmOu!eHruzV|OUox<+Jl|Mr@6~C`T@P%s zW7sgXLF2SSe9Fl^O(I*{9wsFSYb2l%-;&Pi^dpv!{)C3d0AlNY6!4fgmSgj_wQ*7Am7&$z;Jg&wgR-Ih;lUvWS|KTSg!&s_E9_bXBkZvGiC6bFKDWZxsD$*NZ#_8bl zG1P-#@?OQzED7@jlMJTH@V!6k;W>auvft)}g zhoV{7$q=*;=l{O>Q4a@ ziMjf_u*o^PsO)#BjC%0^h>Xp@;5$p{JSYDt)zbb}s{Kbt!T*I@Pk@X0zds6wsefuU zW$XY%yyRGC94=6mf?x+bbA5CDQ2AgW1T-jVAJbm7K(gp+;v6E0WI#kuACgV$r}6L? zd|Tj?^%^*N&b>Dd{Wr$FS2qI#Ucs1yd4N+RBUQiSZGujH`#I)mG&VKoDh=KKFl4=G z&MagXl6*<)$6P}*Tiebpz5L=oMaPrN+caUXRJ`D?=K9!e0f{@D&cZLKN?iNP@X0aF zE(^pl+;*T5qt?1jRC=5PMgV!XNITRLS_=9{CJExaQj;lt!&pdzpK?8p>%Mb+D z?yO*uSung=-`QQ@yX@Hyd4@CI^r{2oiu`%^bNkz+Nkk!IunjwNC|WcqvX~k=><-I3 zDQdbdb|!v+Iz01$w@aMl!R)koD77Xp;eZwzSl-AT zr@Vu{=xvgfq9akRrrM)}=!=xcs+U1JO}{t(avgz`6RqiiX<|hGG1pmop8k6Q+G_mv zJv|RfDheUp2L3=^C=4aCBMBn0aRCU(DQwX-W(RkRwmLeuJYF<0urcaf(=7)JPg<3P zQs!~G)9CT18o!J4{zX{_e}4eS)U-E)0FAt}wEI(c0%HkxgggW;(1E=>J17_hsH^sP z%lT0LGgbUXHx-K*CI-MCrP66UP0PvGqM$MkeLyqHdbgP|_Cm!7te~b8p+e6sQ_3k| zVcwTh6d83ltdnR>D^)BYQpDKlLk3g0Hdcgz2}%qUs9~~Rie)A-BV1mS&naYai#xcZ z(d{8=-LVpTp}2*y)|gR~;qc7fp26}lPcLZ#=JpYcn3AT9(UIdOyg+d(P5T7D&*P}# zQCYplZO5|7+r19%9e`v^vfSS1sbX1c%=w1;oyruXB%Kl$ACgKQ6=qNWLsc=28xJjg zwvsI5-%SGU|3p>&zXVl^vVtQT3o-#$UT9LI@Npz~6=4!>mc431VRNN8od&Ul^+G_kHC`G=6WVWM z%9eWNyy(FTO|A+@x}Ou3CH)oi;t#7rAxdIXfNFwOj_@Y&TGz6P_sqiB`Q6Lxy|Q{`|fgmRG(k+!#b*M+Z9zFce)f-7;?Km5O=LHV9f9_87; zF7%R2B+$?@sH&&-$@tzaPYkw0;=i|;vWdI|Wl3q_Zu>l;XdIw2FjV=;Mq5t1Q0|f< zs08j54Bp`3RzqE=2enlkZxmX6OF+@|2<)A^RNQpBd6o@OXl+i)zO%D4iGiQNuXd+zIR{_lb96{lc~bxsBveIw6umhShTX+3@ZJ=YHh@ zWY3(d0azg;7oHn>H<>?4@*RQbi>SmM=JrHvIG(~BrvI)#W(EAeO6fS+}mxxcc+X~W6&YVl86W9WFSS}Vz-f9vS?XUDBk)3TcF z8V?$4Q)`uKFq>xT=)Y9mMFVTUk*NIA!0$?RP6Ig0TBmUFrq*Q-Agq~DzxjStQyJ({ zBeZ;o5qUUKg=4Hypm|}>>L=XKsZ!F$yNTDO)jt4H0gdQ5$f|d&bnVCMMXhNh)~mN z@_UV6D7MVlsWz+zM+inZZp&P4fj=tm6fX)SG5H>OsQf_I8c~uGCig$GzuwViK54bcgL;VN|FnyQl>Ed7(@>=8$a_UKIz|V6CeVSd2(P z0Uu>A8A+muM%HLFJQ9UZ5c)BSAv_zH#1f02x?h9C}@pN@6{>UiAp>({Fn(T9Q8B z^`zB;kJ5b`>%dLm+Ol}ty!3;8f1XDSVX0AUe5P#@I+FQ-`$(a;zNgz)4x5hz$Hfbg z!Q(z26wHLXko(1`;(BAOg_wShpX0ixfWq3ponndY+u%1gyX)_h=v1zR#V}#q{au6; z!3K=7fQwnRfg6FXtNQmP>`<;!N137paFS%y?;lb1@BEdbvQHYC{976l`cLqn;b8lp zIDY>~m{gDj(wfnK!lpW6pli)HyLEiUrNc%eXTil|F2s(AY+LW5hkKb>TQ3|Q4S9rr zpDs4uK_co6XPsn_z$LeS{K4jFF`2>U`tbgKdyDne`xmR<@6AA+_hPNKCOR-Zqv;xk zu5!HsBUb^!4uJ7v0RuH-7?l?}b=w5lzzXJ~gZcxRKOovSk@|#V+MuX%Y+=;14i*%{)_gSW9(#4%)AV#3__kac1|qUy!uyP{>?U#5wYNq}y$S9pCc zFc~4mgSC*G~j0u#qqp9 z${>3HV~@->GqEhr_Xwoxq?Hjn#=s2;i~g^&Hn|aDKpA>Oc%HlW(KA1?BXqpxB;Ydx)w;2z^MpjJ(Qi(X!$5RC z*P{~%JGDQqojV>2JbEeCE*OEu!$XJ>bWA9Oa_Hd;y)F%MhBRi*LPcdqR8X`NQ&1L# z5#9L*@qxrx8n}LfeB^J{%-?SU{FCwiWyHp682F+|pa+CQa3ZLzBqN1{)h4d6+vBbV zC#NEbQLC;}me3eeYnOG*nXOJZEU$xLZ1<1Y=7r0(-U0P6-AqwMAM`a(Ed#7vJkn6plb4eI4?2y3yOTGmmDQ!z9`wzbf z_OY#0@5=bnep;MV0X_;;SJJWEf^E6Bd^tVJ9znWx&Ks8t*B>AM@?;D4oWUGc z!H*`6d7Cxo6VuyS4Eye&L1ZRhrRmN6Lr`{NL(wDbif|y&z)JN>Fl5#Wi&mMIr5i;x zBx}3YfF>>8EC(fYnmpu~)CYHuHCyr5*`ECap%t@y=jD>!_%3iiE|LN$mK9>- zHdtpy8fGZtkZF?%TW~29JIAfi2jZT8>OA7=h;8T{{k?c2`nCEx9$r zS+*&vt~2o^^J+}RDG@+9&M^K*z4p{5#IEVbz`1%`m5c2};aGt=V?~vIM}ZdPECDI)47|CWBCfDWUbxBCnmYivQ*0Nu_xb*C>~C9(VjHM zxe<*D<#dQ8TlpMX2c@M<9$w!RP$hpG4cs%AI){jp*Sj|*`m)5(Bw*A0$*i-(CA5#%>a)$+jI2C9r6|(>J8InryENI z$NohnxDUB;wAYDwrb*!N3noBTKPpPN}~09SEL18tkG zxgz(RYU_;DPT{l?Q$+eaZaxnsWCA^ds^0PVRkIM%bOd|G2IEBBiz{&^JtNsODs;5z zICt_Zj8wo^KT$7Bg4H+y!Df#3mbl%%?|EXe!&(Vmac1DJ*y~3+kRKAD=Ovde4^^%~ zw<9av18HLyrf*_>Slp;^i`Uy~`mvBjZ|?Ad63yQa#YK`4+c6;pW4?XIY9G1(Xh9WO8{F-Aju+nS9Vmv=$Ac0ienZ+p9*O%NG zMZKy5?%Z6TAJTE?o5vEr0r>f>hb#2w2U3DL64*au_@P!J!TL`oH2r*{>ffu6|A7tv zL4juf$DZ1MW5ZPsG!5)`k8d8c$J$o;%EIL0va9&GzWvkS%ZsGb#S(?{!UFOZ9<$a| zY|a+5kmD5N&{vRqkgY>aHsBT&`rg|&kezoD)gP0fsNYHsO#TRc_$n6Lf1Z{?+DLziXlHrq4sf(!>O{?Tj;Eh@%)+nRE_2VxbN&&%%caU#JDU%vL3}Cb zsb4AazPI{>8H&d=jUaZDS$-0^AxE@utGs;-Ez_F(qC9T=UZX=>ok2k2 ziTn{K?y~a5reD2A)P${NoI^>JXn>`IeArow(41c-Wm~)wiryEP(OS{YXWi7;%dG9v zI?mwu1MxD{yp_rrk!j^cKM)dc4@p4Ezyo%lRN|XyD}}>v=Xoib0gOcdXrQ^*61HNj z=NP|pd>@yfvr-=m{8$3A8TQGMTE7g=z!%yt`8`Bk-0MMwW~h^++;qyUP!J~ykh1GO z(FZ59xuFR$(WE;F@UUyE@Sp>`aVNjyj=Ty>_Vo}xf`e7`F;j-IgL5`1~-#70$9_=uBMq!2&1l zomRgpD58@)YYfvLtPW}{C5B35R;ZVvB<<#)x%srmc_S=A7F@DW8>QOEGwD6suhwCg z>Pa+YyULhmw%BA*4yjDp|2{!T98~<6Yfd(wo1mQ!KWwq0eg+6)o1>W~f~kL<-S+P@$wx*zeI|1t7z#Sxr5 zt6w+;YblPQNplq4Z#T$GLX#j6yldXAqj>4gAnnWtBICUnA&-dtnlh=t0Ho_vEKwV` z)DlJi#!@nkYV#$!)@>udAU*hF?V`2$Hf=V&6PP_|r#Iv*J$9)pF@X3`k;5})9^o4y z&)~?EjX5yX12O(BsFy-l6}nYeuKkiq`u9145&3Ssg^y{5G3Pse z9w(YVa0)N-fLaBq1`P!_#>SS(8fh_5!f{UrgZ~uEdeMJIz7DzI5!NHHqQtm~#CPij z?=N|J>nPR6_sL7!f4hD_|KH`vf8(Wpnj-(gPWH+ZvID}%?~68SwhPTC3u1_cB`otq z)U?6qo!ZLi5b>*KnYHWW=3F!p%h1;h{L&(Q&{qY6)_qxNfbP6E3yYpW!EO+IW3?@J z);4>g4gnl^8klu7uA>eGF6rIGSynacogr)KUwE_R4E5Xzi*Qir@b-jy55-JPC8c~( zo!W8y9OGZ&`xmc8;=4-U9=h{vCqfCNzYirONmGbRQlR`WWlgnY+1wCXbMz&NT~9*| z6@FrzP!LX&{no2!Ln_3|I==_4`@}V?4a;YZKTdw;vT<+K+z=uWbW(&bXEaWJ^W8Td z-3&1bY^Z*oM<=M}LVt>_j+p=2Iu7pZmbXrhQ_k)ysE9yXKygFNw$5hwDn(M>H+e1&9BM5!|81vd%r%vEm zqxY3?F@fb6O#5UunwgAHR9jp_W2zZ}NGp2%mTW@(hz7$^+a`A?mb8|_G*GNMJ) zjqegXQio=i@AINre&%ofexAr95aop5C+0MZ0m-l=MeO8m3epm7U%vZB8+I+C*iNFM z#T3l`gknX;D$-`2XT^Cg*vrv=RH+P;_dfF++cP?B_msQI4j+lt&rX2)3GaJx%W*Nn zkML%D{z5tpHH=dksQ*gzc|}gzW;lwAbxoR07VNgS*-c3d&8J|;@3t^ zVUz*J*&r7DFRuFVDCJDK8V9NN5hvpgGjwx+5n)qa;YCKe8TKtdnh{I7NU9BCN!0dq zczrBk8pE{{@vJa9ywR@mq*J=v+PG;?fwqlJVhijG!3VmIKs>9T6r7MJpC)m!Tc#>g zMtVsU>wbwFJEfwZ{vB|ZlttNe83)$iz`~#8UJ^r)lJ@HA&G#}W&ZH*;k{=TavpjWE z7hdyLZPf*X%Gm}i`Y{OGeeu^~nB8=`{r#TUrM-`;1cBvEd#d!kPqIgYySYhN-*1;L z^byj%Yi}Gx)Wnkosi337BKs}+5H5dth1JA{Ir-JKN$7zC)*}hqeoD(WfaUDPT>0`- z(6sa0AoIqASwF`>hP}^|)a_j2s^PQn*qVC{Q}htR z5-)duBFXT_V56-+UohKXlq~^6uf!6sA#ttk1o~*QEy_Y-S$gAvq47J9Vtk$5oA$Ct zYhYJ@8{hsC^98${!#Ho?4y5MCa7iGnfz}b9jE~h%EAAv~Qxu)_rAV;^cygV~5r_~?l=B`zObj7S=H=~$W zPtI_m%g$`kL_fVUk9J@>EiBH zOO&jtn~&`hIFMS5S`g8w94R4H40mdNUH4W@@XQk1sr17b{@y|JB*G9z1|CrQjd+GX z6+KyURG3;!*BQrentw{B2R&@2&`2}n(z-2&X7#r!{yg@Soy}cRD~j zj9@UBW+N|4HW4AWapy4wfUI- zZ`gSL6DUlgj*f1hSOGXG0IVH8HxK?o2|3HZ;KW{K+yPAlxtb)NV_2AwJm|E)FRs&& z=c^e7bvUsztY|+f^k7NXs$o1EUq>cR7C0$UKi6IooHWlK_#?IWDkvywnzg&ThWo^? z2O_N{5X39#?eV9l)xI(>@!vSB{DLt*oY!K1R8}_?%+0^C{d9a%N4 zoxHVT1&Lm|uDX%$QrBun5e-F`HJ^T$ zmzv)p@4ZHd_w9!%Hf9UYNvGCw2TTTbrj9pl+T9%-_-}L(tES>Or-}Z4F*{##n3~L~TuxjirGuIY#H7{%$E${?p{Q01 zi6T`n;rbK1yIB9jmQNycD~yZq&mbIsFWHo|ZAChSFPQa<(%d8mGw*V3fh|yFoxOOiWJd(qvVb!Z$b88cg->N=qO*4k~6;R==|9ihg&riu#P~s4Oap9O7f%crSr^rljeIfXDEg>wi)&v*a%7zpz<9w z*r!3q9J|390x`Zk;g$&OeN&ctp)VKRpDSV@kU2Q>jtok($Y-*x8_$2piTxun81@vt z!Vj?COa0fg2RPXMSIo26T=~0d`{oGP*eV+$!0I<(4azk&Vj3SiG=Q!6mX0p$z7I}; z9BJUFgT-K9MQQ-0@Z=^7R<{bn2Fm48endsSs`V7_@%8?Bxkqv>BDoVcj?K#dV#uUP zL1ND~?D-|VGKe3Rw_7-Idpht>H6XRLh*U7epS6byiGvJpr%d}XwfusjH9g;Z98H`x zyde%%5mhGOiL4wljCaWCk-&uE4_OOccb9c!ZaWt4B(wYl!?vyzl%7n~QepN&eFUrw zFIOl9c({``6~QD+43*_tzP{f2x41h(?b43^y6=iwyB)2os5hBE!@YUS5?N_tXd=h( z)WE286Fbd>R4M^P{!G)f;h<3Q>Fipuy+d2q-)!RyTgt;wr$(?9ox3;q+{E*ZQHhOn;lM`cjnu9 zXa48ks-v(~b*;MAI<>YZH(^NV8vjb34beE<_cwKlJoR;k6lJNSP6v}uiyRD?|0w+X@o1ONrH8a$fCxXpf? z?$DL0)7|X}Oc%h^zrMKWc-NS9I0Utu@>*j}b@tJ=ixQSJ={4@854wzW@E>VSL+Y{i z#0b=WpbCZS>kUCO_iQz)LoE>P5LIG-hv9E+oG}DtlIDF>$tJ1aw9^LuhLEHt?BCj& z(O4I8v1s#HUi5A>nIS-JK{v!7dJx)^Yg%XjNmlkWAq2*cv#tHgz`Y(bETc6CuO1VkN^L-L3j_x<4NqYb5rzrLC-7uOv z!5e`GZt%B782C5-fGnn*GhDF$%(qP<74Z}3xx+{$4cYKy2ikxI7B2N+2r07DN;|-T->nU&!=Cm#rZt%O_5c&1Z%nlWq3TKAW0w zQqemZw_ue--2uKQsx+niCUou?HjD`xhEjjQd3%rrBi82crq*~#uA4+>vR<_S{~5ce z-2EIl?~s z1=GVL{NxP1N3%=AOaC}j_Fv=ur&THz zyO!d9kHq|c73kpq`$+t+8Bw7MgeR5~`d7ChYyGCBWSteTB>8WAU(NPYt2Dk`@#+}= zI4SvLlyk#pBgVigEe`?NG*vl7V6m+<}%FwPV=~PvvA)=#ths==DRTDEYh4V5}Cf$z@#;< zyWfLY_5sP$gc3LLl2x+Ii)#b2nhNXJ{R~vk`s5U7Nyu^3yFg&D%Txwj6QezMX`V(x z=C`{76*mNb!qHHs)#GgGZ_7|vkt9izl_&PBrsu@}L`X{95-2jf99K)0=*N)VxBX2q z((vkpP2RneSIiIUEnGb?VqbMb=Zia+rF~+iqslydE34cSLJ&BJW^3knX@M;t*b=EA zNvGzv41Ld_T+WT#XjDB840vovUU^FtN_)G}7v)1lPetgpEK9YS^OWFkPoE{ovj^=@ zO9N$S=G$1ecndT_=5ehth2Lmd1II-PuT~C9`XVePw$y8J#dpZ?Tss<6wtVglm(Ok7 z3?^oi@pPio6l&!z8JY(pJvG=*pI?GIOu}e^EB6QYk$#FJQ%^AIK$I4epJ+9t?KjqA+bkj&PQ*|vLttme+`9G=L% ziadyMw_7-M)hS(3E$QGNCu|o23|%O+VN7;Qggp?PB3K-iSeBa2b}V4_wY`G1Jsfz4 z9|SdB^;|I8E8gWqHKx!vj_@SMY^hLEIbSMCuE?WKq=c2mJK z8LoG-pnY!uhqFv&L?yEuxo{dpMTsmCn)95xanqBrNPTgXP((H$9N${Ow~Is-FBg%h z53;|Y5$MUN)9W2HBe2TD`ct^LHI<(xWrw}$qSoei?}s)&w$;&!14w6B6>Yr6Y8b)S z0r71`WmAvJJ`1h&poLftLUS6Ir zC$bG9!Im_4Zjse)#K=oJM9mHW1{%l8sz$1o?ltdKlLTxWWPB>Vk22czVt|1%^wnN@*!l)}?EgtvhC>vlHm^t+ogpgHI1_$1ox9e;>0!+b(tBrmXRB`PY1vp-R**8N7 zGP|QqI$m(Rdu#=(?!(N}G9QhQ%o!aXE=aN{&wtGP8|_qh+7a_j_sU5|J^)vxq;# zjvzLn%_QPHZZIWu1&mRAj;Sa_97p_lLq_{~j!M9N^1yp3U_SxRqK&JnR%6VI#^E12 z>CdOVI^_9aPK2eZ4h&^{pQs}xsijXgFYRIxJ~N7&BB9jUR1fm!(xl)mvy|3e6-B3j zJn#ajL;bFTYJ2+Q)tDjx=3IklO@Q+FFM}6UJr6km7hj7th9n_&JR7fnqC!hTZoM~T zBeaVFp%)0cbPhejX<8pf5HyRUj2>aXnXBqDJe73~J%P(2C?-RT{c3NjE`)om! zl$uewSgWkE66$Kb34+QZZvRn`fob~Cl9=cRk@Es}KQm=?E~CE%spXaMO6YmrMl%9Q zlA3Q$3|L1QJ4?->UjT&CBd!~ru{Ih^in&JXO=|<6J!&qp zRe*OZ*cj5bHYlz!!~iEKcuE|;U4vN1rk$xq6>bUWD*u(V@8sG^7>kVuo(QL@Ki;yL zWC!FT(q{E8#on>%1iAS0HMZDJg{Z{^!De(vSIq&;1$+b)oRMwA3nc3mdTSG#3uYO_ z>+x;7p4I;uHz?ZB>dA-BKl+t-3IB!jBRgdvAbW!aJ(Q{aT>+iz?91`C-xbe)IBoND z9_Xth{6?(y3rddwY$GD65IT#f3<(0o#`di{sh2gm{dw*#-Vnc3r=4==&PU^hCv$qd zjw;>i&?L*Wq#TxG$mFIUf>eK+170KG;~+o&1;Tom9}}mKo23KwdEM6UonXgc z!6N(@k8q@HPw{O8O!lAyi{rZv|DpgfU{py+j(X_cwpKqcalcqKIr0kM^%Br3SdeD> zHSKV94Yxw;pjzDHo!Q?8^0bb%L|wC;4U^9I#pd5O&eexX+Im{ z?jKnCcsE|H?{uGMqVie_C~w7GX)kYGWAg%-?8|N_1#W-|4F)3YTDC+QSq1s!DnOML3@d`mG%o2YbYd#jww|jD$gotpa)kntakp#K;+yo-_ZF9qrNZw<%#C zuPE@#3RocLgPyiBZ+R_-FJ_$xP!RzWm|aN)S+{$LY9vvN+IW~Kf3TsEIvP+B9Mtm! zpfNNxObWQpLoaO&cJh5>%slZnHl_Q~(-Tfh!DMz(dTWld@LG1VRF`9`DYKhyNv z2pU|UZ$#_yUx_B_|MxUq^glT}O5Xt(Vm4Mr02><%C)@v;vPb@pT$*yzJ4aPc_FZ3z z3}PLoMBIM>q_9U2rl^sGhk1VUJ89=*?7|v`{!Z{6bqFMq(mYiA?%KbsI~JwuqVA9$H5vDE+VocjX+G^%bieqx->s;XWlKcuv(s%y%D5Xbc9+ zc(_2nYS1&^yL*ey664&4`IoOeDIig}y-E~_GS?m;D!xv5-xwz+G`5l6V+}CpeJDi^ z%4ed$qowm88=iYG+(`ld5Uh&>Dgs4uPHSJ^TngXP_V6fPyl~>2bhi20QB%lSd#yYn zO05?KT1z@?^-bqO8Cg`;ft>ilejsw@2%RR7;`$Vs;FmO(Yr3Fp`pHGr@P2hC%QcA|X&N2Dn zYf`MqXdHi%cGR@%y7Rg7?d3?an){s$zA{!H;Ie5exE#c~@NhQUFG8V=SQh%UxUeiV zd7#UcYqD=lk-}sEwlpu&H^T_V0{#G?lZMxL7ih_&{(g)MWBnCZxtXg znr#}>U^6!jA%e}@Gj49LWG@*&t0V>Cxc3?oO7LSG%~)Y5}f7vqUUnQ;STjdDU}P9IF9d9<$;=QaXc zL1^X7>fa^jHBu_}9}J~#-oz3Oq^JmGR#?GO7b9a(=R@fw@}Q{{@`Wy1vIQ#Bw?>@X z-_RGG@wt|%u`XUc%W{J z>iSeiz8C3H7@St3mOr_mU+&bL#Uif;+Xw-aZdNYUpdf>Rvu0i0t6k*}vwU`XNO2he z%miH|1tQ8~ZK!zmL&wa3E;l?!!XzgV#%PMVU!0xrDsNNZUWKlbiOjzH-1Uoxm8E#r`#2Sz;-o&qcqB zC-O_R{QGuynW14@)7&@yw1U}uP(1cov)twxeLus0s|7ayrtT8c#`&2~Fiu2=R;1_4bCaD=*E@cYI>7YSnt)nQc zohw5CsK%m?8Ack)qNx`W0_v$5S}nO|(V|RZKBD+btO?JXe|~^Qqur%@eO~<8-L^9d z=GA3-V14ng9L29~XJ>a5k~xT2152zLhM*@zlp2P5Eu}bywkcqR;ISbas&#T#;HZSf z2m69qTV(V@EkY(1Dk3`}j)JMo%ZVJ*5eB zYOjIisi+igK0#yW*gBGj?@I{~mUOvRFQR^pJbEbzFxTubnrw(Muk%}jI+vXmJ;{Q6 zrSobKD>T%}jV4Ub?L1+MGOD~0Ir%-`iTnWZN^~YPrcP5y3VMAzQ+&en^VzKEb$K!Q z<7Dbg&DNXuow*eD5yMr+#08nF!;%4vGrJI++5HdCFcGLfMW!KS*Oi@=7hFwDG!h2< zPunUEAF+HncQkbfFj&pbzp|MU*~60Z(|Ik%Tn{BXMN!hZOosNIseT?R;A`W?=d?5X zK(FB=9mZusYahp|K-wyb={rOpdn=@;4YI2W0EcbMKyo~-#^?h`BA9~o285%oY zfifCh5Lk$SY@|2A@a!T2V+{^!psQkx4?x0HSV`(w9{l75QxMk!)U52Lbhn{8ol?S) zCKo*7R(z!uk<6*qO=wh!Pul{(qq6g6xW;X68GI_CXp`XwO zxuSgPRAtM8K7}5E#-GM!*ydOOG_{A{)hkCII<|2=ma*71ci_-}VPARm3crFQjLYV! z9zbz82$|l01mv`$WahE2$=fAGWkd^X2kY(J7iz}WGS z@%MyBEO=A?HB9=^?nX`@nh;7;laAjs+fbo!|K^mE!tOB>$2a_O0y-*uaIn8k^6Y zSbuv;5~##*4Y~+y7Z5O*3w4qgI5V^17u*ZeupVGH^nM&$qmAk|anf*>r zWc5CV;-JY-Z@Uq1Irpb^O`L_7AGiqd*YpGUShb==os$uN3yYvb`wm6d=?T*it&pDk zo`vhw)RZX|91^^Wa_ti2zBFyWy4cJu#g)_S6~jT}CC{DJ_kKpT`$oAL%b^!2M;JgT zM3ZNbUB?}kP(*YYvXDIH8^7LUxz5oE%kMhF!rnPqv!GiY0o}NR$OD=ITDo9r%4E>E0Y^R(rS^~XjWyVI6 zMOR5rPXhTp*G*M&X#NTL`Hu*R+u*QNoiOKg4CtNPrjgH>c?Hi4MUG#I917fx**+pJfOo!zFM&*da&G_x)L(`k&TPI*t3e^{crd zX<4I$5nBQ8Ax_lmNRa~E*zS-R0sxkz`|>7q_?*e%7bxqNm3_eRG#1ae3gtV9!fQpY z+!^a38o4ZGy9!J5sylDxZTx$JmG!wg7;>&5H1)>f4dXj;B+@6tMlL=)cLl={jLMxY zbbf1ax3S4>bwB9-$;SN2?+GULu;UA-35;VY*^9Blx)Jwyb$=U!D>HhB&=jSsd^6yw zL)?a|>GxU!W}ocTC(?-%z3!IUhw^uzc`Vz_g>-tv)(XA#JK^)ZnC|l1`@CdX1@|!| z_9gQ)7uOf?cR@KDp97*>6X|;t@Y`k_N@)aH7gY27)COv^P3ya9I{4z~vUjLR9~z1Z z5=G{mVtKH*&$*t0@}-i_v|3B$AHHYale7>E+jP`ClqG%L{u;*ff_h@)al?RuL7tOO z->;I}>%WI{;vbLP3VIQ^iA$4wl6@0sDj|~112Y4OFjMs`13!$JGkp%b&E8QzJw_L5 zOnw9joc0^;O%OpF$Qp)W1HI!$4BaXX84`%@#^dk^hFp^pQ@rx4g(8Xjy#!X%+X5Jd@fs3amGT`}mhq#L97R>OwT5-m|h#yT_-v@(k$q7P*9X~T*3)LTdzP!*B} z+SldbVWrrwQo9wX*%FyK+sRXTa@O?WM^FGWOE?S`R(0P{<6p#f?0NJvnBia?k^fX2 zNQs7K-?EijgHJY}&zsr;qJ<*PCZUd*x|dD=IQPUK_nn)@X4KWtqoJNHkT?ZWL_hF? zS8lp2(q>;RXR|F;1O}EE#}gCrY~#n^O`_I&?&z5~7N;zL0)3Tup`%)oHMK-^r$NT% zbFg|o?b9w(q@)6w5V%si<$!U<#}s#x@0aX-hP>zwS#9*75VXA4K*%gUc>+yzupTDBOKH8WR4V0pM(HrfbQ&eJ79>HdCvE=F z|J>s;;iDLB^3(9}?biKbxf1$lI!*Z%*0&8UUq}wMyPs_hclyQQi4;NUY+x2qy|0J; zhn8;5)4ED1oHwg+VZF|80<4MrL97tGGXc5Sw$wAI#|2*cvQ=jB5+{AjMiDHmhUC*a zlmiZ`LAuAn_}hftXh;`Kq0zblDk8?O-`tnilIh|;3lZp@F_osJUV9`*R29M?7H{Fy z`nfVEIDIWXmU&YW;NjU8)EJpXhxe5t+scf|VXM!^bBlwNh)~7|3?fWwo_~ZFk(22% zTMesYw+LNx3J-_|DM~`v93yXe=jPD{q;li;5PD?Dyk+b? zo21|XpT@)$BM$%F=P9J19Vi&1#{jM3!^Y&fr&_`toi`XB1!n>sbL%U9I5<7!@?t)~ z;&H%z>bAaQ4f$wIzkjH70;<8tpUoxzKrPhn#IQfS%9l5=Iu))^XC<58D!-O z{B+o5R^Z21H0T9JQ5gNJnqh#qH^na|z92=hONIM~@_iuOi|F>jBh-?aA20}Qx~EpDGElELNn~|7WRXRFnw+Wdo`|# zBpU=Cz3z%cUJ0mx_1($X<40XEIYz(`noWeO+x#yb_pwj6)R(__%@_Cf>txOQ74wSJ z0#F3(zWWaR-jMEY$7C*3HJrohc79>MCUu26mfYN)f4M~4gD`}EX4e}A!U}QV8!S47 z6y-U-%+h`1n`*pQuKE%Av0@)+wBZr9mH}@vH@i{v(m-6QK7Ncf17x_D=)32`FOjjo zg|^VPf5c6-!FxN{25dvVh#fog=NNpXz zfB$o+0jbRkHH{!TKhE709f+jI^$3#v1Nmf80w`@7-5$1Iv_`)W^px8P-({xwb;D0y z7LKDAHgX<84?l!I*Dvi2#D@oAE^J|g$3!)x1Ua;_;<@#l1fD}lqU2_tS^6Ht$1Wl} zBESo7o^)9-Tjuz$8YQSGhfs{BQV6zW7dA?0b(Dbt=UnQs&4zHfe_sj{RJ4uS-vQpC zX;Bbsuju4%!o8?&m4UZU@~ZZjeFF6ex2ss5_60_JS_|iNc+R0GIjH1@Z z=rLT9%B|WWgOrR7IiIwr2=T;Ne?30M!@{%Qf8o`!>=s<2CBpCK_TWc(DX51>e^xh8 z&@$^b6CgOd7KXQV&Y4%}_#uN*mbanXq(2=Nj`L7H7*k(6F8s6{FOw@(DzU`4-*77{ zF+dxpv}%mFpYK?>N_2*#Y?oB*qEKB}VoQ@bzm>ptmVS_EC(#}Lxxx730trt0G)#$b zE=wVvtqOct1%*9}U{q<)2?{+0TzZzP0jgf9*)arV)*e!f`|jgT{7_9iS@e)recI#z zbzolURQ+TOzE!ymqvBY7+5NnAbWxvMLsLTwEbFqW=CPyCsmJ}P1^V30|D5E|p3BC5 z)3|qgw@ra7aXb-wsa|l^in~1_fm{7bS9jhVRkYVO#U{qMp z)Wce+|DJ}4<2gp8r0_xfZpMo#{Hl2MfjLcZdRB9(B(A(f;+4s*FxV{1F|4d`*sRNd zp4#@sEY|?^FIJ;tmH{@keZ$P(sLh5IdOk@k^0uB^BWr@pk6mHy$qf&~rI>P*a;h0C{%oA*i!VjWn&D~O#MxN&f@1Po# zKN+ zrGrkSjcr?^R#nGl<#Q722^wbYcgW@{+6CBS<1@%dPA8HC!~a`jTz<`g_l5N1M@9wn9GOAZ>nqNgq!yOCbZ@1z`U_N`Z>}+1HIZxk*5RDc&rd5{3qjRh8QmT$VyS;jK z;AF+r6XnnCp=wQYoG|rT2@8&IvKq*IB_WvS%nt%e{MCFm`&W*#LXc|HrD?nVBo=(8*=Aq?u$sDA_sC_RPDUiQ+wnIJET8vx$&fxkW~kP9qXKt zozR)@xGC!P)CTkjeWvXW5&@2?)qt)jiYWWBU?AUtzAN}{JE1I)dfz~7$;}~BmQF`k zpn11qmObXwRB8&rnEG*#4Xax3XBkKlw(;tb?Np^i+H8m(Wyz9k{~ogba@laiEk;2! zV*QV^6g6(QG%vX5Um#^sT&_e`B1pBW5yVth~xUs#0}nv?~C#l?W+9Lsb_5)!71rirGvY zTIJ$OPOY516Y|_014sNv+Z8cc5t_V=i>lWV=vNu#!58y9Zl&GsMEW#pPYPYGHQ|;vFvd*9eM==$_=vc7xnyz0~ zY}r??$<`wAO?JQk@?RGvkWVJlq2dk9vB(yV^vm{=NVI8dhsX<)O(#nr9YD?I?(VmQ z^r7VfUBn<~p3()8yOBjm$#KWx!5hRW)5Jl7wY@ky9lNM^jaT##8QGVsYeaVywmpv>X|Xj7gWE1Ezai&wVLt3p)k4w~yrskT-!PR!kiyQlaxl(( zXhF%Q9x}1TMt3~u@|#wWm-Vq?ZerK={8@~&@9r5JW}r#45#rWii};t`{5#&3$W)|@ zbAf2yDNe0q}NEUvq_Quq3cTjcw z@H_;$hu&xllCI9CFDLuScEMg|x{S7GdV8<&Mq=ezDnRZAyX-8gv97YTm0bg=d)(>N z+B2FcqvI9>jGtnK%eO%y zoBPkJTk%y`8TLf4)IXPBn`U|9>O~WL2C~C$z~9|0m*YH<-vg2CD^SX#&)B4ngOSG$ zV^wmy_iQk>dfN@Pv(ckfy&#ak@MLC7&Q6Ro#!ezM*VEh`+b3Jt%m(^T&p&WJ2Oqvj zs-4nq0TW6cv~(YI$n0UkfwN}kg3_fp?(ijSV#tR9L0}l2qjc7W?i*q01=St0eZ=4h zyGQbEw`9OEH>NMuIe)hVwYHsGERWOD;JxEiO7cQv%pFCeR+IyhwQ|y@&^24k+|8fD zLiOWFNJ2&vu2&`Jv96_z-Cd5RLgmeY3*4rDOQo?Jm`;I_(+ejsPM03!ly!*Cu}Cco zrQSrEDHNyzT(D5s1rZq!8#?f6@v6dB7a-aWs(Qk>N?UGAo{gytlh$%_IhyL7h?DLXDGx zgxGEBQoCAWo-$LRvM=F5MTle`M})t3vVv;2j0HZY&G z22^iGhV@uaJh(XyyY%} zd4iH_UfdV#T=3n}(Lj^|n;O4|$;xhu*8T3hR1mc_A}fK}jfZ7LX~*n5+`8N2q#rI$ z@<_2VANlYF$vIH$ zl<)+*tIWW78IIINA7Rr7i{<;#^yzxoLNkXL)eSs=%|P>$YQIh+ea_3k z_s7r4%j7%&*NHSl?R4k%1>Z=M9o#zxY!n8sL5>BO-ZP;T3Gut>iLS@U%IBrX6BA3k z)&@q}V8a{X<5B}K5s(c(LQ=%v1ocr`t$EqqY0EqVjr65usa=0bkf|O#ky{j3)WBR(((L^wmyHRzoWuL2~WTC=`yZ zn%VX`L=|Ok0v7?s>IHg?yArBcync5rG#^+u)>a%qjES%dRZoIyA8gQ;StH z1Ao7{<&}6U=5}4v<)1T7t!J_CL%U}CKNs-0xWoTTeqj{5{?Be$L0_tk>M9o8 zo371}S#30rKZFM{`H_(L`EM9DGp+Mifk&IP|C2Zu_)Ghr4Qtpmkm1osCf@%Z$%t+7 zYH$Cr)Ro@3-QDeQJ8m+x6%;?YYT;k6Z0E-?kr>x33`H%*ueBD7Zx~3&HtWn0?2Wt} zTG}*|v?{$ajzt}xPzV%lL1t-URi8*Zn)YljXNGDb>;!905Td|mpa@mHjIH%VIiGx- zd@MqhpYFu4_?y5N4xiHn3vX&|e6r~Xt> zZG`aGq|yTNjv;9E+Txuoa@A(9V7g?1_T5FzRI;!=NP1Kqou1z5?%X~Wwb{trRfd>i z8&y^H)8YnKyA_Fyx>}RNmQIczT?w2J4SNvI{5J&}Wto|8FR(W;Qw#b1G<1%#tmYzQ zQ2mZA-PAdi%RQOhkHy9Ea#TPSw?WxwL@H@cbkZwIq0B!@ns}niALidmn&W?!Vd4Gj zO7FiuV4*6Mr^2xlFSvM;Cp_#r8UaqIzHJQg_z^rEJw&OMm_8NGAY2)rKvki|o1bH~ z$2IbfVeY2L(^*rMRU1lM5Y_sgrDS`Z??nR2lX;zyR=c%UyGb*%TC-Dil?SihkjrQy~TMv6;BMs7P8il`H7DmpVm@rJ;b)hW)BL)GjS154b*xq-NXq2cwE z^;VP7ua2pxvCmxrnqUYQMH%a%nHmwmI33nJM(>4LznvY*k&C0{8f*%?zggpDgkuz&JBx{9mfb@wegEl2v!=}Sq2Gaty0<)UrOT0{MZtZ~j5y&w zXlYa_jY)I_+VA-^#mEox#+G>UgvM!Ac8zI<%JRXM_73Q!#i3O|)lOP*qBeJG#BST0 zqohi)O!|$|2SeJQo(w6w7%*92S})XfnhrH_Z8qe!G5>CglP=nI7JAOW?(Z29;pXJ9 zR9`KzQ=WEhy*)WH>$;7Cdz|>*i>=##0bB)oU0OR>>N<21e4rMCHDemNi2LD>Nc$;& zQRFthpWniC1J6@Zh~iJCoLOxN`oCKD5Q4r%ynwgUKPlIEd#?QViIqovY|czyK8>6B zSP%{2-<;%;1`#0mG^B(8KbtXF;Nf>K#Di72UWE4gQ%(_26Koiad)q$xRL~?pN71ZZ zujaaCx~jXjygw;rI!WB=xrOJO6HJ!!w}7eiivtCg5K|F6$EXa)=xUC za^JXSX98W`7g-tm@uo|BKj39Dl;sg5ta;4qjo^pCh~{-HdLl6qI9Ix6f$+qiZ$}s= zNguKrU;u+T@ko(Vr1>)Q%h$?UKXCY>3se%&;h2osl2D zE4A9bd7_|^njDd)6cI*FupHpE3){4NQ*$k*cOWZ_?CZ>Z4_fl@n(mMnYK62Q1d@+I zr&O))G4hMihgBqRIAJkLdk(p(D~X{-oBUA+If@B}j& zsHbeJ3RzTq96lB7d($h$xTeZ^gP0c{t!Y0c)aQE;$FY2!mACg!GDEMKXFOPI^)nHZ z`aSPJpvV0|bbrzhWWkuPURlDeN%VT8tndV8?d)eN*i4I@u zVKl^6{?}A?P)Fsy?3oi#clf}L18t;TjNI2>eI&(ezDK7RyqFxcv%>?oxUlonv(px) z$vnPzRH`y5A(x!yOIfL0bmgeMQB$H5wenx~!ujQK*nUBW;@Em&6Xv2%s(~H5WcU2R z;%Nw<$tI)a`Ve!>x+qegJnQsN2N7HaKzrFqM>`6R*gvh%O*-%THt zrB$Nk;lE;z{s{r^PPm5qz(&lM{sO*g+W{sK+m3M_z=4=&CC>T`{X}1Vg2PEfSj2x_ zmT*(x;ov%3F?qoEeeM>dUn$a*?SIGyO8m806J1W1o+4HRhc2`9$s6hM#qAm zChQ87b~GEw{ADfs+5}FJ8+|bIlIv(jT$Ap#hSHoXdd9#w<#cA<1Rkq^*EEkknUd4& zoIWIY)sAswy6fSERVm&!SO~#iN$OgOX*{9@_BWFyJTvC%S++ilSfCrO(?u=Dc?CXZ zzCG&0yVR{Z`|ZF0eEApWEo#s9osV>F{uK{QA@BES#&;#KsScf>y zvs?vIbI>VrT<*!;XmQS=bhq%46-aambZ(8KU-wOO2=en~D}MCToB_u;Yz{)1ySrPZ z@=$}EvjTdzTWU7c0ZI6L8=yP+YRD_eMMos}b5vY^S*~VZysrkq<`cK3>>v%uy7jgq z0ilW9KjVDHLv0b<1K_`1IkbTOINs0=m-22c%M~l=^S}%hbli-3?BnNq?b`hx^HX2J zIe6ECljRL0uBWb`%{EA=%!i^4sMcj+U_TaTZRb+~GOk z^ZW!nky0n*Wb*r+Q|9H@ml@Z5gU&W`(z4-j!OzC1wOke`TRAYGZVl$PmQ16{3196( zO*?`--I}Qf(2HIwb2&1FB^!faPA2=sLg(@6P4mN)>Dc3i(B0;@O-y2;lM4akD>@^v z=u>*|!s&9zem70g7zfw9FXl1bpJW(C#5w#uy5!V?Q(U35A~$dR%LDVnq@}kQm13{} zd53q3N(s$Eu{R}k2esbftfjfOITCL;jWa$}(mmm}d(&7JZ6d3%IABCapFFYjdEjdK z&4Edqf$G^MNAtL=uCDRs&Fu@FXRgX{*0<(@c3|PNHa>L%zvxWS={L8%qw`STm+=Rd zA}FLspESSIpE_^41~#5yI2bJ=9`oc;GIL!JuW&7YetZ?0H}$$%8rW@*J37L-~Rsx!)8($nI4 zZhcZ2^=Y+p4YPl%j!nFJA|*M^gc(0o$i3nlphe+~-_m}jVkRN{spFs(o0ajW@f3K{ zDV!#BwL322CET$}Y}^0ixYj2w>&Xh12|R8&yEw|wLDvF!lZ#dOTHM9pK6@Nm-@9Lnng4ZHBgBSrr7KI8YCC9DX5Kg|`HsiwJHg2(7#nS;A{b3tVO?Z% za{m5b3rFV6EpX;=;n#wltDv1LE*|g5pQ+OY&*6qCJZc5oDS6Z6JD#6F)bWxZSF@q% z+1WV;m!lRB!n^PC>RgQCI#D1br_o^#iPk>;K2hB~0^<~)?p}LG%kigm@moD#q3PE+ zA^Qca)(xnqw6x>XFhV6ku9r$E>bWNrVH9fum0?4s?Rn2LG{Vm_+QJHse6xa%nzQ?k zKug4PW~#Gtb;#5+9!QBgyB@q=sk9=$S{4T>wjFICStOM?__fr+Kei1 z3j~xPqW;W@YkiUM;HngG!;>@AITg}vAE`M2Pj9Irl4w1fo4w<|Bu!%rh%a(Ai^Zhi zs92>v5;@Y(Zi#RI*ua*h`d_7;byQSa*v9E{2x$<-_=5Z<7{%)}4XExANcz@rK69T0x3%H<@frW>RA8^swA+^a(FxK| zFl3LD*ImHN=XDUkrRhp6RY5$rQ{bRgSO*(vEHYV)3Mo6Jy3puiLmU&g82p{qr0F?ohmbz)f2r{X2|T2 z$4fdQ=>0BeKbiVM!e-lIIs8wVTuC_m7}y4A_%ikI;Wm5$9j(^Y z(cD%U%k)X>_>9~t8;pGzL6L-fmQO@K; zo&vQzMlgY95;1BSkngY)e{`n0!NfVgf}2mB3t}D9@*N;FQ{HZ3Pb%BK6;5#-O|WI( zb6h@qTLU~AbVW#_6?c!?Dj65Now7*pU{h!1+eCV^KCuPAGs28~3k@ueL5+u|Z-7}t z9|lskE`4B7W8wMs@xJa{#bsCGDFoRSNSnmNYB&U7 zVGKWe%+kFB6kb)e;TyHfqtU6~fRg)f|>=5(N36)0+C z`hv65J<$B}WUc!wFAb^QtY31yNleq4dzmG`1wHTj=c*=hay9iD071Hc?oYoUk|M*_ zU1GihAMBsM@5rUJ(qS?9ZYJ6@{bNqJ`2Mr+5#hKf?doa?F|+^IR!8lq9)wS3tF_9n zW_?hm)G(M+MYb?V9YoX^_mu5h-LP^TL^!Q9Z7|@sO(rg_4+@=PdI)WL(B7`!K^ND- z-uIuVDCVEdH_C@c71YGYT^_Scf_dhB8Z2Xy6vGtBSlYud9vggOqv^L~F{BraSE_t} zIkP+Hp2&nH^-MNEs}^`oMLy11`PQW$T|K(`Bu*(f@)mv1-qY(_YG&J2M2<7k;;RK~ zL{Fqj9yCz8(S{}@c)S!65aF<=&eLI{hAMErCx&>i7OeDN>okvegO87OaG{Jmi<|}D zaT@b|0X{d@OIJ7zvT>r+eTzgLq~|Dpu)Z&db-P4z*`M$UL51lf>FLlq6rfG)%doyp z)3kk_YIM!03eQ8Vu_2fg{+osaEJPtJ-s36R+5_AEG12`NG)IQ#TF9c@$99%0iye+ zUzZ57=m2)$D(5Nx!n)=5Au&O0BBgwxIBaeI(mro$#&UGCr<;C{UjJVAbVi%|+WP(a zL$U@TYCxJ=1{Z~}rnW;7UVb7+ZnzgmrogDxhjLGo>c~MiJAWs&&;AGg@%U?Y^0JhL ze(x6Z74JG6FlOFK(T}SXQfhr}RIFl@QXKnIcXYF)5|V~e-}suHILKT-k|<*~Ij|VF zC;t@=uj=hot~*!C68G8hTA%8SzOfETOXQ|3FSaIEjvBJp(A)7SWUi5!Eu#yWgY+;n zlm<$+UDou*V+246_o#V4kMdto8hF%%Lki#zPh}KYXmMf?hrN0;>Mv%`@{0Qn`Ujp) z=lZe+13>^Q!9zT);H<(#bIeRWz%#*}sgUX9P|9($kexOyKIOc`dLux}c$7It4u|Rl z6SSkY*V~g_B-hMPo_ak>>z@AVQ(_N)VY2kB3IZ0G(iDUYw+2d7W^~(Jq}KY=JnWS( z#rzEa&0uNhJ>QE8iiyz;n2H|SV#Og+wEZv=f2%1ELX!SX-(d3tEj$5$1}70Mp<&eI zCkfbByL7af=qQE@5vDVxx1}FSGt_a1DoE3SDI+G)mBAna)KBG4p8Epxl9QZ4BfdAN zFnF|Y(umr;gRgG6NLQ$?ZWgllEeeq~z^ZS7L?<(~O&$5|y)Al^iMKy}&W+eMm1W z7EMU)u^ke(A1#XCV>CZ71}P}0x)4wtHO8#JRG3MA-6g=`ZM!FcICCZ{IEw8Dm2&LQ z1|r)BUG^0GzI6f946RrBlfB1Vs)~8toZf~7)+G;pv&XiUO(%5bm)pl=p>nV^o*;&T z;}@oZSibzto$arQgfkp|z4Z($P>dTXE{4O=vY0!)kDO* zGF8a4wq#VaFpLfK!iELy@?-SeRrdz%F*}hjKcA*y@mj~VD3!it9lhRhX}5YOaR9$} z3mS%$2Be7{l(+MVx3 z(4?h;P!jnRmX9J9sYN#7i=iyj_5q7n#X(!cdqI2lnr8T$IfOW<_v`eB!d9xY1P=2q&WtOXY=D9QYteP)De?S4}FK6#6Ma z=E*V+#s8>L;8aVroK^6iKo=MH{4yEZ_>N-N z`(|;aOATba1^asjxlILk<4}f~`39dBFlxj>Dw(hMYKPO3EEt1@S`1lxFNM+J@uB7T zZ8WKjz7HF1-5&2=l=fqF-*@>n5J}jIxdDwpT?oKM3s8Nr`x8JnN-kCE?~aM1H!hAE z%%w(3kHfGwMnMmNj(SU(w42OrC-euI>Dsjk&jz3ts}WHqmMpzQ3vZrsXrZ|}+MHA7 z068obeXZTsO*6RS@o3x80E4ok``rV^Y3hr&C1;|ZZ0|*EKO`$lECUYG2gVFtUTw)R z4Um<0ZzlON`zTdvVdL#KFoMFQX*a5wM0Czp%wTtfK4Sjs)P**RW&?lP$(<}q%r68Z zS53Y!d@&~ne9O)A^tNrXHhXBkj~$8j%pT1%%mypa9AW5E&s9)rjF4@O3ytH{0z6riz|@< zB~UPh*wRFg2^7EbQrHf0y?E~dHlkOxof_a?M{LqQ^C!i2dawHTPYUE=X@2(3<=OOxs8qn_(y>pU>u^}3y&df{JarR0@VJn0f+U%UiF=$Wyq zQvnVHESil@d|8&R<%}uidGh7@u^(%?$#|&J$pvFC-n8&A>utA=n3#)yMkz+qnG3wd zP7xCnF|$9Dif@N~L)Vde3hW8W!UY0BgT2v(wzp;tlLmyk2%N|0jfG$%<;A&IVrOI< z!L)o>j>;dFaqA3pL}b-Je(bB@VJ4%!JeX@3x!i{yIeIso^=n?fDX`3bU=eG7sTc%g%ye8$v8P@yKE^XD=NYxTb zbf!Mk=h|otpqjFaA-vs5YOF-*GwWPc7VbaOW&stlANnCN8iftFMMrUdYNJ_Bnn5Vt zxfz@Ah|+4&P;reZxp;MmEI7C|FOv8NKUm8njF7Wb6Gi7DeODLl&G~}G4be&*Hi0Qw z5}77vL0P+7-B%UL@3n1&JPxW^d@vVwp?u#gVcJqY9#@-3X{ok#UfW3<1fb%FT`|)V~ggq z(3AUoUS-;7)^hCjdT0Kf{i}h)mBg4qhtHHBti=~h^n^OTH5U*XMgDLIR@sre`AaB$ zg)IGBET_4??m@cx&c~bA80O7B8CHR7(LX7%HThkeC*@vi{-pL%e)yXp!B2InafbDF zjPXf1mko3h59{lT6EEbxKO1Z5GF71)WwowO6kY|6tjSVSWdQ}NsK2x{>i|MKZK8%Q zfu&_0D;CO-Jg0#YmyfctyJ!mRJp)e#@O0mYdp|8x;G1%OZQ3Q847YWTyy|%^cpA;m zze0(5p{tMu^lDkpe?HynyO?a1$_LJl2L&mpeKu%8YvgRNr=%2z${%WThHG=vrWY@4 zsA`OP#O&)TetZ>s%h!=+CE15lOOls&nvC~$Qz0Ph7tHiP;O$i|eDwpT{cp>+)0-|; zY$|bB+Gbel>5aRN3>c0x)4U=|X+z+{ zn*_p*EQoquRL+=+p;=lm`d71&1NqBz&_ph)MXu(Nv6&XE7(RsS)^MGj5Q?Fwude-(sq zjJ>aOq!7!EN>@(fK7EE#;i_BGvli`5U;r!YA{JRodLBc6-`n8K+Fjgwb%sX;j=qHQ z7&Tr!)!{HXoO<2BQrV9Sw?JRaLXV8HrsNevvnf>Y-6|{T!pYLl7jp$-nEE z#X!4G4L#K0qG_4Z;Cj6=;b|Be$hi4JvMH!-voxqx^@8cXp`B??eFBz2lLD8RRaRGh zn7kUfy!YV~p(R|p7iC1Rdgt$_24i0cd-S8HpG|`@my70g^y`gu%#Tf_L21-k?sRRZHK&at(*ED0P8iw{7?R$9~OF$Ko;Iu5)ur5<->x!m93Eb zFYpIx60s=Wxxw=`$aS-O&dCO_9?b1yKiPCQmSQb>T)963`*U+Ydj5kI(B(B?HNP8r z*bfSBpSu)w(Z3j7HQoRjUG(+d=IaE~tv}y14zHHs|0UcN52fT8V_<@2ep_ee{QgZG zmgp8iv4V{k;~8@I%M3<#B;2R>Ef(Gg_cQM7%}0s*^)SK6!Ym+~P^58*wnwV1BW@eG z4sZLqsUvBbFsr#8u7S1r4teQ;t)Y@jnn_m5jS$CsW1um!p&PqAcc8!zyiXHVta9QC zY~wCwCF0U%xiQPD_INKtTb;A|Zf29(mu9NI;E zc-e>*1%(LSXB`g}kd`#}O;veb<(sk~RWL|f3ljxCnEZDdNSTDV6#Td({6l&y4IjKF z^}lIUq*ZUqgTPumD)RrCN{M^jhY>E~1pn|KOZ5((%F)G|*ZQ|r4zIbrEiV%42hJV8 z3xS)=!X1+=olbdGJ=yZil?oXLct8FM{(6ikLL3E%=q#O6(H$p~gQu6T8N!plf!96| z&Q3=`L~>U0zZh;z(pGR2^S^{#PrPxTRHD1RQOON&f)Siaf`GLj#UOk&(|@0?zm;Sx ztsGt8=29-MZs5CSf1l1jNFtNt5rFNZxJPvkNu~2}7*9468TWm>nN9TP&^!;J{-h)_ z7WsHH9|F%I`Pb!>KAS3jQWKfGivTVkMJLO-HUGM_a4UQ_%RgL6WZvrW+Z4ujZn;y@ zz9$=oO!7qVTaQAA^BhX&ZxS*|5dj803M=k&2%QrXda`-Q#IoZL6E(g+tN!6CA!CP* zCpWtCujIea)ENl0liwVfj)Nc<9mV%+e@=d`haoZ*`B7+PNjEbXBkv=B+Pi^~L#EO$D$ZqTiD8f<5$eyb54-(=3 zh)6i8i|jp(@OnRrY5B8t|LFXFQVQ895n*P16cEKTrT*~yLH6Z4e*bZ5otpRDri&+A zfNbK1D5@O=sm`fN=WzWyse!za5n%^+6dHPGX#8DyIK>?9qyX}2XvBWVqbP%%D)7$= z=#$WulZlZR<{m#gU7lwqK4WS1Ne$#_P{b17qe$~UOXCl>5b|6WVh;5vVnR<%d+Lnp z$uEmML38}U4vaW8>shm6CzB(Wei3s#NAWE3)a2)z@i{4jTn;;aQS)O@l{rUM`J@K& l00vQ5JBs~;vo!vr%%-k{2_Fq1Mn4QF81S)AQ99zk{{c4yR+0b! literal 0 HcmV?d00001 diff --git a/epilogue/gradle/wrapper/gradle-wrapper.properties b/epilogue/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..5e82d67 --- /dev/null +++ b/epilogue/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/epilogue/gradlew b/epilogue/gradlew new file mode 100644 index 0000000..1aa94a4 --- /dev/null +++ b/epilogue/gradlew @@ -0,0 +1,249 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/epilogue/gradlew.bat b/epilogue/gradlew.bat new file mode 100644 index 0000000..93e3f59 --- /dev/null +++ b/epilogue/gradlew.bat @@ -0,0 +1,92 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/epilogue/settings.gradle b/epilogue/settings.gradle new file mode 100644 index 0000000..52de01f --- /dev/null +++ b/epilogue/settings.gradle @@ -0,0 +1,3 @@ +rootProject.name = 'epilogue' +include 'epilogue-processor' +include 'epilogue-runtime' diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index d64cd4917707c1f8861d8cb53dd15194d4248596..a4b76b9530d66f5e68d973ea569d8e19de379189 100644 GIT binary patch delta 34592 zcmY(qRX`kF)3u#IAjsf0xCD212@LM;?(PINyAue(f;$XO2=4Cg1P$=#e%|lo zKk1`B>Q#GH)wNd-&cJog!qw7YfYndTeo)CyX{fOHsQjGa<{e=jamMNwjdatD={CN3>GNchOE9OGPIqr)3v>RcKWR3Z zF-guIMjE2UF0Wqk1)21791y#}ciBI*bAenY*BMW_)AeSuM5}vz_~`+1i!Lo?XAEq{TlK5-efNFgHr6o zD>^vB&%3ZGEWMS>`?tu!@66|uiDvS5`?bF=gIq3rkK(j<_TybyoaDHg8;Y#`;>tXI z=tXo~e9{U!*hqTe#nZjW4z0mP8A9UUv1}C#R*@yu9G3k;`Me0-BA2&Aw6f`{Ozan2 z8c8Cs#dA-7V)ZwcGKH}jW!Ja&VaUc@mu5a@CObzNot?b{f+~+212lwF;!QKI16FDS zodx>XN$sk9;t;)maB^s6sr^L32EbMV(uvW%or=|0@U6cUkE`_!<=LHLlRGJx@gQI=B(nn z-GEjDE}*8>3U$n(t^(b^C$qSTI;}6q&ypp?-2rGpqg7b}pyT zOARu2x>0HB{&D(d3sp`+}ka+Pca5glh|c=M)Ujn_$ly^X6&u z%Q4Y*LtB_>i6(YR!?{Os-(^J`(70lZ&Hp1I^?t@~SFL1!m0x6j|NM!-JTDk)%Q^R< z@e?23FD&9_W{Bgtr&CG&*Oer3Z(Bu2EbV3T9FeQ|-vo5pwzwQ%g&=zFS7b{n6T2ZQ z*!H(=z<{D9@c`KmHO&DbUIzpg`+r5207}4D=_P$ONIc5lsFgn)UB-oUE#{r+|uHc^hzv_df zV`n8&qry%jXQ33}Bjqcim~BY1?KZ}x453Oh7G@fA(}+m(f$)TY%7n=MeLi{jJ7LMB zt(mE*vFnep?YpkT_&WPV9*f>uSi#n#@STJmV&SLZnlLsWYI@y+Bs=gzcqche=&cBH2WL)dkR!a95*Ri)JH_4c*- zl4pPLl^as5_y&6RDE@@7342DNyF&GLJez#eMJjI}#pZN{Y8io{l*D+|f_Y&RQPia@ zNDL;SBERA|B#cjlNC@VU{2csOvB8$HzU$01Q?y)KEfos>W46VMh>P~oQC8k=26-Ku)@C|n^zDP!hO}Y z_tF}0@*Ds!JMt>?4y|l3?`v#5*oV-=vL7}zehMON^=s1%q+n=^^Z{^mTs7}*->#YL z)x-~SWE{e?YCarwU$=cS>VzmUh?Q&7?#Xrcce+jeZ|%0!l|H_=D_`77hBfd4Zqk&! zq-Dnt_?5*$Wsw8zGd@?woEtfYZ2|9L8b>TO6>oMh%`B7iBb)-aCefM~q|S2Cc0t9T zlu-ZXmM0wd$!gd-dTtik{bqyx32%f;`XUvbUWWJmpHfk8^PQIEsByJm+@+-aj4J#D z4#Br3pO6z1eIC>X^yKk|PeVwX_4B+IYJyJyc3B`4 zPrM#raacGIzVOexcVB;fcsxS=s1e&V;Xe$tw&KQ`YaCkHTKe*Al#velxV{3wxx}`7@isG zp6{+s)CG%HF#JBAQ_jM%zCX5X;J%-*%&jVI?6KpYyzGbq7qf;&hFprh?E5Wyo=bZ) z8YNycvMNGp1836!-?nihm6jI`^C`EeGryoNZO1AFTQhzFJOA%Q{X(sMYlzABt!&f{ zoDENSuoJQIg5Q#@BUsNJX2h>jkdx4<+ipUymWKFr;w+s>$laIIkfP6nU}r+?J9bZg zUIxz>RX$kX=C4m(zh-Eg$BsJ4OL&_J38PbHW&7JmR27%efAkqqdvf)Am)VF$+U3WR z-E#I9H6^)zHLKCs7|Zs<7Bo9VCS3@CDQ;{UTczoEprCKL3ZZW!ffmZFkcWU-V|_M2 zUA9~8tE9<5`59W-UgUmDFp11YlORl3mS3*2#ZHjv{*-1#uMV_oVTy{PY(}AqZv#wF zJVks)%N6LaHF$$<6p8S8Lqn+5&t}DmLKiC~lE{jPZ39oj{wR&fe*LX-z0m}9ZnZ{U z>3-5Bh{KKN^n5i!M79Aw5eY=`6fG#aW1_ZG;fw7JM69qk^*(rmO{|Z6rXy?l=K=#_ zE-zd*P|(sskasO(cZ5L~_{Mz&Y@@@Q)5_8l<6vB$@226O+pDvkFaK8b>%2 zfMtgJ@+cN@w>3)(_uR;s8$sGONbYvoEZ3-)zZk4!`tNzd<0lwt{RAgplo*f@Z)uO` zzd`ljSqKfHJOLxya4_}T`k5Ok1Mpo#MSqf~&ia3uIy{zyuaF}pV6 z)@$ZG5LYh8Gge*LqM_|GiT1*J*uKes=Oku_gMj&;FS`*sfpM+ygN&yOla-^WtIU#$ zuw(_-?DS?6DY7IbON7J)p^IM?N>7x^3)(7wR4PZJu(teex%l>zKAUSNL@~{czc}bR z)I{XzXqZBU3a;7UQ~PvAx8g-3q-9AEd}1JrlfS8NdPc+!=HJ6Bs( zCG!0;e0z-22(Uzw>hkEmC&xj?{0p|kc zM}MMXCF%RLLa#5jG`+}{pDL3M&|%3BlwOi?dq!)KUdv5__zR>u^o|QkYiqr(m3HxF z6J*DyN#Jpooc$ok=b7{UAVM@nwGsr6kozSddwulf5g1{B=0#2)zv!zLXQup^BZ4sv*sEsn)+MA?t zEL)}3*R?4(J~CpeSJPM!oZ~8;8s_=@6o`IA%{aEA9!GELRvOuncE`s7sH91 zmF=+T!Q6%){?lJn3`5}oW31(^Of|$r%`~gT{eimT7R~*Mg@x+tWM3KE>=Q>nkMG$U za7r>Yz2LEaA|PsMafvJ(Y>Xzha?=>#B!sYfVob4k5Orb$INFdL@U0(J8Hj&kgWUlO zPm+R07E+oq^4f4#HvEPANGWLL_!uF{nkHYE&BCH%l1FL_r(Nj@M)*VOD5S42Gk-yT z^23oAMvpA57H(fkDGMx86Z}rtQhR^L!T2iS!788E z+^${W1V}J_NwdwdxpXAW8}#6o1(Uu|vhJvubFvQIH1bDl4J4iDJ+181KuDuHwvM?` z%1@Tnq+7>p{O&p=@QT}4wT;HCb@i)&7int<0#bj8j0sfN3s6|a(l7Bj#7$hxX@~iP z1HF8RFH}irky&eCN4T94VyKqGywEGY{Gt0Xl-`|dOU&{Q;Ao;sL>C6N zXx1y^RZSaL-pG|JN;j9ADjo^XR}gce#seM4QB1?S`L*aB&QlbBIRegMnTkTCks7JU z<0(b+^Q?HN1&$M1l&I@>HMS;!&bb()a}hhJzsmB?I`poqTrSoO>m_JE5U4=?o;OV6 zBZjt;*%1P>%2{UL=;a4(aI>PRk|mr&F^=v6Fr&xMj8fRCXE5Z2qdre&;$_RNid5!S zm^XiLK25G6_j4dWkFqjtU7#s;b8h?BYFxV?OE?c~&ME`n`$ix_`mb^AWr+{M9{^^Rl;~KREplwy2q;&xe zUR0SjHzKVYzuqQ84w$NKVPGVHL_4I)Uw<$uL2-Ml#+5r2X{LLqc*p13{;w#E*Kwb*1D|v?e;(<>vl@VjnFB^^Y;;b3 z=R@(uRj6D}-h6CCOxAdqn~_SG=bN%^9(Ac?zfRkO5x2VM0+@_qk?MDXvf=@q_* z3IM@)er6-OXyE1Z4sU3{8$Y$>8NcnU-nkyWD&2ZaqX1JF_JYL8y}>@V8A5%lX#U3E zet5PJM`z79q9u5v(OE~{by|Jzlw2<0h`hKpOefhw=fgLTY9M8h+?37k@TWpzAb2Fc zQMf^aVf!yXlK?@5d-re}!fuAWu0t57ZKSSacwRGJ$0uC}ZgxCTw>cjRk*xCt%w&hh zoeiIgdz__&u~8s|_TZsGvJ7sjvBW<(C@}Y%#l_ID2&C`0;Eg2Z+pk;IK}4T@W6X5H z`s?ayU-iF+aNr5--T-^~K~p;}D(*GWOAYDV9JEw!w8ZYzS3;W6*_`#aZw&9J ziXhBKU3~zd$kKzCAP-=t&cFDeQR*_e*(excIUxKuD@;-twSlP6>wWQU)$|H3Cy+`= z-#7OW!ZlYzZxkdQpfqVDFU3V2B_-eJS)Fi{fLtRz!K{~7TR~XilNCu=Z;{GIf9KYz zf3h=Jo+1#_s>z$lc~e)l93h&RqW1VHYN;Yjwg#Qi0yzjN^M4cuL>Ew`_-_wRhi*!f zLK6vTpgo^Bz?8AsU%#n}^EGigkG3FXen3M;hm#C38P@Zs4{!QZPAU=m7ZV&xKI_HWNt90Ef zxClm)ZY?S|n**2cNYy-xBlLAVZ=~+!|7y`(fh+M$#4zl&T^gV8ZaG(RBD!`3?9xcK zp2+aD(T%QIgrLx5au&TjG1AazI;`8m{K7^!@m>uGCSR;Ut{&?t%3AsF{>0Cm(Kf)2 z?4?|J+!BUg*P~C{?mwPQ#)gDMmro20YVNsVx5oWQMkzQ? zsQ%Y>%7_wkJqnSMuZjB9lBM(o zWut|B7w48cn}4buUBbdPBW_J@H7g=szrKEpb|aE>!4rLm+sO9K%iI75y~2HkUo^iw zJ3se$8$|W>3}?JU@3h@M^HEFNmvCp|+$-0M?RQ8SMoZ@38%!tz8f8-Ptb@106heiJ z^Bx!`0=Im z1!NUhO=9ICM*+||b3a7w*Y#5*Q}K^ar+oMMtekF0JnO>hzHqZKH0&PZ^^M(j;vwf_ z@^|VMBpcw8;4E-9J{(u7sHSyZpQbS&N{VQ%ZCh{c1UA5;?R} z+52*X_tkDQ(s~#-6`z4|Y}3N#a&dgP4S_^tsV=oZr4A1 zaSoPN1czE(UIBrC_r$0HM?RyBGe#lTBL4~JW#A`P^#0wuK)C-2$B6TvMi@@%K@JAT_IB^T7Zfqc8?{wHcSVG_?{(wUG%zhCm=%qP~EqeqKI$9UivF zv+5IUOs|%@ypo6b+i=xsZ=^G1yeWe)z6IX-EC`F=(|_GCNbHbNp(CZ*lpSu5n`FRA zhnrc4w+Vh?r>her@Ba_jv0Omp#-H7avZb=j_A~B%V0&FNi#!S8cwn0(Gg-Gi_LMI{ zCg=g@m{W@u?GQ|yp^yENd;M=W2s-k7Gw2Z(tsD5fTGF{iZ%Ccgjy6O!AB4x z%&=6jB7^}pyftW2YQpOY1w@%wZy%}-l0qJlOSKZXnN2wo3|hujU+-U~blRF!^;Tan z0w;Srh0|Q~6*tXf!5-rCD)OYE(%S|^WTpa1KHtpHZ{!;KdcM^#g8Z^+LkbiBHt85m z;2xv#83lWB(kplfgqv@ZNDcHizwi4-8+WHA$U-HBNqsZ`hKcUI3zV3d1ngJP-AMRET*A{> zb2A>Fk|L|WYV;Eu4>{a6ESi2r3aZL7x}eRc?cf|~bP)6b7%BnsR{Sa>K^0obn?yiJ zCVvaZ&;d_6WEk${F1SN0{_`(#TuOOH1as&#&xN~+JDzX(D-WU_nLEI}T_VaeLA=bc zl_UZS$nu#C1yH}YV>N2^9^zye{rDrn(rS99>Fh&jtNY7PP15q%g=RGnxACdCov47= zwf^9zfJaL{y`R#~tvVL#*<`=`Qe zj_@Me$6sIK=LMFbBrJps7vdaf_HeX?eC+P^{AgSvbEn?n<}NDWiQGQG4^ZOc|GskK z$Ve2_n8gQ-KZ=s(f`_X!+vM5)4+QmOP()2Fe#IL2toZBf+)8gTVgDSTN1CkP<}!j7 z0SEl>PBg{MnPHkj4wj$mZ?m5x!1ePVEYI(L_sb0OZ*=M%yQb?L{UL(2_*CTVbRxBe z@{)COwTK1}!*CK0Vi4~AB;HF(MmQf|dsoy(eiQ>WTKcEQlnKOri5xYsqi61Y=I4kzAjn5~{IWrz_l))|Ls zvq7xgQs?Xx@`N?f7+3XKLyD~6DRJw*uj*j?yvT3}a;(j_?YOe%hUFcPGWRVBXzpMJ zM43g6DLFqS9tcTLSg=^&N-y0dXL816v&-nqC0iXdg7kV|PY+js`F8dm z2PuHw&k+8*&9SPQ6f!^5q0&AH(i+z3I7a?8O+S5`g)>}fG|BM&ZnmL;rk)|u{1!aZ zEZHpAMmK_v$GbrrWNP|^2^s*!0waLW=-h5PZa-4jWYUt(Hr@EA(m3Mc3^uDxwt-me^55FMA9^>hpp26MhqjLg#^Y7OIJ5%ZLdNx&uDgIIqc zZRZl|n6TyV)0^DDyVtw*jlWkDY&Gw4q;k!UwqSL6&sW$B*5Rc?&)dt29bDB*b6IBY z6SY6Unsf6AOQdEf=P1inu6(6hVZ0~v-<>;LAlcQ2u?wRWj5VczBT$Op#8IhppP-1t zfz5H59Aa~yh7EN;BXJsLyjkjqARS5iIhDVPj<=4AJb}m6M@n{xYj3qsR*Q8;hVxDyC4vLI;;?^eENOb5QARj#nII5l$MtBCI@5u~(ylFi$ zw6-+$$XQ}Ca>FWT>q{k)g{Ml(Yv=6aDfe?m|5|kbGtWS}fKWI+})F6`x@||0oJ^(g|+xi zqlPdy5;`g*i*C=Q(aGeDw!eQg&w>UUj^{o?PrlFI=34qAU2u@BgwrBiaM8zoDTFJ< zh7nWpv>dr?q;4ZA?}V}|7qWz4W?6#S&m>hs4IwvCBe@-C>+oohsQZ^JC*RfDRm!?y zS4$7oxcI|##ga*y5hV>J4a%HHl^t$pjY%caL%-FlRb<$A$E!ws?8hf0@(4HdgQ!@> zds{&g$ocr9W4I84TMa9-(&^_B*&R%^=@?Ntxi|Ejnh;z=!|uVj&3fiTngDPg=0=P2 zB)3#%HetD84ayj??qrxsd9nqrBem(8^_u_UY{1@R_vK-0H9N7lBX5K(^O2=0#TtUUGSz{ z%g>qU8#a$DyZ~EMa|8*@`GOhCW3%DN%xuS91T7~iXRr)SG`%=Lfu%U~Z_`1b=lSi?qpD4$vLh$?HU6t0MydaowUpb zQr{>_${AMesCEffZo`}K0^~x>RY_ZIG{(r39MP>@=aiM@C;K)jUcfQV8#?SDvq>9D zI{XeKM%$$XP5`7p3K0T}x;qn)VMo>2t}Ib(6zui;k}<<~KibAb%p)**e>ln<=qyWU zrRDy|UXFi9y~PdEFIAXejLA{K)6<)Q`?;Q5!KsuEw({!#Rl8*5_F{TP?u|5(Hijv( ztAA^I5+$A*+*e0V0R~fc{ET-RAS3suZ}TRk3r)xqj~g_hxB`qIK5z(5wxYboz%46G zq{izIz^5xW1Vq#%lhXaZL&)FJWp0VZNO%2&ADd?+J%K$fM#T_Eke1{dQsx48dUPUY zLS+DWMJeUSjYL453f@HpRGU6Dv)rw+-c6xB>(=p4U%}_p>z^I@Ow9`nkUG21?cMIh9}hN?R-d)*6%pr6d@mcb*ixr7 z)>Lo<&2F}~>WT1ybm^9UO{6P9;m+fU^06_$o9gBWL9_}EMZFD=rLJ~&e?fhDnJNBI zKM=-WR6g7HY5tHf=V~6~QIQ~rakNvcsamU8m28YE=z8+G7K=h%)l6k zmCpiDInKL6*e#)#Pt;ANmjf`8h-nEt&d}(SBZMI_A{BI#ck-_V7nx)K9_D9K-p@?Zh81#b@{wS?wCcJ%og)8RF*-0z+~)6f#T` zWqF7_CBcnn=S-1QykC*F0YTsKMVG49BuKQBH%WuDkEy%E?*x&tt%0m>>5^HCOq|ux zuvFB)JPR-W|%$24eEC^AtG3Gp4qdK%pjRijF5Sg3X}uaKEE z-L5p5aVR!NTM8T`4|2QA@hXiLXRcJveWZ%YeFfV%mO5q#($TJ`*U>hicS+CMj%Ip# zivoL;dd*araeJK9EA<(tihD50FHWbITBgF9E<33A+eMr2;cgI3Gg6<-2o|_g9|> zv5}i932( zYfTE9?4#nQhP@a|zm#9FST2 z!y+p3B;p>KkUzH!K;GkBW}bWssz)9b>Ulg^)EDca;jDl+q=243BddS$hY^fC6lbpM z(q_bo4V8~eVeA?0LFD6ZtKcmOH^75#q$Eo%a&qvE8Zsqg=$p}u^|>DSWUP5i{6)LAYF4E2DfGZuMJ zMwxxmkxQf}Q$V3&2w|$`9_SQS^2NVbTHh;atB>=A%!}k-f4*i$X8m}Ni^ppZXk5_oYF>Gq(& z0wy{LjJOu}69}~#UFPc;$7ka+=gl(FZCy4xEsk);+he>Nnl>hb5Ud-lj!CNicgd^2 z_Qgr_-&S7*#nLAI7r()P$`x~fy)+y=W~6aNh_humoZr7MWGSWJPLk}$#w_1n%(@? z3FnHf1lbxKJbQ9c&i<$(wd{tUTX6DAKs@cXIOBv~!9i{wD@*|kwfX~sjKASrNFGvN zrFc=!0Bb^OhR2f`%hrp2ibv#KUxl)Np1aixD9{^o=)*U%n%rTHX?FSWL^UGpHpY@7 z74U}KoIRwxI#>)Pn4($A`nw1%-D}`sGRZD8Z#lF$6 zOeA5)+W2qvA%m^|$WluUU-O+KtMqd;Pd58?qZj})MbxYGO<{z9U&t4D{S2G>e+J9K ztFZ?}ya>SVOLp9hpW)}G%kTrg*KXXXsLkGdgHb+R-ZXqdkdQC0_)`?6mqo8(EU#d( zy;u&aVPe6C=YgCRPV!mJ6R6kdY*`e+VGM~`VtC>{k27!9vAZT)x2~AiX5|m1Rq}_= z;A9LX^nd$l-9&2%4s~p5r6ad-siV`HtxKF}l&xGSYJmP=z!?Mlwmwef$EQq~7;#OE z)U5eS6dB~~1pkj#9(}T3j!((8Uf%!W49FfUAozijoxInUE7z`~U3Y^}xc3xp){#9D z<^Tz2xw}@o@fdUZ@hnW#dX6gDOj4R8dV}Dw`u!h@*K)-NrxT8%2`T}EvOImNF_N1S zy?uo6_ZS>Qga4Xme3j#aX+1qdFFE{NT0Wfusa$^;eL5xGE_66!5_N8!Z~jCAH2=${ z*goHjl|z|kbmIE{cl-PloSTtD+2=CDm~ZHRgXJ8~1(g4W=1c3=2eF#3tah7ho`zm4 z05P&?nyqq$nC?iJ-nK_iBo=u5l#|Ka3H7{UZ&O`~t-=triw=SE7ynzMAE{Mv-{7E_ zViZtA(0^wD{iCCcg@c{54Ro@U5p1QZq_XlEGtdBAQ9@nT?(zLO0#)q55G8_Ug~Xnu zR-^1~hp|cy&52iogG@o?-^AD8Jb^;@&Ea5jEicDlze6%>?u$-eE};bQ`T6@(bED0J zKYtdc?%9*<<$2LCBzVx9CA4YV|q-qg*-{yQ;|0=KIgI6~z0DKTtajw2Oms3L zn{C%{P`duw!(F@*P)lFy11|Z&x`E2<=$Ln38>UR~z6~za(3r;45kQK_^QTX%!s zNzoIFFH8|Y>YVrUL5#mgA-Jh>j7)n)5}iVM4%_@^GSwEIBA2g-;43* z*)i7u*xc8jo2z8&=8t7qo|B-rsGw)b8UXnu`RgE4u!(J8yIJi(5m3~aYsADcfZ!GG zzqa7p=sg`V_KjiqI*LA-=T;uiNRB;BZZ)~88 z`C%p8%hIev2rxS12@doqsrjgMg3{A&N8A?%Ui5vSHh7!iC^ltF&HqG~;=16=h0{ygy^@HxixUb1XYcR36SB}}o3nxu z_IpEmGh_CK<+sUh@2zbK9MqO!S5cao=8LSQg0Zv4?ju%ww^mvc0WU$q@!oo#2bv24 z+?c}14L2vlDn%Y0!t*z=$*a!`*|uAVu&NO!z_arim$=btpUPR5XGCG0U3YU`v>yMr z^zmTdcEa!APX zYF>^Q-TP11;{VgtMqC}7>B^2gN-3KYl33gS-p%f!X<_Hr?`rG8{jb9jmuQA9U;BeG zHj6Pk(UB5c6zwX%SNi*Py*)gk^?+729$bAN-EUd*RKN7{CM4`Q65a1qF*-QWACA&m zrT)B(M}yih{2r!Tiv5Y&O&=H_OtaHUz96Npo_k0eN|!*s2mLe!Zkuv>^E8Xa43ZwH zOI058AZznYGrRJ+`*GmZzMi6yliFmGMge6^j?|PN%ARns!Eg$ufpcLc#1Ns!1@1 zvC7N8M$mRgnixwEtX{ypBS^n`k@t2cCh#_6L6WtQb8E~*Vu+Rr)YsKZRX~hzLG*BE zaeU#LPo?RLm(Wzltk79Jd1Y$|6aWz1)wf1K1RtqS;qyQMy@H@B805vQ%wfSJB?m&&=^m4i* zYVH`zTTFbFtNFkAI`Khe4e^CdGZw;O0 zqkQe2|NG_y6D%h(|EZNf&77_!NU%0y={^E=*gKGQ=)LdKPM3zUlM@otH2X07Awv8o zY8Y7a1^&Yy%b%m{mNQ5sWNMTIq96Wtr>a(hL>Qi&F(ckgKkyvM0IH<_}v~Fv-GqDapig=3*ZMOx!%cYY)SKzo7ECyem z9Mj3C)tCYM?C9YIlt1?zTJXNOo&oVxu&uXKJs7i+j8p*Qvu2PAnY}b`KStdpi`trk ztAO}T8eOC%x)mu+4ps8sYZ=vYJp16SVWEEgQyFKSfWQ@O5id6GfL`|2<}hMXLPszS zgK>NWOoR zBRyKeUPevpqKKShD|MZ`R;~#PdNMB3LWjqFKNvH9k+;(`;-pyXM55?qaji#nl~K8m z_MifoM*W*X9CQiXAOH{cZcP0;Bn10E1)T@62Um>et2ci!J2$5-_HPy(AGif+BJpJ^ ziHWynC_%-NlrFY+(f7HyVvbDIM$5ci_i3?22ZkF>Y8RPBhgx-7k3M2>6m5R24C|~I z&RPh9xpMGzhN4bii*ryWaN^d(`0 zTOADlU)g`1p+SVMNLztd)c+;XjXox(VHQwqzu>FROvf0`s&|NEv26}(TAe;@=FpZq zaVs6mp>W0rM3Qg*6x5f_bPJd!6dQGmh?&v0rpBNfS$DW-{4L7#_~-eA@7<2BsZV=X zow){3aATmLZOQrs>uzDkXOD=IiX;Ue*B(^4RF%H zeaZ^*MWn4tBDj(wj114r(`)P96EHq4th-;tWiHhkp2rDlrklX}I@ib-nel0slFoQO zOeTc;Rh7sMIebO`1%u)=GlEj+7HU;c|Nj>2j)J-kpR)s3#+9AiB zd$hAk6;3pu9(GCR#)#>aCGPYq%r&i02$0L9=7AlIGYdlUO5%eH&M!ZWD&6^NBAj0Y9ZDcPg@r@8Y&-}e!aq0S(`}NuQ({;aigCPnq75U9cBH&Y7 ze)W0aD>muAepOKgm7uPg3Dz7G%)nEqTUm_&^^3(>+eEI;$ia`m>m0QHEkTt^=cx^JsBC68#H(3zc~Z$E9I)oSrF$3 zUClHXhMBZ|^1ikm3nL$Z@v|JRhud*IhOvx!6X<(YSX(9LG#yYuZeB{=7-MyPF;?_8 zy2i3iVKG2q!=JHN>~!#Bl{cwa6-yB@b<;8LSj}`f9pw7#x3yTD>C=>1S@H)~(n_K4 z2-yr{2?|1b#lS`qG@+823j;&UE5|2+EdU4nVw5=m>o_gj#K>>(*t=xI7{R)lJhLU{ z4IO6!x@1f$aDVIE@1a0lraN9!(j~_uGlks)!&davUFRNYHflp<|ENwAxsp~4Hun$Q z$w>@YzXp#VX~)ZP8`_b_sTg(Gt7?oXJW%^Pf0UW%YM+OGjKS}X`yO~{7WH6nX8S6Z ztl!5AnM2Lo*_}ZLvo%?iV;D2z>#qdpMx*xY2*GGlRzmHCom`VedAoR=(A1nO)Y>;5 zCK-~a;#g5yDgf7_phlkM@)C8s!xOu)N2UnQhif-v5kL$*t=X}L9EyBRq$V(sI{90> z=ghTPGswRVbTW@dS2H|)QYTY&I$ljbpNPTc_T|FEJkSW7MV!JM4I(ksRqQ8)V5>}v z2Sf^Z9_v;dKSp_orZm09jb8;C(vzFFJgoYuWRc|Tt_&3k({wPKiD|*m!+za$(l*!gNRo{xtmqjy1=kGzFkTH=Nc>EL@1Um0BiN1)wBO$i z6rG={bRcT|%A3s3xh!Bw?=L&_-X+6}L9i~xRj2}-)7fsoq0|;;PS%mcn%_#oV#kAp zGw^23c8_0~ ze}v9(p};6HM0+qF5^^>BBEI3d=2DW&O#|(;wg}?3?uO=w+{*)+^l_-gE zSw8GV=4_%U4*OU^hibDV38{Qb7P#Y8zh@BM9pEM_o2FuFc2LWrW2jRRB<+IE)G=Vx zuu?cp2-`hgqlsn|$nx@I%TC!`>bX^G00_oKboOGGXLgyLKXoo$^@L7v;GWqfUFw3< zekKMWo0LR;TaFY}Tt4!O$3MU@pqcw!0w0 zA}SnJ6Lb597|P5W8$OsEHTku2Kw9y4V=hx*K%iSn!#LW9W#~OiWf^dXEP$^2 zaok=UyGwy3GRp)bm6Gqr>8-4h@3=2`Eto2|JE6Sufh?%U6;ut1v1d@#EfcQP2chCt z+mB{Bk5~()7G>wM3KYf7Xh?LGbwg1uWLotmc_}Z_o;XOUDyfU?{9atAT$={v82^w9 z(MW$gINHt4xB3{bdbhRR%T}L?McK?!zkLK3(e>zKyei(yq%Nsijm~LV|9mll-XHavFcc$teX7v);H>=oN-+E_Q{c|! zp

    JV~-9AH}jxf6IF!PxrB9is{_9s@PYth^`pb%DkwghLdAyDREz(csf9)HcVRq z+2Vn~>{(S&_;bq_qA{v7XbU?yR7;~JrLfo;g$Lkm#ufO1P`QW_`zWW+4+7xzQZnO$ z5&GyJs4-VGb5MEDBc5=zxZh9xEVoY(|2yRv&!T7LAlIs@tw+4n?v1T8M>;hBv}2n) zcqi+>M*U@uY>4N3eDSAH2Rg@dsl!1py>kO39GMP#qOHipL~*cCac2_vH^6x@xmO|E zkWeyvl@P$2Iy*mCgVF+b{&|FY*5Ygi8237i)9YW#Fp& z?TJTQW+7U)xCE*`Nsx^yaiJ0KSW}}jc-ub)8Z8x(|K7G>`&l{Y&~W=q#^4Gf{}aJ%6kLXsmv6cr=Hi*uB`V26;dr4C$WrPnHO>g zg1@A%DvIWPDtXzll39kY6#%j;aN7grYJP9AlJgs3FnC?crv$wC7S4_Z?<_s0j;MmE z75yQGul2=bY%`l__1X3jxju2$Ws%hNv75ywfAqjgFO7wFsFDOW^)q2%VIF~WhwEW0 z45z^+r+}sJ{q+>X-w(}OiD(!*&cy4X&yM`!L0Fe+_RUfs@=J{AH#K~gArqT=#DcGE z!FwY(h&+&811rVCVoOuK)Z<-$EX zp`TzcUQC256@YWZ*GkE@P_et4D@qpM92fWA6c$MV=^qTu7&g)U?O~-fUR&xFqNiY1 zRd=|zUs_rmFZhKI|H}dcKhy%Okl(#y#QuMi81zsY56Y@757xBQqDNkd+XhLQhp2BB zBF^aJ__D676wLu|yYo6jNJNw^B+Ce;DYK!f$!dNs1*?D^97u^jKS++7S z5qE%zG#HY-SMUn^_yru=T6v`)CM%K<>_Z>tPe|js`c<|y7?qol&)C=>uLWkg5 zmzNcSAG_sL)E9or;i+O}tY^70@h7+=bG1;YDlX{<4zF_?{)K5B&?^tKZ6<$SD%@>F zY0cl2H7)%zKeDX%Eo7`ky^mzS)s;842cP{_;dzFuyd~Npb4u!bwkkhf8-^C2e3`q8>MuPhgiv0VxHxvrN9_`rJv&GX0fWz-L-Jg^B zrTsm>)-~j0F1sV=^V?UUi{L2cp%YwpvHwwLaSsCIrGI#({{QfbgDxMqR1Z0TcrO*~ z;`z(A$}o+TN+QHHSvsC2`@?YICZ>s8&hY;SmOyF0PKaZIauCMS*cOpAMn@6@g@rZ+ z+GT--(uT6#mL8^*mMf7BE`(AVj?zLY-2$aI%TjtREu}5AWdGlcWLvfz(%wn72tGczwUOgGD3RXpWs%onuMxs9!*D^698AupW z9qTDQu4`!>n|)e35b4t+d(+uOx+>VC#nXCiRex_Fq4fu1f`;C`>g;IuS%6KgEa3NK z<8dsc`?SDP0g~*EC3QU&OZH-QpPowNEUd4rJF9MGAgb@H`mjRGq;?wFRDVQY7mMpm z3yoB7eQ!#O#`XIBDXqU>Pt~tCe{Q#awQI4YOm?Q3muUO6`nZ4^zi5|(wb9R)oyarG?mI|I@A0U!+**&lW7_bYKF2biJ4BDbi~*$h?kQ`rCC(LG-oO(nPxMU zfo#Z#n8t)+3Ph87roL-y2!!U4SEWNCIM16i~-&+f55;kxC2bL$FE@jH{5p$Z8gxOiP%Y`hTTa_!v{AKQz&- ztE+dosg?pN)leO5WpNTS>IKdEEn21zMm&?r28Q52{$e2tGL44^Ys=^?m6p=kOy!gJ zWm*oFGKS@mqj~{|SONA*T2)3XC|J--en+NrnPlNhAmXMqmiXs^*154{EVE{Uc%xqF zrbcQ~sezg;wQkW;dVezGrdC0qf!0|>JG6xErVZ8_?B(25cZrr-sL&=jKwW>zKyYMY zdRn1&@Rid0oIhoRl)+X4)b&e?HUVlOtk^(xldhvgf^7r+@TXa!2`LC9AsB@wEO&eU2mN) z(2^JsyA6qfeOf%LSJx?Y8BU1m=}0P;*H3vVXSjksEcm>#5Xa`}jj5D2fEfH2Xje-M zUYHgYX}1u_p<|fIC+pI5g6KGn%JeZPZ-0!!1})tOab>y=S>3W~x@o{- z6^;@rhHTgRaoor06T(UUbrK4+@5bO?r=!vckDD+nwK+>2{{|{u4N@g}r(r z#3beB`G2`XrO(iR6q2H8yS9v;(z-=*`%fk%CVpj%l#pt?g4*)yP|xS-&NBKOeW5_5 zXkVr;A)BGS=+F;j%O|69F0Lne?{U*t=^g?1HKy7R)R*<>%xD>K zelPqrp$&BF_?^mZ&U<*tWDIuhrw3HJj~--_0)GL8jxYs2@VLev2$;`DG7X6UI9Z)P zq|z`w46OtLJ1=V3U8B%9@FSsRP+Ze)dQ@;zLq|~>(%J5G-n}dRZ6&kyH|cQ!{Vil( zBUvQvj*~0_A1JCtaGZW|?6>KdP}!4A%l>(MnVv>A%d;!|qA>*t&-9-JFU4GZhn`jG z8GrgNsQJ%JSLgNFP`5;(=b+M9GO8cg+ygIz^4i?=eR@IY>IcG?+on?I4+Y47p-DB8 zjrlar)KtoI{#kBcqL&4?ub@Df+zMt*USCD_T8O$J$~oMrC6*TP7j@H5trGV$r0P6I zV7EZ{MWH`5`DrX*wx&`d;C`jjYoc_PMSqNB290QXlRn_4*F{5hBmEE4DHBC$%EsbR zQGb7p;)4MAjY@Bd*2F3L?<8typrrUykb$JXr#}c1|BL*QF|18D{ZTYBZ_=M&Ec6IS ziv{(%>CbeR(9Aog)}hA!xSm1p@K?*ce*-6R%odqGGk?I4@6q3dmHq)4jbw+B?|%#2 zbX;ioJ_tcGO*#d0v?il&mPAi+AKQvsQnPf*?8tX6qfOPsf-ttT+RZX6Dm&RF6beP3 zdotcJDI1Kn7wkq=;Au=BIyoGfXCNVjCKTj+fxU@mxp*d*7aHec0GTUPt`xbN8x%fe zikv87g)u~0cpQaf zd<7Mi9GR0B@*S&l&9pCl-HEaNX?ZY8MoXaYHGDf}733;(88<{E%)< z^k)X#To3=_O2$lKPsc9P-MkDAhJ~{x<=xTJw2aRY5SSZIA6Gij5cFzsGk@S)4@C65 zwN^6CwOI9`5c(3?cqRrH_gSq+ox(wtSBZc-Jr5N%^t3N&WB|TT_i4!i3lxwI=*p)Y zn7fb%HlXhf8OGjhzswj!=Crh~YwQYb+p~UaV@s%YPgiH_);$|Gx3{{v5v?7s<)+cb zxlT0Bb!OwtE!K>gx6c4v^M9mL0F=It*NfQL0J0O$RCpt746=H1pPNG#AZC|Y`SZt( zG`yKMBPV_0I|S?}?$t7GU%;*_39bCGO*x3+R|<=9WNe!8jH- zw5ZJS(k@wws?6w1rejjyZ>08aizReJBo%IRb3b3|VuR6Uo&sL?L5j(isqs%CYe@@b zIID7kF*hyqmy+7D(SPa^xNVm54hVF3{;4I9+mh)F22+_YFP>ux`{F)8l;uRX>1-cH zXqPnGsFRr|UZwJtjG=1x2^l_tF-mS0@sdC38kMi$kDw8W#zceJowZuV=@agQ_#l5w znB`g+sb1mhkrXh$X4y(<-CntwmVwah5#oA_p-U<_5$ zGDc%(b6Z=!QQ%w6YZS&HWovIaN8wMw1B-9N+Vyl=>(yIgy}BrAhpc2}8YL-i*_KY7 ztV+`WKcC?{RKA@t3pu*BtqZJFSd2d)+cc07-Z#4x&7Dnd{yg6)lz@`z%=Sl-`9Z~*io zck_Lshk9JRJs=t>1jmKB~>`6+(J z@(S}J2Q{Q{a-ASTnIViecW(FIagWQ%G41y?zS)gpooM z@c<2$7TykMs4LH*UUYfts(!Ncn`?eZl}f zg)wx@0N0J(X(OJ^=$2()HLn)=Cn~=zx(_9(B@L04%{F_Zn}5!~5Ec5D4ibN6G_AD} zzxY^T_JF##qM8~B%aZ1OC}X^kQu`JDwaRaZnt!YcRrP7fq>eIihJW1UY{Xhkn>NdX zKy|<6-wD*;GtE08sLYryW<-e)?7k;;B>e$u?v!QhU9jPK6*Y$o8{Tl`N`+QvG ze}71rVC)fis9TZ<>EJ2JR`80F^2rkB7dihm$1Ta2bR?&wz>e`)w<4)1{3SfS$uKfV z3R=JT!eY+i7+IIfl3SIgiR|KvBWH*s;OEuF5tq~wLOB^xP_Dc7-BbNjpC|dHYJrZCWj-ucmv4;YS~eN!LvwER`NCd`R4Xh5%zP$V^nU>j zdOkNvbyB_117;mhiTiL_TBcy&Grvl->zO_SlCCX5dFLd`q7x-lBj*&ykj^ zR3@z`y0<8XlBHEhlCk7IV=ofWsuF|d)ECS}qnWf?I#-o~5=JFQM8u+7I!^>dg|wEb zbu4wp#rHGayeYTT>MN+(x3O`nFMpOSERQdpzQv2ui|Z5#Qd zB(+GbXda|>CW55ky@mG13K0wfXAm8yoek3MJG!Hujn$5)Q(6wWb-l4ogu?jj2Q|srw?r z-TG0$OfmDx%(qcX`Fc`D!WS{3dN*V%SZas3$vFXQy98^y3oT~8Yv>$EX0!uiRae?m z_}pvK=rBy5Z_#_!8QEmix_@_*w8E8(2{R5kf^056;GzbLOPr2uqFYaG6Fkrv($n_51%7~QN<>9$WdjE=H}>(a41KM%d2x#e@K3{W|+=-h*mR&2C01e z2sMP;YjU)9h+1kxOKJ+g*W=&D@=$q4jF%@HyRtCwOmEmpS|Rr9V_2br*NOd^ z4LN#oxd5yL=#MPWN{9Vo^X-Wo{a7IF2hvYWB%eUCkAZq+=NQ=iLI9?~@ zr+|ky4Rgm7yEDuc2dIe941~qc8V_$7;?7|XLk6+nbrh}e&Tt20EWZ@dRFDoYbwhkn zjJ$th974Z0F${3wtVLk_Ty;*J-Pi zP0IwrAT!Lj34GcoSB8g?IKPt%!iLD-$s+f_eZg@9q!2Si?`F#fUqY`!{bM0O7V^G%VB|A zyMM>SKNg|KKP}+>>?n6|5MlPK3Vto&;nxppD;yk@z4DXPm0z9hxb+U&Fv4$y&G>q= z799L0$A2&#>CfSgCuu$+9W>s<-&yq3!C{F9N!{d?I|g|+Qd9@*d;GplgY5Fk$LOV+ zoMealKns!!80PWsJ%(}L61B!7l?j1_5P#LRrVv%NBhs{R`;aufHYb&b+mF%A+DGl5 zBemAHtbLFi++KT(wv9*?;awp>ROX~P?e<4#Uf5RKIV{c3NxmUz!LYO#Cxdz*CoRQp zSvX|#NN06=q_eTU5-T!RmUJ?Ht=XQF8t)f+GnY5nY5>-}WLR1+R5pou?l@Y|F@KEX zk=jh-yq=Rn9;riE*;Slo}PfNKhXO#;FrZCf%VZ9h7W z<63YWE^s_SlAVQh6B(En9i<9%4AT|2bTQ4Ph2)pI?f2S`$j?bp`>_3(`Fz&?ig-FJ zoO7KAh@4BDOU>sBXV84Eajr9;>wlbW&OSUt&dug?oAV;`+3oBzpI18%%1wA4blzmb z-{QPYJmn_2-F$A5JI!a8+-p8Bk*^U?^f5j7uZ}jEz0E3;XbahB2iZwS&l4jj4WRS6 z3O&!w=ymQSl~7LUE99noXd2y1)9E>yK`+ouR%sTOQ@Qjt@<;lErGLk1wrw7r zV)M})+amJXs_9hQa++&vrqgU&Xr8T)=G&5Vy6vOnvt37L*nU7&ws&ZO-9`)TGA**t zpby#0X|df;etRud+s~#Y_7zlPZ=_oLg%q&wraF6s>g@;VO#2sUseO=^+3%&Z?61(- z_IKzU`+Kw;Blil&LR#qv&{rzQnG|%i(Q3zLI@gh)2FE^H;~1dx9G|AOj(e%mSwT(C z71Zp!jar*i3S|_ik_3{n0L4KavYWWZ2x3MhyU!66E$h=L+A&-s$9X_w9Q_e;+`-{ZW# z^Zn2H_I~`}!vGeFRRY^DyKK#pORBr{&?X}ut`1a(x__(dt3y_-*Np0pX~q39D{Rns z!iXBWZO~+oZu>($Mrf0rjM>$JZar!n_0_!*e@yT7n=HfVT6#jbYZ0wYEXnTgPDZ0N zVE5?$1-v94G2@1jFyj##-E1Um(naG-8WuGy@rRAg)t9Oe0$RJ3OoWV8X4DXvW+ftx zk%S(O8h?#_3B9-1NHn&@ZAXtr=PXcAATV*GzFBXK>hVb9*`iMM-zvA6RwMH#2^901uxUFh&4fT% zmP?pjNsiRIMD)<6xZyOeThl_DN_ZJ*?KUIHgnx{vz`WKxj&!7HbM8{w?{Rued(M1v zKHsK{_q=YI88@Bf0*RW@cIV@=<{eGsG21xrTrWycT7*KBd!eD2zb1R(O@H~k7>Duv zHPwp=n8;t#1>7~fuM9IaD5w%BpwLtNCe_Sq9eal4oj2DB1#<+(MGR-P&Ig%3t%=!< zS$|KxI1a~an2Q>L$s;1$9nQJal4dk)Box$YsAKgCiEGni##jr|%So6Y4J@pYBF!;~ zhXwpKhc7&QZ$=e~Sb&ABZ4o)&U~N*dSU`2G^eQh-WCe9tA}~Ae369btLlB{GjOKB@yEDH!C7Q&df^#X zi~?{rCuAE|kAjKzt+r#t6s)1h840@A<%i5(O;$Q&tD(opg0)yzgm#=ucf4CSqkqYS zaTdivk5I~#=1Z9K5M*uV6H??6s9*ynT`vzr2@%Tkr4k+Tr_ib40$fPP7$yLA$cwJ@ zF@`94=op)$x^0t+QAsNY$pi!4e7hp~gO=|yD=^8JTvTiC(HAamYEQ}t z+hR~QoKTOz%)IHEg&6iC4vP=3mw&u4wvcSwi$vNBGQE5RoSUs^l+u{A+6s~aMMkXG z+1g4wD8^Y27Oe4f``K{+tm76n(*d6BUA4;pLa26`6RD6?Rq?2K1yMXVAk`&xbks*~{+``Mhg4cQEuw+aM zaI9{}9en8DCh*S9CojIk)qh|k?#iNiCQ}rAmr&iYRJiND ztt+j*c+}Fv&6x&7U~!(Sb1eAz1N@Nf`w?YxGJdhy+seiNNZEYIG1_<^?&pm^P8W?d ze(p@$nWC`Pxqpf8d&AIGNJn#Ty)j z1NbA^Y}pNQ>OfTdiAp+WR>C6390IrFj;YZglitGH8r7(GvVRpWjZd7|r24M{u66B) zs#VS$?R*!1FT&sO-ssvW8s5jh$-O=^9=7^y z75||~QA6zLW}Lu!YOZh1J$j46m zNH|;^a$U_RKgla5h>5(igl^ek(~2nL5a_0}ipvA_Xf0k*E-ExJNld0{LZ;F^DzqAL+IZGJ7<3i1szf zxMRkQ(|@;wj9%I7h{c*{;?g%giylU}Dz{iwb(1vGK<-vlnKs!|Mb9}iTt)Rl&NZka zkkugrMiY(ng3QseY!npaOf1jo3|r35nK+eTYh*`DHabuv@IFy zG7@V!LWE0&)bvqgQ8=-L-(vt#Z-&xaOj3G@Nqw1FfbNQ`!bFEl@z)0)+#Z5e#_hQ|Rd!KrEoRn^aFz zkzYzz%hher>ixcg6fW`=rr>Nx@enQ!sQqYR{<2^|eUfw?e8;B_`T)Kxkp8${U>g?k*VhCd zp^yYLvi}<#5TDjrx@{0U$jx*tQn+mhcXsq2e46a@44^-Sd;C6S2=}sK1LQ_OUhgO` z^4yN+e9Dv9TQ64y1Bw)0i4u)98(^+@R~eUUsG!Ye84 zFa7-?x3cqUXX)$G<2MgYiGWhjq?Q-CE(|sm-68_z>h_O2vME5nX;RodIf)=No(={I z_<&3QJcPg8kAI}_Vd+OH4z{NsFMmjv3;kunMSh94VNnqD?85uOps%nq=q?kU_JT5@ zwih;eQlhxr)7d^K#-~InWlc&<*#?{A(8f^+C_WmRR{B&Yh3pxhLU9-toLz%rCPi}} zE!cw^pQlXB3aACUpacU&ZlBUl(Jo4fxpbDVwDn^m{VG||ar9B)9}@K`(SJxmAWro& z_3yzfUqLoXg`H($!I;FTudPdo6FTJm2@^S|&42H(XbSRW7!)V&=I`{;mWicu@BT7z zQs!)F9t-K|aFaMsoJ_6z-ICrzjW5#yJRs>~)bugki)ST$8T%!D4F@EBliCNSA5!fl zN;OuKbR3m0rj=rrq}5`nq<<%iHIl|euXt6QA}$hFNqV)oR?_Rm4oPnoLy|ru_DQ-= zJTDFa;zjY2p{sg zWqz0I5y>-U{xR1Rl4r{NQ?6Ge&y@N7t~Vsll=-(^?@FF2^Y6JnkbgW==09{7N}eh4 z?h`%x-LM8D}+*41ZA#EG0D9KQjc2#z59Pq zO9u!y^MeiK3jhHB6_epc9Fs0q7m}w4lLmSnf6Gb(F%*XXShZTmYQ1gTje=G?4qg`Z zf*U~;6hT37na-R}qnQiIv@S#+#J6xEf(swOhZ4_JMMMtdob%^9e?s#9@%jc}19Jk8 z4-eKFdIEVQN4T|=j2t&EtMI{9_E$cx)DHN2-1mG28IEdMq557#dRO3U?22M($g zlriC81f!!ELd`)1V?{MBFnGYPgmrGp{4)cn6%<#sg5fMU9E|fi%iTOm9KgiN)zu3o zSD!J}c*e{V&__#si_#}hO9u$51d|3zY5@QM=aUgu9h0?tFMkPm8^?8iLjVN0f)0|R zWazNhlxTrCNF5d_LAD%TwkbkKL>+-8TV4VSawTAw*fNnD^2giQT{goNRR~OwAH5%vorH%=FNNm``;VB z_N`CeB%?_hv?RK-S(>S)VQBau{&NwD>j_ zF-Hwk*KNZb#pqexc5oKPcXjOO*cH#{XIq~NkPxH{TYm*Rtv_hwbV2JZd$e=Z)-pN0 z^PH`XkLz~lpy{|;F6Sq&pjD@}vs!0PGe z6v$ZT%$%iV1Z}J(*k7K8=sNv;I#+Ovvr?~~bXs?u{hF!CQ|_-`Y?!WYn_8|j3&GBu zl|F+DcYh8nxg49<-)ESHyI0Vo;oInYTMcVX9@5;g9>>x1BRMQ@KPJc%Za)^J6|_nr zKQ#*4^Z(G>Pt6Lgrp6!zX?X+rXibm;)WBbN1WBP~{Iw45)a0toTeof%G+Oh5Wryxb zN@p5YCm&YsN!Jd$jG8^|w^_Wo-1ad{*|(#*+kcnS97j-dxV>sGIk+cCchX&K1yxY6 z`dB};!Xf&3!*LyHut$Qlnc5WEME3}4k)j3H$aVHvxg78Y3_E@b3u@5wjX7b zPLz^7h65uMRj8d}5Y1tP55ozK;r0{r?;WHL>g4laujaX3dTd*h+xuy|LOa-f%M7RA zuz#V1WlscYXGzO0Xsu-c>6UPEVQ}o>+w7v~meKw6 zfS|`8k|tL(5VDPt0$*C)(&lVYGnVeCrsb+>%XBrvR5fz~VkMmn-RV#V&X1#`XH?fx zvxb>b_48WV%}uD=X5}V20@O1vluQ2hQ-2>^k+tl+2Al20(<||vxfpIJ~|9`dJ zVH^pxv&RS97h5DqN9ZW4!UT{rMgsH>#tHOouVIW{%W|QnHohN<4ZE5RR@l7FPk$#A zI?0%8pKlXW%QH2&OfWTY{1~5fO3=QyMi3vb*?iSmEU7hC;l7%nHAo*ucA`RmedXLF zXlD(SytNYn`{9Rs;@fw21qcpYFGUH*Xmdk{4fK z0AKh-FGJC#f0Ik!{d{T7B7elr2J8>e z4=VKi^h2D=Q8&0_LHc1j$T9pQ7-FcHxZj3w-{RF}MXBm@?_X&zG?V%-Bet=g# zgEZn=6W?w3jeoQ(!&ECWHqJ zs;lJ@+Tf9MhC9~LX7*WT*0A%cJEpn#(bX;0i-*TF1j2A3zeOFlEi7~=R7B$hpH(7@ zc$q9Z%JU#Am8%BTa1gvUGZPX)hL@#()Y8UP?D?tiCHan51waKUtqypCE-ALn&``k4jkeO@}6ROkhI5oJaRd?*oW z5XmD5>YOZAT4pPd`M`dOKE|;8c#wXMeqKQ__X$u$!F<91^W0T4GtRNpyh;fxIv+8{ zOV!mig|0Jq`E}FfEGH;5uUHx|3whm^-h~cRG|loa&)cs`#D7mW5K(xZ?6+)vAgAZC zD+2J-T)KRUZh~%1{k&VASQx^y`SF+OS6KX4kyjRJJpeT){PgS47=e2L=`KjGaKL_s zUIno%SwM4WAF(xl=4hpof(h_9QEfU}Rt7%rCFq{-h?=0}Z_#HJdX0XYPezSbpFe{d z0C)YJ60>{(bbnZJLT@3P<#<0>aI5md?+Lo2+D-Fke_x?5v0p-So~;%rL+cL|`Xc=y zDo2?BXJ-XJpB{>GjhRUa08Q0fc~|Te5H?$jM>&XZG_?d?@$c3DX04&{U<}^Kj^=z zll8%>K>i=dqr$~=S9jB6O9hsxyPZc556Zw=j_nVDRZX|_LS7YaUr=}9egcpXb&Lyu z)YmbNGJh^0d;nj66%_}BAGOYHUX^~)0N68LkJ^TyJHrdKncoeHWg@5uMJ!*CaF?vi zs}inQ2`7nFmB(0lPrqn_`mS~KaI)&6rO6}?TrFA@(Ja=?UzYTXI{;CnCeCzb>5&FP zU9f&`4m+(A>lG0a8$bbgJoRdhk?tvg@Ikz#RDUy9`Bv_`)Mkhjai_S8ErG{n6Y!ZX zjPs#^rE8v{eXb(WZW}1zS0~dl)qaDzZc6#Eb{ck_GRA z#30&5L=j;Tg=w(=Im_LHt$@}KL1QA*~192~ak5Zap zUm99S=A}`1@@=9=5f6x7EHE6dJZ-x$j_M#N`oWZ#8SoMRTSbJEkaI_E1S`LPb#u`l za~4L#=6*e^6>@H+e`vvSoIfb`u^orz|9^Gmf4h-i>_^V46i#@Dxdo?h3>Vd9UB7Q1 zd*h%uq=*CJ?O?Lm(&(J#sK(r_I|5=@p*QJ8=tPJL3W(!iGFv{}j#xpF;@rMTpd4td z<_1}s1;k09u3T^?RJY`6H5?F+aq(TFbgz!+$2p?$R`cYY_JBwWirgNmvn*Q5HGe{f z-XaT1oDGR#3t6;+$vF}g;7xCzl>r&9Od6(sppYNY?IXMuZ9`V@!`mKeeSE_wM4Gd+URu(#jex(s}ep9w1GC3 z7Kw+jq#o_EXrxGYA1~6D%cM+Ge1B+?9*7ocTWaW4s-L{|jmQn!kxEX{y*KxIy1Xsk zjnC7@NQ-xSD&Z?q_a#!IA$;sPe$gu?Z@nHJio8s36Lg7G@2AP18uG-3n|dSD^zhIP z+Lua-$Q13Lqz^#~2=HF178_n9HXiZ3Ovmd`>ukdKrc^2!X-ZAeBT)7dg@2>+{JWz! z=p-xnDEg15lCRLp=uPi))DZP-pCqq%wfcyWMMo@`orpju`U#jwh%@+&z~1$+@gb_i z)6qj`VXXJU%FkkS64rkme)%TMc?)t4l%`DCsP&j<&wVcTDtWIqWv3~3;0Bqggf}`x z?`&K}p9&;=Aun6(T&k=7S$}GZhkTxv`XW6!32V~_TI%bru-U&74|$7pp-A6@^%t>z zik|j#`C5GOo6l26yv4Vpk#1d>ruU>0Sp1{7@3N40)z%`t|2VeC&_KN}@=GU4?^hP}~YUu?KOKHT)vA#ce-FMp(9pP!wPTFk%# zEwqky;$|C=p1Ezu@6K6!t$>6N_Ie-e^%}k#xcn}ovllZSv|SPDuQ-}tU^i{{+`l1; z+iYOZMxq` zyNmevH37(cCUt;!hJWefMf#0t`kVyL=P%JpzSQp?pS<i{A@amJ0F;?aT#H3gGL(m+ zMd2x(2y7PxEPwgIW>H_-O1kRG@$x~jQ_UiPlcvRrqG+t>u>Js>8_Xp<>`syJiiA&! ztVK|;R}+4AD**Ck_Nds%Xh&S}{}jiCxVtDeH;a2t6-Dft*jg0#%HQsyNF;oXVK{$( zQQY6LPpMO5t9niY*so`U_cqrfS%ttA> zMrrXr{mf-r8(+hNdUxQONMdM>QWS?n{+OpF2q5te-AZ?0^44=hA%DU`#Rc;$`A425WvPKyy?$o4V#Hc#hepIh#q zrzgc`^ts)D{=4V}+2@w~FVe?kpIh#KoUY0~x7_FGtMoP5=a&0# zq5$MRx9AIxXym?ZxgQhVvd=B|)8ZMaXDKe4fFb_31FMfwok)^Lq|q0WrRvD@ZBR=G z2pQ0I&-V@h0C*ge;YJ*jtBNjvYflqF6o%gs=t3z%xd|2&*IQdyR=^LH8WYpRgrrep z4Mx6Aw}fxhSE$jN_`x6Gk20R2MM&C)-R$h{nfE#GnVgwFe}DZ3unAM( z^yK7C>62cU)*<-~eOtHo^)=lJyq4q2*a>{Y3mU}nkX(`x@nlm*hSem0>o7{ZNZ;O< zZbWN(%QigOG8~nI>Q5dw>RYT0OXvK4;<_A&n$p-%65n=wqR{bejviAOu@}cn>s#w3 zqd~{|=TQiObS+3ii(WV`2`mPoZQ7x1xMY3^WvfM@Sq*HPLJh+LQwQ=`ny&P1^Hu$T ztXM-zVD=*VoC&`n>n>@37!?>fN*sy>#GXLvspC8GGlAj!USU^YC|}skAcN~^Xqe0( zjqx#zAj>muU<=IUs~34|v06u2ahGbSeT-uAG|Vv*Bw$#pf8#qXFt zMfw|VuC{UeT)2WpJ6&O+E6jF;;~n9>cf~Ip6j-_@&PGFD0%Vu*QJ@Ht`C7Og!xt#L> zmqlJGEh<%*ATJUmZc(FfNSB##fy_`Y-70r{Iv3jEfR|~Ii!xC44vZ(KNj#>kjsE86 zE3FB*OayD~$|}3Y&(h6^X|1 z(TcJ}8{Ua3yL1loSfg!2gTekntVO7WNyFQCfwF2ti$UvL8C6{{IPBg01XK~$ThIQx z{)~aw>(9F2L#G36*kRDPqA$P*nq=!@bbQ#RzDpVIfYc*x9=}2N^*2z1E%3epP)i30 z>M4^xlbnuWe_MAGRTTb?O*?TCw6v5$6bS)qZqo=w4J~*9i;eVx4NwO!crrOjhE8U( z&P-ZZU9$We^ubqNd73QDTJqqV55D;u{1?`JQre~$mu9WZ%=z|x?{A;q|NiAy0GH5U z*nIM2xww(4aBEe#)zoy#s-^NN%WJl5hX=Oj8cnY%e+ZYt5!@FfY;fPO8p2xj+f6?; zUE_`~@~KwcX!4d}D<7hA<#M$$MY^)MV_$1K4gr3H8yA&|Ten>yr0v!TT@%u$ScDfR zrzVR=Rjj3cjDj)fWv?wQanp7LL)Me^LS6EzBMR%1w^~9L%8&g(G;d3f4uLKFIqs5J zYKSlle?R1Fyx?%RURbI;6jq>Nh+(uYf`e8J=hO2&ZQCoTU^AKRV>_^&!W{P-3%oVM zaQqOcL1!4cYP)vuF~dMQb1#lKj_HWu4TgBXPYuJQYWv&8km~(7Mlh=5I8HE}*mJ#? zmxhx%#+9e>eorO0)eg#m6uhb7G^KSg`Cbxlf9XizZH9>B@hZcqJ*7VTp6)w1tHLB1 z1}(?)MI0$rLIUS0;Z^atECLmzzb6FE#PKdBl;L{}$M%UdWEi4$AS4ew$#8O?ZRr(G z4syuHkcGi8a#*gRz@QP|7R93=j*A$L;eA}9id+JyWjkK`Mod00;{&DlA!QJFR3&lj zf1vI*O1ec{(V=0QA?ELLVls-W``ELsu7M`3`vI4MzhVcpJ!9#^KGjq|#b-J`!F7h$ z{dUEFmBLuMbYu>nV^(S3q+UC;7s@e_qZG#+N=oo0o$G1>6Y0a{9@&9;EU2+8k|7P6 zp?HMh|8#X5UnwpxGbHw;%WXHXn_~8nedvw09V+G$(lhoq7L}=qb+OaPSD&;$TuUtG(4;py( zh)8|Nord(*d1ZH-Dmw1MqU&RKiI)26r-hE(pqnmo4uixe^`qea7(_HA_R2KjdJ4$g!)7ve&Q^b1Tf+{(Vd6vInCd>i725IomG^(Ez(D8L!4qlUAX=)EV9!3JfWLB4n1z)!ums&0UuuVLUH zP)i30*5f6tnvk?lbhL{|8I78X7|_cA3p(L9<~X5y1L3{K8Sf*xL|5gToDT;aYig?m8z^z zQ`XdEMJqC#*O|ho!7x~+MzT<5g$turF~pS;RSY&GR;6TxR)3Q+&%yG`3&ngIwR*qK&t{TERu@0|fDrKKw3=RE&t-)Xh-$i& zl5|>BSn5)z)hg3d?<~8msU=ye>CHWR!9yT;PU|$KP*qADf(V?zj^n^g~nykv^I)Uz3{78Ty81{n~ zZsS&7WH)#Ach3%UyVD1s=Ahvw9*%Wt z<42vTt%|niux3Zww13+oK)-d~G>VKHM0ov>KXKaUH(Cc)#9GFVSc4EoUbnRudxi}T z8J!VNY=4g*Y7C*Ho7#^wUVt&67&ea4^1oBw%@h^ z+YZ+eK^VI5573*KZosq?pMj(u5257?^lBu&LF9`ao`sYf9&zx;uK2iv&$;8{ z4nFUSFF5$3JHFuHORo5YgFkV{CmcNEicdQDvO7NM;484|f=_+6!)x%g1CL;L9DE%% zT=1xaKZ8v-+-@x1OZ;|0_a9J82MFd71j+6K002-1li@}jlN6Rde_awnSQ^R>8l%uQ zO&WF!6qOdxN;eu7Q-nHAUeckHnK(0P3kdECiu+2%6$MdLP?%OK@`LB_gMXCA`(~0R zX;Tm9uJ&d7>n z%9A~GP*{Z zrpyh7B^|a-)|8b<&(!>OhWQ08$LV}WQ`RD4Od8d3O-;%vhK7#W<7u;XvbxQo0JX@f zY(C0RS6^zcd>jo287k@<4tg;k3q5e5hLHE@&4ooC)S|`w7N|jm>3tns$G}U4o!(2g=!}xLHp?+qF zvj$ztd<%96=4tCKGG@ADSX{=mNZ@ho6rr?EOQ1(G2i@2;GXb&S#U3YtCuVwc*4rJc zPm$kZf2+|!X~X6%(QMj{4u)mZOi!(P(dF3hX4ra9l=RKQ$v(kJFS#;ib+z9K^#Gle z6LKa>&4oMFJ4C&NBJ7hhPSIjcOno$M6iq+l;ExpH9rF68@D3-EgCCf}JJSgVPbI1$ z?JjPPX!_88InA}KX&=#cFH#s3Ix<6LeY==wf5DK*jP`hqF%u+|sI)3HfyywfAj=0O zMNUX2pLR;T(8c+$g&}Z#q9L>(D~t~l&X^VFXp@&w92f8tq+KXMZ&o!an%$#uo^hJh z^9-RjEvqE_s%H8{qw(juo4?SC{YhO*`|H*ibxm%ZF6r=2QC)bE`d3oZ(~?;a-(mX)b!|i%p!VVP>DN6tg*Ry97gUPUJj<}OxaYL1nXE}h zxs-O{twImUw z43Eo6nJ4_RTDIQALB8H!3nq37cE6>oNG;jZZhXh!vORPsMKfzJ8_*?O7DfGmcrL8A z(_NAhSH+JE?u?`xR1|ZThDb;2Dt`9hC;UQ%94^20-MA*;<$KO0{3b&9y(ENIe@&xj z6>X23)Ftc?ax=4pL5FZ06CPOjgG%2*lbx;+sVm6EHifaku2RZ6dm2zO1s^4+O| zX?^Rl!e{47y>uJGVh+yEaNe$4U2tTYyJ3nqt9nkQP8+X`9>;yxHT1=;SB4=QU*?nq zndTZfT|OzWa_zE$8FPQtuK2+Z>H-NyCcc=wWX>wq$q7{vij#xqCQBclE;KU_SpRHh zW?)cb0G=uW2QHH@&UKOjUxp5p-v+$&z!*iIUwCrEeC5gh!qSr;%oC7--UiJO%g(@H zgQD=VC|Kd1c_uQ*S7+LyC@PW!E7G5DDhEzd%(QbXn4J;PQoYKo1+C zI4^v%{X#z$(3LimCoU9YO4kMJJG0PS25}<7q9LXMM{Esm6)13%7{fk7Wdx5wm$C1R5emYB+b4!_g{ zCYC2a7ogf;<2t!#hh+G05lGD55CT^#LlBoxIEo9C9q6 zV^AjZEfZsU6$%s=ojiXT+hlLxY4o6EhgiZ7JP-%P5cLSCVgnh(`W^-bB@{)=b3uwG zE!U6%u3dpFT>%EaE{d8bl@K+c6+w`+ju^dTU{F9&yQvzYmVNS(GoZm{D-R;bE=#wApMmV(yJpr(t7y*s2{B8_zE)_ yL|YQw3&NAZiu6_*%Ye#&V4x{Sc^DWpP)tgl235p9dFD!GE+Jk92JyL|;s5}0b2K*q delta 34555 zcmX7vV`H6d(}mmEwr$(CZQE$vU^m*aZQE(=WXEZ2+l}qF_w)XN>&rEBu9;)4>0JOD zo(HR^Mh47P)@z^^pH!4#b(O8!;$>N+S+v5K5f8RrQ+Qv0_oH#e!pI2>yt4ij>fI9l zW&-hsVAQg%dpn3NRy$kb_vbM2sr`>bZ48b35m{D=OqX;p8A${^Dp|W&J5mXvUl#_I zN!~GCBUzj~C%K?<7+UZ_q|L)EGG#_*2Zzko-&Kck)Qd2%CpS3{P1co1?$|Sj1?E;PO z7alI9$X(MDly9AIEZ-vDLhpAKd1x4U#w$OvBtaA{fW9)iD#|AkMrsSaNz(69;h1iM1#_ z?u?O_aKa>vk=j;AR&*V-p3SY`CI}Uo%eRO(Dr-Te<99WQhi>y&l%UiS%W2m(d#woD zW?alFl75!1NiUzVqgqY98fSQNjhX3uZ&orB08Y*DFD;sjIddWoJF;S_@{Lx#SQk+9 zvSQ-620z0D7cy8-u_7u?PqYt?R0m2k%PWj%V(L|MCO(@3%l&pzEy7ijNv(VXU9byn z@6=4zL|qk*7!@QWd9imT9i%y}1#6+%w=s%WmsHbw@{UVc^?nL*GsnACaLnTbr9A>B zK)H-$tB`>jt9LSwaY+4!F1q(YO!E7@?SX3X-Ug4r($QrmJnM8m#;#LN`kE>?<{vbCZbhKOrMpux zTU=02hy${;n&ikcP8PqufhT9nJU>s;dyl;&~|Cs+o{9pCu{cRF+0{iyuH~6=tIZXVd zR~pJBC3Hf-g%Y|bhTuGyd~3-sm}kaX5=T?p$V?48h4{h2;_u{b}8s~Jar{39PnL7DsXpxcX#3zx@f9K zkkrw9s2*>)&=fLY{=xeIYVICff2Id5cc*~l7ztSsU@xuXYdV1(lLGZ5)?mXyIDf1- zA7j3P{C5s?$Y-kg60&XML*y93zrir8CNq*EMx)Kw)XA(N({9t-XAdX;rjxk`OF%4-0x?ne@LlBQMJe5+$Ir{Oj`@#qe+_-z!g5qQ2SxKQy1ex_x^Huj%u+S@EfEPP-70KeL@7@PBfadCUBt%`huTknOCj{ z;v?wZ2&wsL@-iBa(iFd)7duJTY8z-q5^HR-R9d*ex2m^A-~uCvz9B-1C$2xXL#>ow z!O<5&jhbM&@m=l_aW3F>vjJyy27gY}!9PSU3kITbrbs#Gm0gD?~Tub8ZFFK$X?pdv-%EeopaGB#$rDQHELW!8bVt`%?&>0 zrZUQ0!yP(uzVK?jWJ8^n915hO$v1SLV_&$-2y(iDIg}GDFRo!JzQF#gJoWu^UW0#? z*OC-SPMEY!LYYLJM*(Qov{#-t!3Z!CfomqgzFJld>~CTFKGcr^sUai5s-y^vI5K={ z)cmQthQuKS07e8nLfaIYQ5f}PJQqcmokx?%yzFH*`%k}RyXCt1Chfv5KAeMWbq^2MNft;@`hMyhWg50(!jdAn;Jyx4Yt)^^DVCSu?xRu^$*&&=O6#JVShU_N3?D)|$5pyP8A!f)`| z>t0k&S66T*es5(_cs>0F=twYJUrQMqYa2HQvy)d+XW&rai?m;8nW9tL9Ivp9qi2-` zOQM<}D*g`28wJ54H~1U!+)vQh)(cpuf^&8uteU$G{9BUhOL| zBX{5E1**;hlc0ZAi(r@)IK{Y*ro_UL8Ztf8n{Xnwn=s=qH;fxkK+uL zY)0pvf6-iHfX+{F8&6LzG;&d%^5g`_&GEEx0GU=cJM*}RecV-AqHSK@{TMir1jaFf&R{@?|ieOUnmb?lQxCN!GnAqcii9$ z{a!Y{Vfz)xD!m2VfPH=`bk5m6dG{LfgtA4ITT?Sckn<92rt@pG+sk>3UhTQx9ywF3 z=%B0LZN<=6-B4+UbYWxfQUOe8cmEDY3QL$;mOw&X2;q9x9qNz3J97)3^jb zdlzkDYLKm^5?3IV>t3fdWwNpq3qY;hsj=pk9;P!wVmjP|6Dw^ez7_&DH9X33$T=Q{>Nl zv*a*QMM1-2XQ)O=3n@X+RO~S`N13QM81^ZzljPJIFBh%x<~No?@z_&LAl)ap!AflS zb{yFXU(Uw(dw%NR_l7%eN2VVX;^Ln{I1G+yPQr1AY+0MapBnJ3k1>Zdrw^3aUig*! z?xQe8C0LW;EDY(qe_P!Z#Q^jP3u$Z3hQpy^w7?jI;~XTz0ju$DQNc4LUyX}+S5zh> zGkB%~XU+L?3pw&j!i|x6C+RyP+_XYNm9`rtHpqxvoCdV_MXg847oHhYJqO+{t!xxdbsw4Ugn($Cwkm^+36&goy$vkaFs zrH6F29eMPXyoBha7X^b+N*a!>VZ<&Gf3eeE+Bgz7PB-6X7 z_%2M~{sTwC^iQVjH9#fVa3IO6E4b*S%M;#WhHa^L+=DP%arD_`eW5G0<9Tk=Ci?P@ z6tJXhej{ZWF=idj32x7dp{zmQY;;D2*11&-(~wifGXLmD6C-XR=K3c>S^_+x!3OuB z%D&!EOk;V4Sq6eQcE{UEDsPMtED*;qgcJU^UwLwjE-Ww54d73fQ`9Sv%^H>juEKmxN+*aD=0Q+ZFH1_J(*$~9&JyUJ6!>(Nj zi3Z6zWC%Yz0ZjX>thi~rH+lqv<9nkI3?Ghn7@!u3Ef){G(0Pvwnxc&(YeC=Kg2-7z zr>a^@b_QClXs?Obplq@Lq-l5>W);Y^JbCYk^n8G`8PzCH^rnY5Zk-AN6|7Pn=oF(H zxE#8LkI;;}K7I^UK55Z)c=zn7OX_XVgFlEGSO}~H^y|wd7piw*b1$kA!0*X*DQ~O` z*vFvc5Jy7(fFMRq>XA8Tq`E>EF35{?(_;yAdbO8rrmrlb&LceV%;U3haVV}Koh9C| zTZnR0a(*yN^Hp9u*h+eAdn)d}vPCo3k?GCz1w>OOeme(Mbo*A7)*nEmmUt?eN_vA; z=~2}K_}BtDXJM-y5fn^v>QQo+%*FdZQFNz^j&rYhmZHgDA-TH47#Wjn_@iH4?6R{J z%+C8LYIy>{3~A@|y4kN8YZZp72F8F@dOZWp>N0-DyVb4UQd_t^`P)zsCoygL_>>x| z2Hyu7;n(4G&?wCB4YVUIVg0K!CALjRsb}&4aLS|}0t`C}orYqhFe7N~h9XQ_bIW*f zGlDCIE`&wwyFX1U>}g#P0xRRn2q9%FPRfm{-M7;}6cS(V6;kn@6!$y06lO>8AE_!O z{|W{HEAbI0eD$z9tQvWth7y>qpTKQ0$EDsJkQxAaV2+gE28Al8W%t`Pbh zPl#%_S@a^6Y;lH6BfUfZNRKwS#x_keQ`;Rjg@qj zZRwQXZd-rWngbYC}r6X)VCJ-=D54A+81%(L*8?+&r7(wOxDSNn!t(U}!;5|sjq zc5yF5$V!;%C#T+T3*AD+A({T)#p$H_<$nDd#M)KOLbd*KoW~9E19BBd-UwBX1<0h9 z8lNI&7Z_r4bx;`%5&;ky+y7PD9F^;Qk{`J@z!jJKyJ|s@lY^y!r9p^75D)_TJ6S*T zLA7AA*m}Y|5~)-`cyB+lUE9CS_`iB;MM&0fX**f;$n($fQ1_Zo=u>|n~r$HvkOUK(gv_L&@DE0b4#ya{HN)8bNQMl9hCva zi~j0v&plRsp?_zR zA}uI4n;^_Ko5`N-HCw_1BMLd#OAmmIY#ol4M^UjLL-UAat+xA+zxrFqKc@V5Zqan_ z+LoVX-Ub2mT7Dk_ z<+_3?XWBEM84@J_F}FDe-hl@}x@v-s1AR{_YD!_fMgagH6s9uyi6pW3gdhauG>+H? zi<5^{dp*5-9v`|m*ceT&`Hqv77oBQ+Da!=?dDO&9jo;=JkzrQKx^o$RqAgzL{ zjK@n)JW~lzxB>(o(21ibI}i|r3e;17zTjdEl5c`Cn-KAlR7EPp84M@!8~CywES-`mxKJ@Dsf6B18_!XMIq$Q3rTDeIgJ3X zB1)voa#V{iY^ju>*Cdg&UCbx?d3UMArPRHZauE}c@Fdk;z85OcA&Th>ZN%}=VU%3b9={Q(@M4QaeuGE(BbZ{U z?WPDG+sjJSz1OYFpdImKYHUa@ELn%n&PR9&I7B$<-c3e|{tPH*u@hs)Ci>Z@5$M?lP(#d#QIz}~()P7mt`<2PT4oHH}R&#dIx4uq943D8gVbaa2&FygrSk3*whGr~Jn zR4QnS@83UZ_BUGw;?@T zo5jA#potERcBv+dd8V$xTh)COur`TQ^^Yb&cdBcesjHlA3O8SBeKrVj!-D3+_p6%P zP@e{|^-G-C(}g+=bAuAy8)wcS{$XB?I=|r=&=TvbqeyXiuG43RR>R72Ry7d6RS;n^ zO5J-QIc@)sz_l6%Lg5zA8cgNK^GK_b-Z+M{RLYk5=O|6c%!1u6YMm3jJg{TfS*L%2 zA<*7$@wgJ(M*gyTzz8+7{iRP_e~(CCbGB}FN-#`&1ntct@`5gB-u6oUp3#QDxyF8v zOjxr}pS{5RpK1l7+l(bC)0>M;%7L?@6t}S&a zx0gP8^sXi(g2_g8+8-1~hKO;9Nn%_S%9djd*;nCLadHpVx(S0tixw2{Q}vOPCWvZg zjYc6LQ~nIZ*b0m_uN~l{&2df2*ZmBU8dv`#o+^5p>D5l%9@(Y-g%`|$%nQ|SSRm0c zLZV)45DS8d#v(z6gj&6|ay@MP23leodS8-GWIMH8_YCScX#Xr)mbuvXqSHo*)cY9g z#Ea+NvHIA)@`L+)T|f$Etx;-vrE3;Gk^O@IN@1{lpg&XzU5Eh3!w;6l=Q$k|%7nj^ z|HGu}c59-Ilzu^w<93il$cRf@C(4Cr2S!!E&7#)GgUH@py?O;Vl&joXrep=2A|3Vn zH+e$Ctmdy3B^fh%12D$nQk^j|v=>_3JAdKPt2YVusbNW&CL?M*?`K1mK*!&-9Ecp~>V1w{EK(429OT>DJAV21fG z=XP=%m+0vV4LdIi#(~XpaUY$~fQ=xA#5?V%xGRr_|5WWV=uoG_Z&{fae)`2~u{6-p zG>E>8j({w7njU-5Lai|2HhDPntQ(X@yB z9l?NGoKB5N98fWrkdN3g8ox7Vic|gfTF~jIfXkm|9Yuu-p>v3d{5&hC+ZD%mh|_=* zD5v*u(SuLxzX~owH!mJQi%Z=ALvdjyt9U6baVY<88B>{HApAJ~>`buHVGQd%KUu(d z5#{NEKk6Vy08_8*E(?hqZe2L?P2$>!0~26N(rVzB9KbF&JQOIaU{SumX!TsYzR%wB z<5EgJXDJ=1L_SNCNZcBWBNeN+Y`)B%R(wEA?}Wi@mp(jcw9&^1EMSM58?68gwnXF` zzT0_7>)ep%6hid-*DZ42eU)tFcFz7@bo=<~CrLXpNDM}tv*-B(ZF`(9^RiM9W4xC%@ZHv=>w(&~$Wta%)Z;d!{J;e@z zX1Gkw^XrHOfYHR#hAU=G`v43E$Iq}*gwqm@-mPac0HOZ0 zVtfu7>CQYS_F@n6n#CGcC5R%4{+P4m7uVlg3axX}B(_kf((>W?EhIO&rQ{iUO$16X zv{Abj3ZApUrcar7Ck}B1%RvnR%uocMlKsRxV9Qqe^Y_5C$xQW@9QdCcF%W#!zj;!xWc+0#VQ*}u&rJ7)zc+{vpw+nV?{tdd&Xs`NV zKUp|dV98WbWl*_MoyzM0xv8tTNJChwifP!9WM^GD|Mkc75$F;j$K%Y8K@7?uJjq-w zz*|>EH5jH&oTKlIzueAN2926Uo1OryC|CmkyoQZABt#FtHz)QmQvSX35o`f z<^*5XXxexj+Q-a#2h4(?_*|!5Pjph@?Na8Z>K%AAjNr3T!7RN;7c)1SqAJfHY|xAV z1f;p%lSdE8I}E4~tRH(l*rK?OZ>mB4C{3e%E-bUng2ymerg8?M$rXC!D?3O}_mka? zm*Y~JMu+_F7O4T;#nFv)?Ru6 z92r|old*4ZB$*6M40B;V&2w->#>4DEu0;#vHSgXdEzm{+VS48 z7U1tVn#AnQ3z#gP26$!dmS5&JsXsrR>~rWA}%qd{92+j zu+wYAqrJYOA%WC9nZ>BKH&;9vMSW_59z5LtzS4Q@o5vcrWjg+28#&$*8SMYP z!l5=|p@x6YnmNq>23sQ(^du5K)TB&K8t{P`@T4J5cEFL@qwtsCmn~p>>*b=37y!kB zn6x{#KjM{S9O_otGQub*K)iIjtE2NfiV~zD2x{4r)IUD(Y8%r`n;#)ujIrl8Sa+L{ z>ixGoZJ1K@;wTUbRRFgnltN_U*^EOJS zRo4Y+S`cP}e-zNtdl^S5#%oN#HLjmq$W^(Y6=5tM#RBK-M14RO7X(8Gliy3+&9fO; zXn{60%0sWh1_g1Z2r0MuGwSGUE;l4TI*M!$5dm&v9pO7@KlW@j_QboeDd1k9!7S)jIwBza-V#1)(7ht|sjY}a19sO!T z2VEW7nB0!zP=Sx17-6S$r=A)MZikCjlQHE)%_Ka|OY4+jgGOw=I3CM`3ui^=o0p7u z?xujpg#dRVZCg|{%!^DvoR*~;QBH8ia6%4pOh<#t+e_u!8gjuk_Aic=|*H24Yq~Wup1dTRQs0nlZOy+30f16;f7EYh*^*i9hTZ`h`015%{i|4 z?$7qC3&kt#(jI#<76Biz=bl=k=&qyaH>foM#zA7}N`Ji~)-f-t&tR4^do)-5t?Hz_Q+X~S2bZx{t+MEjwy3kGfbv(ij^@;=?H_^FIIu*HP_7mpV)NS{MY-Rr7&rvWo@Wd~{Lt!8|66rq`GdGu% z@<(<7bYcZKCt%_RmTpAjx=TNvdh+ZiLkMN+hT;=tC?%vQQGc7WrCPIYZwYTW`;x|N zrlEz1yf95FiloUU^(onr3A3>+96;;6aL?($@!JwiQ2hO|^i)b4pCJ7-y&a~B#J`#FO!3uBp{5GG*Cni@K85&o0q~6#LtppE&cVY z3Bv{xQ-;i}LN-60B2*1suMd=Fi%Y|7@52axZ|b=Wiwk^5eg{9X4}(q%4D5N5_Gm)` zg~VyFCwfkIKW(@@ZGAlTra6CO$RA_b*yz#){B82N7AYpQ9)sLQfhOAOMUV7$0|d$=_y&jl>va$3u-H z_+H*|UXBPLe%N2Ukwu1*)kt!$Y>(IH3`YbEt; znb1uB*{UgwG{pQnh>h@vyCE!6B~!k}NxEai#iY{$!_w54s5!6jG9%pr=S~3Km^EEA z)sCnnau+ZY)(}IK#(3jGGADw8V7#v~<&y5cF=5_Ypkrs3&7{}%(4KM7) zuSHVqo~g#1kzNwXc39%hL8atpa1Wd#V^uL=W^&E)fvGivt)B!M)?)Y#Ze&zU6O_I?1wj)*M;b*dE zqlcwgX#eVuZj2GKgBu@QB(#LHMd`qk<08i$hG1@g1;zD*#(9PHjVWl*5!;ER{Q#A9 zyQ%fu<$U?dOW=&_#~{nrq{RRyD8upRi}c-m!n)DZw9P>WGs>o1vefI}ujt_`O@l#Z z%xnOt4&e}LlM1-0*dd?|EvrAO-$fX8i{aTP^2wsmSDd!Xc9DxJB=x1}6|yM~QQPbl z0xrJcQNtWHgt*MdGmtj%x6SWYd?uGnrx4{m{6A9bYx`m z$*UAs@9?3s;@Jl19%$!3TxPlCkawEk12FADYJClt0N@O@Pxxhj+Kk(1jK~laR0*KGAc7%C4nI^v2NShTc4#?!p{0@p0T#HSIRndH;#Ts0YECtlSR}~{Uck+keoJq6iH)(Zc~C!fBe2~4(Wd> zR<4I1zMeW$<0xww(@09!l?;oDiq zk8qjS9Lxv$<5m#j(?4VLDgLz;8b$B%XO|9i7^1M;V{aGC#JT)c+L=BgCfO5k>CTlI zOlf~DzcopV29Dajzt*OcYvaUH{UJPaD$;spv%>{y8goE+bDD$~HQbON>W*~JD`;`- zZEcCPSdlCvANe z=?|+e{6AW$f(H;BND>uy1MvQ`pri>SafK5bK!YAE>0URAW9RS8#LWUHBOc&BNQ9T+ zJpg~Eky!u!9WBk)!$Z?!^3M~o_VPERYnk1NmzVYaGH;1h+;st==-;jzF~2LTn+x*k zvywHZg7~=aiJe=OhS@U>1fYGvT1+jsAaiaM;) zay2xsMKhO+FIeK?|K{G4SJOEt*eX?!>K8jpsZWW8c!X|JR#v(1+Ey5NM^TB1n|_40 z@Db2gH}PNT+3YEyqXP8U@)`E|Xat<{K5K;eK7O0yV72m|b!o43!e-!P>iW>7-9HN7 zmmc7)JX0^lPzF#>$#D~nU^3f!~Q zQWly&oZEb1847&czU;dg?=dS>z3lJkADL1innNtE(f?~OxM`%A_PBp?Lj;zDDomdg zn+lVJBnzA5DamDVIk!-AoSMv~QchAOt&5fk#G=s!$FD}9rL0yDjwDkw<9>|UUuyVm z&o7y|6Ut5WI0!G$M?NiMUy%;s3ugPKJU_+B!Z$eMFm}A**6Z8jHg)_qVmzG-uG7bj zfb6twRQ2wVgd)WY00}ux=jqy@YH4ldI*;T^2iAk+@0u`r_Fu(hmc3}!u-Pb>BDIf{ zCNDDv_Ko`U@})TZvuE=#74~E4SUh)<>8kxZ=7`E?#|c zdDKEoHxbEq;VVpkk^b&~>-y`uO~mX=X0bmP!=F1G1YiluyeEg!D*8Fq-h=NyE-2S;^F6j=QMtUzN4oPedvc*q(BCpbg~*As!D@U z3(sz|;Pe1hn08P_cDQ(klZ6 z;P`q(5_V?*kJYBBrA1^yDgJD|)X1FV_*~sO>?8Sy~I9WdK5K8bc7aeNC zDb{Fe>y3N^{mrD1+GyH{F?@9}YQ2Om3t`nt zQ(}MS8M?6Vk>B=*j*yibz6QCdR=ALgTUcKx61){O@1WkPp-v$$4}e#KgK`HG~2@#A?`BF8em`ah6+8hH-DNA2>@02WWk9(fzhL_iz|~H~qEViQ(*{ zV;3tjb<%&r!whm6B`XtWmmrMWi=#ZO&`{h9`->HVxQ)^_oOS{W z!BzVRjdx5@pCXl#87ovlp<^QU;s<*d$)+|vI;Ai(!8Tjll^mi6!o~CpnlgZAK>6=V zm38^kT`D$_$v@UYeFyVhnsMZI1m`E&8<{V07>bBEI1=fg3cji*N?7pBzuamD`X|^^ zm!)2v?s|6T&H-_^y`KM&$!0!9tai9x&)5<(&sY6B`3D{$$KMAX3@&`SW;X0 zB-}obt^I;|#o_bR>eOv?P>=UC6CGTXIM+lSu?Uy+R9~O;q|c2+FafBP;E)B5M9HJgRIpF|GvRi*E+JTBI~T?T*X}r) zefUd*(+3n_YHZZS(g8)+7=pNV9QR^>Qs8t+iEpbJS!9;wio&9rn=19C0G#Ax zM-tWHp_YlJvXWsUqJUr^`OYFA4wkgL`cSOV;w4?tp>GT1jq}-qPoN zp&G}*;+#+Zh&vqDOp>gRL#^O7;s2yWqs+U4_+R4`{l9rEt-ud(kZ*JZm#0M{4K(OH zb<7kgkgbakPE=G&!#cNkvSgpU{KLkc6)dNU$}BQelv+t+gemD5;)F-0(%cjYUFcm{ zxaUt??ycI({X5Gkk@KIR$WCqy4!wkeO_j)?O7=lFL@zJDfz zrJJRDePaPzCAB)hPOL%05T5D*hq|L5-GG&s5sB97pCT23toUrTxRB{!lejfX_xg(y z;VQ+X91I;EUOB;=mTkswkW0~F$ zS%M}ATlKkIg??F?I|%gdYBhU(h$LqkhE!Xx$7kPS{2U4wLujF_4O+d8^ej{ zgSo(;vA)|(KT8R_n_aQ$YqDQaI9Stqi7u=+l~~*u^3-WsfA$=w=VX6H%gf!6X|O#X z*U6Wg#naq%yrf&|`*$O!?cS94GD zk}Gx%{UU!kx|HFb+{f(RA2h+t#A!32`fxL}QlXUM{QF3m&{=7+hz@aXMq*FirZk?W zoQ~ZCOx>S?o>3`+tC&N0x4R`%m)%O$b@BkW;6zE+aBzeYi47~78w$d~uypaV*p$kQ zJf34Q+pp~vg6)yeTT&qWbnR2|SifwK2gA7fzy#W(DyM^bdCjnee42Ws>5mM9W6_`j zC(|n5Fa&=MT$$@?p~)!IlLezYa}=Uw21^Fz-I#?_AOk(7Ttxm;#>RDD_9EloqhvrS z&7fpbd$q_e21Al+bcz|o{(^p}AG>jX0B}ZZRfzk$WLbNLC{y|lZ|&a(=bOE6Mxum{ zM=Nd+-I2A-N&2giWM2oAH`O&QecJn6%uYl0GWlpx&2*)BIfl3h&2E(>#ODt4oG}Dq z__73?sw2-TOWq@d&gmYKdh`a}-_6YQ5```}bEBEmWLj))O z?*eUM4tw0Cwrr+4Ml^9JkKW9e4|_^oal0*sS-u_Xovjo8RJ18x_m7v!j$eR@-{2(Y z?&K4ZR8^T{MGHL#C(+ZAs6&k}r07Xqo1WzaMLo9V;I<9a6jx2wH2qeU?kv25MJxoj zJKzX`Un|;_e&KY%R2jU~<5lm-`$EjIJLDP~11_5?&W#t3I{~+0Ze++pOh2B4c1Mde zSgj$ODQQm7gk&w{wwfE1_@V(g!C=2Hd%Gwj{{-_K4S|nZu+vk}@k(?&13iccsLkQo z_t8#Ah$HVB-MRyzpab*OHOp zl`$tEcUcF9_=3*qh8KTaW$znGztA7Obzb`QW5IQN+8XC=l%+$FVgZ|*XCU?G4w)}! zmEY+2!(!%R5;h`>W(ACqB|7`GTSp4{d)eEC8O)Mhsr$dQG}WVBk$aN1->sTSV7E)K zBqr;^#^bZJJX4E_{9gdPo8e?Ry>ZrE&qM)zF5z20DP0`)IIm_!vm&s2mzl z2;EPI{HgFH-Mp&fIL^6f74>19^>o^AOj`uyL0+Nb##Slvi9K4LQSs>f+$j?cn9Z__C zAkyZ9C;#uRi3cDYoTA>AT<|*pt{K70oZKG*S1F$r?KE=$4~W3!u53yUvh~(kMrClS zXC?Dmgv4iS`>~wBPJJFL_C8x2tEg*PCDX2=rHQ@z+Zs)Kkr;FYG`GnbUXqdipzvHE z1aZ>G6|e`}Q#)Kru0)(SZnUCN#dN2H zd1}r&xGsaAeEed9#?|0HzMGA7pl2=aehy_zsRV8RKV6+^I8woDd%4J8v9hs$x{ zl*V61wSumovRVWtetd1eJ%i^#z`_~~^B;aeuD`6LgHL66F0b^G5@om^&_3REtGmhz z%j^9{U`BH7-~P_>c_yu9sE+kk)|2`C)-ygYhR?g~gH`OK@JFAGg0O)ng-JzSZMjw< z2f&vA7@qAhrVyoz64A!JaTVa>jb5=I0cbRuTv;gMF@4bX3DVV#!VWZEo>PWHeMQtU!!7ptMzb{H ze`E4ZG!rr4A8>j2AK(A0Vh6mNY0|*1BbLhs4?>jmi6fRaQwed-Z?0d=eT@Hg zLS(%af5#q%h@txY2KaYmJBu>}ZESUv-G02~cJ-(ADz6u8rLVECbAR7+KV~a!DI83H zd!Z(Ekz%vjA-|%4-YpgfymMzxm_RjZg%ruo zT4^x)f*%Ufvg_n`&55cK;~QChP6~Fy_Z67HA`UtdW)@$Xk-2+|opk6A@y0~3Qb;V% z%+B@ArKl|Q^DJW&xuBZD#~SurH7XXf*uE0@|ccNd&MA%Ts*1 zg7TU!xY}~*AOY+tAnFR(Fu)e@^9V!Rm65$;G$-?6e%7w7p9WT098%-R?u#J+zLot@ z4H7R>G8;q~_^uxC_Z=-548YRA`r`CsPDL!^$v0Yy<^M=Jryxz5ZVR_<+qP}nwrxzi z-)Y;nZQHhO+db{>IrD$#DkHP%swyKhV(qn`H9~3h0Bd33H*DAP0S!ypZqPF^1^tZJ z{z;HN?$WJ5{0jQNzYOc|KbJ(Pr42~YhW5ohNdY*rEk=({8q+F}hy)&ziN(@q1;>jL zBN<9(k1N!p2D%uHF0NxFut`XwEMc@ZH-|95>U)PY@}C=bmV_*dakL}J5DUpNZi-y& z+{i0>H@c-g|DBO)HJ>7$VVtn)z3X}H`FuN-t>gcqLas?Lk@MJb5?u@BTn0Q}E(}S~ zXrNX`ysRv*iOn1v@fBDeSDvvR>+;o>kj ztRqEZOWN!fqp(`XQ3ppvC)c{AeyS6b_8pN1M*~0=$U;P31!~Px`Obrz;GNs(8RrJvONy<{Dk1x0z zJJzhQBt{J@&DP6cHugB!q?xi~O`yJYHUsTI zmgulx%I<*?vPSl(!tj;LL$K*k zH(*d31iyB9aYAzw49W&qDi0>f;b5kA31nz(%2W`QFJqaX0&hM`KP1gfdRw?7@}$XB z!^cUI%C!?X!QVQxbqEFSbuP0>_3MTCof6!e4LMAfGRd0;Lt+w0WK@b4EkGHRqX!h{ zrYxwwH&-fM67X7zP&Qpup&vAOaKH|S*pcbI{ksFg@tfw)paaK)5khkys0GSTnAtfC z{mVJkCXt|G-SYwt0O4dM8Hf{L*&^nOeQ271ECyc5Y&z5R0%hCq6~} z$XW$kcz!nnCTAl}NyB0#ikwyg_M};inG%*x38`EYJ%FXdj&A`g)-wJ(R=C`O^r{W` z8$1r{G0X4g`uD+}vw4`H5!*B8TTsmeaYGk3x0{&aar7ocO6?dlGbyV480<#{%^93y zF(ei<%{OYi?n?L9#HL_R-00#zRzbbwVnJ0zt}4f|KNBkT6&=Kb=$E(@aC03vU~p)7$XA@ zq5*`*4Y&u*=Ju>+x}q&Xxsjn;Dd)6Otudner9zi z<*LpeG}*vJ58#P4|qXF-ul1|u*;=-@oGPtmBnQW6VY9(s`5GMsO@!;s_PKo_? z3HbGokZ|vaAA-guf5W0JDwpV}1u8;7XJ=wD;NgcLIJW8S5w!c%O*zU0%~)0M)`!Al-+OFsmPW1zniB%fqF;klqxz`Y z2@srWa3e?B3ot|nhE|Q7VIjr+$D7F^n?wm5g8w?Ro0i72K3u^g)&&F^9~@eHd33YY z9LR!!orc0vq$sd~eR~hW{4?R3Di;~mz{^G1X?#-!|Cli(#0-sm|GHYpcab`ZA=zi3 z5*m>sJyOij{!PgIJa?A0%wL*Ur1fLJdJW$a>&Xj5p_IO=SwyTp@nn&@6L4vIfT79aPyo{LQ4DhIz1 z5g*+hII!(cLGHc5ROH&^^o=02r*x>MxMPx{JFMmNvzJ?AI8p!u_H8L1a`{6~bF@L* zxszth=`>%Vi`=E{jJKd-+6pf^vo93EzqFfTcr)A&V{rERu__UAQVyE1imol78AFmB z7T;pNFxW^M+O3#;Tz^e*`AqsD?M*wPT6pnBFPA^kOTnZYHr@O(JUQ^#6bD&CC*?HG zRAKSXYv9DU)L{V(wM=te@V@Db3}97Sn9r2nroOz06!qV=)+%EKB^MR_K}p$zM5OD1 zzhYv+?%A`7dBrU(#&1hXF;7lzH`nENZKP2I{qp^NxBA8~N>?1H@uZ~Do{d+|KYx9I z_z)J7O(;xu0%0n3o4y7LnJKRPK?RV@_v_YLogYPH;}`>cZmDVyO#%-IMQVq6z9r>@ z?*AQC$=?|aqrY8xGx%vfk0ZeByTz18IrP0XTVlJyRx5!NALYPyjcn|)U5jl^<)_KZ z2C?1|dkBZ;h8e#)3gUPfdf80xu^8evspE%Xf~x zs%phX&YuB{y}>%PuOG>s&EW}5Y0`dyseV)!C|`1(U{Nd4c4>07ZFmdTJS2T3+dEw8 zK%f_x!O?H8+_Qd>$DsYNY!?tC^H;N+!fQS{!4-9c^;uXx)D3|joo_FlBTTdDM4nx{ zPve})D_u{PG>&^G=>$2N-dZ!eMx?9X7FmPNo)7|>Z|A-mNZ0{+884L6=f-{Q4bN3y zAWL{oJIh(js2$bDTaV&bh4Fn=4^M?@N~+$IXxytdnI4{RkYA$8j(}sb2TO$~49JHz z0$K$WB@axSqKsyG>m7&3IVR+?xXLfs7ytuJHH8{`ewhkH;?H7#an)*hPiBLi22jAI z{|tZ;dU=nDUVyfIurEm0VoB6kiaK#ju6RV?{3qaV`NQ4&$)fc4AAVKiXu_1$86nxh zX)Mif*|y>N;S~7UCXQhs3-%nqNuTu>=8wqtp$-#tC?bwc-{&k&0>0nRBku-b5X931zqll&%fn$1$->@El+EIA;L zfEYJY)kaTI%H z{A%hpZ?Xt=;#(++B0e)B>4_a3E7h#8upWz!G;VQBX0rjzKvy9N2LECS2@wrBoS;4G z1PgI50DD!wtwsZ&JoAGuum9s&+0NI&_n}!kUTvpD{tyG9jlSXyQ)m9H8VXoDY$j!w zo;imjJKl;E5u|n4Q?HQsy`*&=VY`SG+YFUqG*+;A9(wKfm_|6^SWh_6>1u63)H3zEGm5Uk)#z>J0XC1L+&pzieqnAo+7zlr$M4kl;-h zjo^h7U5Y3tbY@(_{#h1et^{nbOP9Nw*tJOD;WejSG-4d{(2X$tDM@-rK8SbUqMe}%IPqxOV}m#%mq0)auvNwT2R9)$1-o(2o zpIS;qwy8m^tEBC99O}bYKd7ALbB~$d<=eGd>WML+U0aAl>{Uc8CB|oVWMt zbPe9+6&V{l2Th1)Jx`K64?gUC_<>x#Wk*SOSA<&A=j2q zo_M`Lznpsg1h-W546hm(q@Rf=xL@w5QJ;HxIp?O`;sOMovgc4n%D5`kiDO6%Rhe2^ zzPa=8pd(2&HN-=5JzsiJ^(ZlLVpZD^5!$(rt0PVLQCzh7s#6_N1dRKtQv_vTgSQT5 z63+e@K`67zjbb@QdwMNF8G29tcxAl36SZAGxolCj9aS%>(Tl*6a0eW@3j4!&d!12v z%+~Xc=>VJqBcW!D#JX3#yk4O^;#|O3!ol;J%t8>wc!*6`+`~%?-QE_M{wa&vg14R~ z(M1VT-&l-M(N1>3pNjVfvCIk}d|H4&*7{*8!W-;^tFgD31O%~NtUaK_*-m7CSEt}T zm^Z02X#cQ$Mcw}TG{>1I`vmvNoxujnPra4aSwP55x37=0VvyV<)68QB-b$o-h7p*V z#QQ8?A7`=m`*+dTfYdm=;i1ptR|In}rUF^r&{bKbI@5DT$JEo;?-N}Z13}n16v?G2 z{?@ny^7|!rg(on8b97#GupiPA<(g=o;@P`4 zEx06)SiGKkIKFHzK1M`ctf?vQV#b-{ws=+0U^*LYoTK*pu;A#NB$$I=Tv{LLVQin~ z@aGTp?J<(c_1M!Jr8MK;XA8fcB+*DkFF@oAhQ=B1o*$<@;ZdGs_5O!BKi8XjF2L4n zA&(?SaRDWm+p0UTFXj1prs!*v$(q+s=8S1h(*H8pd5*8%HGN0mgw3yvfsxr4QYT)o zzdjal^6zA56|Z@csYH^3Qr2~ZR#p|Huuh0Yt|$~>oQZJDF75aeH%UlQv)fQ=3P{i1 zRt99gL`$b61Q`pdos?W6yd&%2IWK#}$wWOa9wJW&($J4h0M|9sFtQu9k)ZtYEQ#vu zS+uD(3`7T~t?I;f%z8N~nG&FVwxGXrTL!k9s#LB}FSo;a+V-j}H^myGwQq@jTIycD zP5A{w+a;^kOQW^C%9W{j^&o@)3!v~U(?wx42E5G*bd82&a1p6ax|pk)#8nG9risCw zOERH8;tq?Q4ymxf*9_aF-sTpLvETwD#sB#ID1D+WohEt0s557Ij5)ldexY+diQJ*l ziBo;1v*vx(F|lI8udAo450QIQTmPqf(7oULr5*0dE9i>i#D&k%WyfM*4{*?_%9k>g zg1_1%x?#`Xm7M@YZ?!zJs$AxS&8sBLI@c|-vSiG<*OZyw>CL*p6#N~p z#VywqpWdZ;{ylc5d7W8E7Jx_H+5e#N$h#{ni@#TlGqz`yah-qCC_;P8?N*>CPJ03b ze(YVDvbIR$#lJEkuf}L7F8q$fKCWz&>{uFg9JgTOmA*Rux-{|#+pO`!s!!4;PlE%9ys+;|)oK%&V$*FH!G2%|y(zz>X zUwdXer0HIIJkelANg_W!ofsyiN{zi2=}G1UL{`V81}1D1Sz zviLV^w-$RE9fE4@H+ys>u;OY!sgqe&V-oFE9Fn$P9HbpOI{}esLIvc zV5S-9(XjFzn1qzo2owwg_d%7_)cR*!d&%@S&D($cFFMXXd!GdUxw5tZ_W@zRbjVfU zzx13(Hc!$teqA2WOYo^+SHpRz16DOcYqaXHSMZl2Ax$)f^WC??al8lfX9)O_p9#Ml}LB(N8yJ! zj&_UD9K54Rt#yqvhklEMZ3bRC&)(^h`#kzq-#_QN?J6eLT$ zMWG-mP;HkB@5;2*lAP&1*4C)HWEs{gtp15Y%y|*%(3UOMu*v4kTi0@pWvg2Y%7yI* z%XNlZa$@AZ(Z#Elv`5MUei~VFCjF8El)@g&>(v;E; z;laavf&ANfk9*0LA@oP4QmbCBF-lB^Mj~wo)eGG57gqAKC>Hd80Eb+7b;iJzV5RsL z8>ddQH8PnC;l{M(t4c$M=q78GW6=*d#c`-jK$q#-{9c)UNO4eLm9c!DWcCth4O-FU zboSKPhL-lq3q<)m8Xw7+l=Z)H=rGgMI0H?KrPjc;iDzY5g|Ve$8?SE`8*sb1u*>dm zD~f9~j2H~6Oo2`_1 zq@_mmUbFQV25E7XJ)zBRQktT12@qHHy-@aCdAFWv4iZVN0B3}E;k(jg>X|eqOrqgM z4yBUuA*BHdnN9v;5>3#L$NFREyHW&Q*rWYa_q zhC~>M&bMFgXC6AeQ`P-s<}Ot_x^cb51r7ArPbRRs&Dd_TEeugnjR(O#V5i6OYjzRF zw1@Rvo;_wEfQA@P%I^9ljrhxxuqf9g^cWSKq~+kiVxa`&EBDqmB=C1G+XB7`TQeiV zR_k?`$&W&+ntIPeEtM9hqcj|yfW>x7&1Ht1@;!d#Wo%1hO+^Q{E?VD|`-OvV9G?tp;6{sI%L-u)Hw z;|`uN6~VqZ!g~K#B@W7?wDcbO?XS4hnW9kS1Hbi=U_m*~7`N~3oK;qFTX$$LQ#CkL z6I?a(HkF8SKJU8mT{K35ekfP3`05!M{gmrV0E-=IyqP=N;K<&jOnPcjdXrbk$%)z9cUe|#I0unK5^+qGx8#2 zz_!bmzVG*Uat*&f4P>&sV2RswlITV}wPz?_;(S;19}e}54fP|K5l_c2kU5(-Zh!7t zz=B2HktD~ap{s%*CDEl?x6o+91T-xH895-S1}M=*KhFM7Nm&1$OB++Robv0T`OBcJ zXNX%Xio0_ryjr)!Osc7au35UM`B}Ru4zN_o+C!+s&e7|}Zc;5?whP$@J@DE`>w-XH zlVmbrI4|-Z^2^I^EzuYKD+JA@8lx%>aLFZq7KT1~lAu}8cj$<-JJ4ljkcSA;{PNr)d-6P5Z!6Q=t!t*8%X)a|;_92=XXN=WMV))*gWR-wHzU(G6FPTfSjd9) zm8e1mfj4qFmlXO*a3};$&jgc$nfG>NR&iao(jYk`%E75h=K~dJ{Jqs%UH|aGHL8)-1MOyS2B?OJsyeA_YbGMDpE+>=NFcyoI;N z>1>3G4QR2~EP{L{x2e@E1U0jGGV5H$aeigDq&Dr zQ3FwJ+& zndX7VK+XD)t06uUY=)Cfo!ke%uDpOmq^bpEB`iv6(CKTGgEZUi4ddfNXJi_z4;)ob z?R+qj2SYX*zi8z=DXChEEDW+Cy>w-0agE|A7MoRJ4}-(|go-rP#sr%a(5k%wV z&Jllj+6XuSoIfZX9|mK!bbd)7TuaHBvoa(`9C$*XUh}hH1;Q7cTJQR)c>h}Hfr$aS z64c7#D^f{mN3s#2=SEf1$(*Vj{vZjF6Qc{a=VbTske7L^EY&A1I1sgXaYSH7(lF1V zZ<7`Rq33WZuu`!HK$wRr1=uE}#&JMftnZ&(P17gWF;>$TA&$ZQnIz>blTrW@49Z&H9yhgLBpFw(57K1dbIQW4fn1X(IiFWEKmPzV8gAa|ak)HAsmcQ7stP|q0hEzBNL=4YdXEkyfS zF+K+CVB#~(qd7eeZqR-VKIYJVmK2ePk``4I^PfQ*C7NUR z`w9lb?iHv2$4_p-+a+O}Fq6SnPiz>aV!~d=l3VdgDuwAPMR9eR`)b_`lg~{oX0lf1(zbBrnj4+-q zOl^#`)XKn=`()B-jExviKVTYrAKa27KAg3cboG+}D6*R;<`GC-b?i=e;aV7n(}XDS zK5xAEV=T^r#eThV+3C<^H>SuvAP&fw;Yn67eY%4=Y(p$~!`~h12 zQHM|f0#pQP_s$Q+TtMMvBdjQbLWw9cW?gl_+P z)2T94UJaYG2!yXITYjYl-@#5_47g{N|5=P~m|e}-F)*^L+{7O$#wv2e##5Y=A{>jN z6NhQSor9ulwP3gfxTF?V`P7AJ#E)ij$I`gc2fnmp&9w6qS2-Ct}6 z$#O%mKtP>I2VUBMt^Xm3LjP*D=xEyV?|8Psb91ZEj=gM(C3^Kcfvbx*$NK+MhP>W;OneZ{Q>eFEmxv}%ZCJ32=zr_OZd>6~v@ z6+3JzX%9qOvKS393r&R9O+te&#?{Q9nLkOV-eLg9!{WK}WyUWLZ7bQ5u26*u9c*T1 z_s1)j1k5&b8&5@YnmtS{tsmQaLW2%8D*8G-9w#PcVQh6sQY`!tBpU=8EZR!zfB{f{ za<+Err#ZNM4JEx5n9!zuC#KmeI*%tRXP}jpswzymT7J{YpXdzA{J7K)j1tBF8B3DL zZXkec{`rT_{__t_`!E7veO1rg1tFzVeUTBjut*3ZOq}A$r%sWXn4v4|rA+7uMvy9n zL~2WHKLg$BeD2Wq%?frTUM^c}?K?3#L+Q2-?PR+e1Fn-XUThl8^}8JOyDZz-wcFh5 zYJCJ%J_Pf~bX(0A?Z4hGw(mY?J$j#Vo&@9O>in*f)*`H6&(Z-5xx5}$V@dR)-lxgN z=DMA_EJO4+^w_+D7N>4=%{6AbvpDG<(b)xE5Ezo~oEg~cEM?mwyY?3ZtFE;RyDS`u z(^sa_s%B<)vktqh=1|?Uv6DXsA`D^B9%_mXqx1C=a#KurOE?49)P_ixiHAA)D)oqEjQ6_v0UC9mTtMu&kf8&7uRiiigPD{$Cf(&DuOj0 zr*5{zPyO@Kq(|Ttu@wxKanV=^OPOjh-_$MbNz})ou6*9nq_XQo86WJ@JN~-b=Ln_8>Nz_ZS#QpRGt+bzH*-;{#x7PFqie+ z7p5e})fcDq)J2z=z~%nrFGFjbVu~0ICDHW3=HgtCW)?Z(%Cx$z!QuszcOCe&3!Al2 z`793RnB{Jj4QpQ2N#oKT>aY~aNxz_6B2&vPdJadbC4qp#H^<@o50}m>7WR?NO0$ZI z9OKTM+jxMFWX9mi7(@j)1Ji6~?HLU!KT0Y5a^-?|XH^B?R@T zn&a_U_XFAsGrNX@S~g1<=uz@~dCcZO=1??VC@PML{g}lbuN?j|_1S=dJgbT~o}}hs zP_uYZ&0+mWY1fupe(+6nn6<9-)Xluk97yX-!!lqSXq~!kL-=+4$Dy>O$sKO7M^1QY zhZGZfiNQu+?sef?E>5sqj$kHmf;kMv<>Gu)!^4!#7T009vBzq(m2aoHu#+93HBq7T z;Fs8IHvUlmxCB2hkDbm&xwFQcXUD_&sdeu|EYhFpf7v5_LCcVua9aunVe)qoGmyg# zIGlj&IrLKg=id@t7s916d&Gf(%X7^FFR9^bz-;*o1~Sa=`cKfJ0i}X+pBKN=?}!dP zg`ZMtP6xSuvHb=5HYH%ELaGxwqH{ zpY>Ic^}J!OwM!VmNM!$nUg$qN9DLtKuBvn1(x-P+tA*UHoOc727>5?^J;JFo_ac@) zU57%w^U2ME z@z^ZsB!AhyOscE8;~Ft$)NL)GcLteq4d32fw??L0QuWt_M9IJMgZ71Jm%2khx|QN+ zkm4zQ@OjyM+l=Rv(!k?%cYwnf7HWs^M+P^zo5o?7;E)V0v*zf}(;?ms0oUK)wKmZY)mSTGN4X@2=ZU!Gy73M(ftmHJHLFKQDcu`d% zeqiW{G`?}AtEP zKCnHuWzXZ_Hc>{cP@h~M$#q}kG{52%zmhATR3AbNGR~*6(%^Gs@UZ3i%7%PJ1mB^S zcdcrFDbD6lEJGZ4k6JT;eB_JbgIkkOqkz0I{q`d^kWl6a!%w4V?Y!;8%uU(-UA4Ti z{pv2+5CN^ba{ALpu1&qm`sMP@_L=-a)@-zC1*`f)uV5MU$xJj51%?S^ zoo@;kqY@4Zw0B!+hIvTT8KK*~9H@u54r>s{MX_|#z`Z$55bDJo#=hz~k)7CTbf>Gn z=!u;@JViT~(>P7UDdIOL;6kPDzOZNl16jLo5tHS4a%~T&AlicnCwZ5pZ;+WIB3tJE zv|J^!X0Kb|8njISx#zoB(Pv#!6=D}Uq(6Dg*ll##3kfDxdHdBXN*8dZOM0I{eLTO4 z=L}zF35GJX4Wee`#h=aCB+ZV0xcaZiLCH3bOFYTmEn0qf?uC#lOPC7>+nVeO1KQ@S zcZ5Z0gfk8hH03QrC@NnEKNi15bWP;FEKsGi0iUHN4L&2_auv%tIM}UFfgRyp5HWt()pn#0P9+xF2H!8zMqf`WJ*9YB zq~m+%xLtVjza4>CO4*%thB2k;Gv1Ani%8)IP6Pm^BAigXgOUHWcQDEgB??AtdsOx5 z+pXKfU4>+8ViRUJ;h()e88jRLEzSN7%O|=MovCW3@VxK@Z*xS$WLG=u_Nenb0wP@Y z6zs##uQ7oFvcSdh5?6kZ!%8l$Xuz^Rc!lv4q?e$mv(=#@x)s_VFF50vGuE_Nr{4zXB>y?7FOMC5^sBZr`mS*t_@%LYN9wl z+lsqD#V5JR63GEr9^&9*f)kFs zJ-A(>>!h~d0%9*wd+AY+&oryzurfV{QP{&-AtDs}#iq;dal?A9jE;huq2gExb3z+- zVQB@UHlVfsy1$)dF`dcZuc(GLnim09jrI9nJ6<#=03FVrkuINg2`RTPloS^^@KYD6 z1-C-Oj2OI0y9Tdx>=dNHhOYVvx!J#4EMhold-PGClLuLA~k2VDl6cPuV4lI5c(w9@7sllth~H@)0+v~XYqqC6&*fSX~S4Bii^0& z=M)D(5FoZsKxB&M$J_7lbS>$kF=@B|Z$#D|LHJQIr$aO51ta6s96Ug*Jk;|>9Yd$! zoF2W+)lFzY)J<>U$PHwbe9>BKLAeo~e%=Qy#qhvK&`)b2 z(U9#8bba`eGr9tr$SvM4`y`lLavOzPm`l<%-(R<1urb(AX0RE=R=#&QI)klkwrJ5%D5YHZ!~s zGwK?zKZeX|uO*Y|xLjO#6uzO%iXWsSE8#zLOWc! z&2L8sdT;bhUW495)_fGCcOLM-@DfGcb1xjf(ezYJxYOv<7YE$lBCrkbfBA{`I(GH- z(yHy1h=bg~fE$aIbB_3l`|p$R_p0b(+aL(~b<-Am9H@?s!T2*7{+*Vj?pCpV5&WJO z*GbW%PLj|(hbd!fQK5Y-kgDHV!-I$y6G>Y|&uo9+79v}}$s=l$>#F-_F{TjUn~-!M zBN>n)@(LkzI0Sg?f1s}uBZi`wRB}ywU7wqq-PwaS%3nitaXb{&Q=x!xvOPfiQmmkd zWpe2@y7?wbI;hF|hlqf@x+3@a4$wLdJ1PZBoRc9oRGgdM+vm*;5XBZcMZ+@4_{aPUS|`NsD4YP2JUM zZEvA&!QLB$K*%gHy~y-RVs-C zkN^usP)S1pZXjj)nugy#?&vpiE^DS|QlhiBOc?nC$9CK}Ze)ihI{p-m$pgYV^5L~B zQTU>)x*fvKCNK*9j$@Gyt@@I2LF8c7YvDJDCf%1h0zVyNg7E~R$`6JE1EQk~-c1xG zE@xT)TesWHs}ny!5_7F_AyGL9K?Q~mP?>Vs!(oWZR42kf?*iTV*h5>tnzpljZL8IR zb7}l8q%Ckfh{^e3k^3pQMk=gLu60`Ja8HdkzVbeAU*exs*ajmRVp}O}l)TqX!?G7e z{4-~g?Gq%~)IJJ7p1k*WSnL3jqECe1OU}5nirS66_-$3FzMT5t3X zg{jgP^5?%zb(vMa!S|1cOYk4W!vG2KKd{YFIbPCk3_74HL`fWJASs{fxpzY@$(}Q- zK5I4TKS~`mfiDoDOm;XycF6mi|K|+d=lh=@U?9_V)BDDaZAnEw43`Ls1677I-+uFi zG?^$Fbc*pPun65{D!fH=3Oyp$WZAY!{JhzaUtIgYCWXf@)AkTa@x4xGjp0c zs7@JB012~&;z=SMbCp8d=Ga{l0(iwx<@o(f!OwmyH-gBN6wewq7A_h)oKg)koFPft zNfdie%F63S?rGDQR(N=bPuK>G0t^ax$0P8`N_cvR8rOf(O9T7$9#5!B;#!XUpLZXu z5C(OESAmE*2+hV}!bg$4K%`cQHBk!>##tW>1RbC%am`*|5IbvoLh!BqpAi2OmdXqf zHp%|!N;d!LN_26809n^14YVJJBe7aL87U~>HZ)VK%d|rZp(~zwNH#VGuX!vfal&Vv z-c)h33DOB@xl*~m5ZZ22sVRK>8I9+)QMVtsAB>r~SMkGMZaQ;Xi|?~Xxnmx;cYwYx z^nNxRxGcq7I!sO#b%$!0vQ(OqXm6T4mTilvMlYj|*i|=MK%kT2df;bZGW@NrgeX>( zf7eBsjJv}pNuEuHPEs42>}a`ut-O9lZDNh)_CsBpeHKvPKnpcWh^bC2QtnB5a4qy) zSrZhafuAkk5{yiM|zdiecKh zuc2R;6^;@i07fmepeofAJdX*knDzBA{3tyVYu6z#z;Lsi&x_bzzLEpfXtH*NrY_G`= z^X!;eI#hV*mmjjEOlo{TxQwSdUv0P$!Qvijpv9plBI@FUU#RJ)8Vn1ZGA$ATqF&s= zvcTS>Z8pepd>k=sjPY^3fpCB@aW8$Oq%fW;R?GpYoT@ki@N#2LxgTk1dYZHNrk@lx z7=yYr0FT$I>z~I0nXpPp$t3)}D?2^<@KWH#E{irFy2`)5r{AyvWHYzn`5@h;GVj0@ zJ@1fbD9gX=vQNR7PG5i}jFE}9#!;ote)FHdW?VVe6v4dWEz(R?!HC4KeVde*DGr=F zRotamm=!I~=_{|m;mCI4#5{C3_gBXan1<>!K!8O|)&K?O_L`}=uKCJ-s&+!XTk?wi z%Bwa_&k>4}`a` zFCG!c^Cdj#Bc2z2PXBCW$G)<%9X6;oZiigwvMLXQ$0f+2bKDCKCGR*cG>+;UTQ2bj z(2r#Od&Ulv*{?U~hq`j8W&8aggxHo<6*$&cDG#k;GS?mLx0^7mda35tz zHTnFA6vB^rczV1Ai8I&XyJX?jiEcQ}n;PYCl~EUPIxF@V%#c7LW`44<>ezAiG>1ff zeOSeCd#PW2z5z+<4Y?Qc#tb&+uH++5^G@!BaaDeVN8x=3ZB{R=Z5e+zf&13+nz{l% z{{#>B^OaIK}1Xh z;}?)W)sfwuf~?Ov1!oiQ-@WVG>D#(JL4Ob-h*l`y&hBY*!EkULKFdt9+VGJ?E=r85 zl*~dE)e4&l8Fdq`I@T2BAme(u7_)}y$TNu^lWWK-M8UQ(ZuBcA(qHG3; z&7bO_w9Cp!REZ3VB`&kfYOCmrNQxu7pbLoFkf)9Jkas&36ZnTBL?~cDug+T3bw?o! z$U-GUnOTkujjaB8vxcenWsZ4UrH*vMmACDj!95aG?gE5-g<6v8X9%kXThF|rP(0eu za*9aK6%^Qu4oyr(1t4hqmPX~~L7tB(;C{DH&MWDzUG+6I(;TGeM)jR#hK~O13LRwk zRc2;#m|qsRADyxC<6XC8u+lvVXoH+-HNTQXImy0_oM&D=ngI3OP?c>&k8&P2iV%hg zq{#n%P=0$dYJ2o$clJWqpVH&Q;S5Hv`T0-)mU2aa$XL#RH`0~|_g zmmfHkP7#d=iuiU1lL&5T+egS~-01WrWiiA=({_yWBnY@x5eX}`?y?3Xdic;`1dn5T zxTwLw{;Qt1MSWowZ}r+U?8Q+R46Avz>o>^}4zhvZaa_*Jd(2A!dP8ah=_*lh!W#a~ zNUm{^sD#HbDq!m*EK}(GzVn4N2GeNpEp8Z<_tctC_id9X=Irqhb_{b^H;~}qwZI&F z3t^MPXp4BuDv9@1Kr3*u zZ|&i`IKW!_Rv5(CaTJBndmX9B{YL8HJ2}u)`_>#J_-m{T-xpj%|2|{xmnVF#+X3=* zY*5{hDkk6M{+!Ved>d}mD@q^#{3qo9ZYb-+75cj*gH%I+d=}E+qSCK>vj4p z81UxB7>Gz}5QU^Pv-AJ*EHMW3g`EwB^^}ps>1E2$#r*H_{O{u)J@@1m$?Pu=va`3n z?so1N_WbU8U+4Nb|AN$Gv|%%33+!xpvv3iSLv&=qIUrD|3^*|rn7cNTWHgpaH0mTS zbXS-J>ZVOG~>BOwxVSa1sk6ivguYJD`$YgKkB!awl#vZ1NenaIidf zIo;H>3%L>R^l(kGI`c9&1a9H-s~68yw>3t6~N-Bv<9hyv4@0XlT|13}n_wh4#^(`bgWSiUFD z?SO{pz~eEqAvU|UZ-MPN$ZoAzAm@B5l}5B&MB(X&#FQ{BiwixOTe9@pn>F;%(9zOZ zly7ELHP0wS+Ikfr4P>I383O6E%8Ps6HYh5VLs3+bL1$J`TkTm6$wnI&{gh;r(^g9_ zB1RO-zhYoFDSl^oIQ*3Sm`H4%TTjHtuLbN&=j+P%iuVlxfEi zjsZUV9XdHY8m9muB8q5Vz z(`L%J6y+JTwbc>-nW(k@1!b!V8X7{S8M4^jErN(9CY}WtZ%l(hygPSA0+WuRy2zYP z{I1rh;dEB2eq9TUxCz{Gyr5B`eQAc=V{W%c+@W5W-mHRf!`2j21`y@SR^7Oz6_2Pt zkOomwUO=FaWS0^zE_8fOUJ%bwuxpLG@_{*8@bC&b7t2Op`l< z@kNX+GMUc*Zm2{Mv|>~c3<+pti9iF4V#K8sFm1soxJDi@ z0hJgP6;T1hrbc}rAns8Ko;#S9v5&XknRCva_O>&b{J*(Da_#Ad?20`5$%Xl&Puge2 zx?l9eH%e}NIwyYKT%Sue)L;7I7JYB)tpVNP7pm4j0n6@>Y|3y<8rov)IM#WzE@P_p zpPF3p<9y7UBK}GHof5CwW07klGghQ%{IeT#5013G-@n^&IFHZTJJ6g~ zCL1d0jcUJO-+8y)#+Wl0=`qCJo^!~ia8$-;rOBE~#*_zRZ*s~5n>IEYEtin@n6TMCEC;3v*irJ77~dTlkH+Ea~ni&gW~z zEBWCpC22aJfc1md!}q~j@)~H{%|IZpVtGYMh}wWjmPAVGFG{e*)g0Ukf*24y3)BXV zL{F7d(CXNXPzVFQlu~e}UL~fsmSnqLDoUS5FIMR1VZnVc3TinGDcHznFA6zTs<73? z4WUqG_@f*^v&jR_Q>a63^$bI30RuiF&nnl+1=px4kSzi_XB+AxOARqt@H;ZXlCce# zxlDYVFRiA{;DaYx(}XclB2S^eT1Q#1;p=9y6{`}J_sm<1Th)5PG zzzBlA<6+TFhl2c=Jl_@yJ}518aXJd2YFCAVu-7TMwT$KZefT7 zs5NxjtWvoM1u)bqHBp$PBs0RBf))u;m?bp>hDT6vTw&Lr!dBTtgj5XtcKJWphk_H; zeH09+T|vQZQ8Efz6lS0!cG`T`QE*MzYzhh@C0zhrg|>NSMAtY9%Huc+TF>Ppkl@@zX1imQDFMlS23i7E;Qs+kyyrF{7O&UZxN+ z-QgiSOj1$l30gw2$s1etFkp1{tI8Eq=&i{Q(-jkZqNBkxHjo*)Mn|Eg=J}ZZ*M!@$ m8X&e#V;O~v<{(@8u;?|riGH1;*CyBcIM_}B>Hc%VBjPV`^lBFX diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67..524c237 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.1-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a4..f5feea6 100644 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59..9d21a21 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail diff --git a/src/main/java/com/choreo/lib/Choreo.java b/src/main/java/com/choreo/lib/Choreo.java index fe6d10f..d4bf0f0 100644 --- a/src/main/java/com/choreo/lib/Choreo.java +++ b/src/main/java/com/choreo/lib/Choreo.java @@ -4,10 +4,8 @@ package com.choreo.lib; import com.google.gson.Gson; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; @@ -114,40 +112,61 @@ private static ChoreoTrajectory loadFile(File path) { public static Command choreoSwerveCommand( ChoreoTrajectory trajectory, Supplier poseSupplier, - double targetTimeStart, - double targetTimeEnd, - Supplier targetAngle, - PIDController targetController, PIDController xController, PIDController yController, PIDController rotationController, Consumer outputChassisSpeeds, - Consumer outputTargetPose, BooleanSupplier mirrorTrajectory, Subsystem... requirements ) { - ChoreoControlFunction controller = choreoSwerveController(xController, yController, rotationController); + return choreoSwerveCommand( + trajectory, + poseSupplier, + choreoSwerveController(xController, yController, rotationController), + outputChassisSpeeds, + mirrorTrajectory, + requirements + ); + } + + /** + * Create a command to follow a Choreo path. + * + * @param trajectory The trajectory to follow. Use Choreo.getTrajectory(String trajName) to load + * this from the deploy directory. + * @param poseSupplier A function that returns the current field-relative pose of the robot. + * @param controller A ChoreoControlFunction to follow the current trajectory state. Use + * ChoreoCommands.choreoSwerveController(PIDController xController, PIDController yController, + * PIDController rotationController) to create one using PID controllers for each degree of + * freedom. You can also pass in a function with the signature (Pose2d currentPose, + * ChoreoTrajectoryState referenceState) -> ChassisSpeeds to implement a custom follower + * (i.e. for logging). + * @param outputChassisSpeeds A function that consumes the target robot-relative chassis speeds + * and commands them to the robot. + * @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side, + * while keeping the same coordinate system origin. This will be called every loop during the + * command. + * @param requirements The subsystem(s) to require, typically your drive subsystem only. + * @return A command that follows a Choreo path. + */ + public static Command choreoSwerveCommand( + ChoreoTrajectory trajectory, + Supplier poseSupplier, + ChoreoControlFunction controller, + Consumer outputChassisSpeeds, + BooleanSupplier mirrorTrajectory, + Subsystem... requirements + ) { var timer = new Timer(); return new FunctionalCommand( timer::restart, () -> { - Pose2d pose = poseSupplier.get(); - ChoreoTrajectoryState sample = trajectory.sample(timer.get(), mirrorTrajectory.getAsBoolean()); - ChassisSpeeds speeds = controller.apply(pose, sample); - Pose2d targetOutput = sample.getPose(); - if (targetTimeStart >= 0 && timer.get() >= targetTimeStart && (targetTimeEnd >= 0 ? timer.get() <= targetTimeEnd : true)) { - double angle = targetAngle.get(); - speeds = - new ChassisSpeeds( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - targetController.calculate(MathUtil.angleModulus(pose.getRotation().getRadians()), angle) - ); - targetOutput = new Pose2d(targetOutput.getX(), targetOutput.getY(), new Rotation2d(angle)); - } - - outputChassisSpeeds.accept(speeds); - outputTargetPose.accept(targetOutput); + outputChassisSpeeds.accept( + controller.apply( + poseSupplier.get(), + trajectory.sample(timer.get(), mirrorTrajectory.getAsBoolean()) + ) + ); }, interrupted -> { timer.stop(); @@ -188,7 +207,10 @@ public static ChoreoControlFunction choreoSwerveController( double xFeedback = xController.calculate(pose.getX(), referenceState.x); double yFeedback = yController.calculate(pose.getY(), referenceState.y); - double rotationFeedback = rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading); + double rotationFeedback = rotationController.calculate( + pose.getRotation().getRadians(), + referenceState.heading + ); return ChassisSpeeds.fromFieldRelativeSpeeds( xFF + xFeedback, diff --git a/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java b/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java index b413984..d3b70c8 100644 --- a/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java +++ b/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java @@ -12,7 +12,7 @@ /** A single robot state in a ChoreoTrajectory. */ public class ChoreoTrajectoryState implements Interpolatable { - private static final double FIELD_WIDTH_METERS = 8.0136; + private static final double FIELD_WIDTH_METERS = 16.55445; /** The timestamp of this state, relative to the beginning of the trajectory. */ public final double timestamp; @@ -124,11 +124,11 @@ public double[] asArray() { public ChoreoTrajectoryState flipped() { return new ChoreoTrajectoryState( this.timestamp, - this.x, - FIELD_WIDTH_METERS - this.y, - -this.heading, - this.velocityX, - -this.velocityY, + FIELD_WIDTH_METERS - this.x, + this.y, + Math.PI - this.heading, + -this.velocityX, + this.velocityY, -this.angularVelocity ); } diff --git a/src/main/java/org/team340/lib/GRRDashboard.java b/src/main/java/org/team340/lib/GRRDashboard.java deleted file mode 100644 index 882c1f2..0000000 --- a/src/main/java/org/team340/lib/GRRDashboard.java +++ /dev/null @@ -1,413 +0,0 @@ -package org.team340.lib; - -import com.choreo.lib.ChoreoTrajectory; -import com.choreo.lib.ChoreoTrajectoryState; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Watchdog; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.LinkedHashMap; -import java.util.List; -import java.util.Map; -import java.util.UUID; -import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Supplier; -import org.team340.lib.controller.Controller2; -import org.team340.lib.util.Alliance; - -/** - * Utility class for interfacing with the dashboard. - */ -public final class GRRDashboard { - - private static final ReentrantLock localTableMutex = new ReentrantLock(); - private static final Watchdog watchdog = new Watchdog(Double.MAX_VALUE, GRRDashboard::reportOverrun); - private static final List> powerUsageGetters = new ArrayList<>(); - private static final List powerUsageUpdaters = new ArrayList<>(); - private static double lastPowerUsage = 0.0; - private static double lastWatchdog = 0.0; - - private GRRDashboard() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - /** - * The network table used by GRRDashboard. - */ - private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("GRRDashboard"); - /** - * A local table assigning values to keys in network tables. - */ - private static final Map localTable = new HashMap<>(); - /** - * The auto chooser. - */ - private static final AutoChooserSendable autoChooser = new AutoChooserSendable(); - /** - * The notifier that runs telemetry updates on a separate thread. - */ - private static Notifier notifier; - /** - * If the dashboard has been initialized. - */ - private static boolean initialized = false; - - /** - * Initializes the dashboard, with telemetry polling and updating performed on the main thread. - * Power usage updates are also performed on the main thread. - * @param robot The robot base. - * @param telemetryPeriod The period in seconds to update telemetry at. - * @param powerUsagePeriod The period in seconds to update power usage at. - */ - public static void initSync(TimedRobot robot, double telemetryPeriod, double powerUsagePeriod) { - init(robot, telemetryPeriod, powerUsagePeriod); - robot.addPeriodic(GRRDashboard::updateValues, telemetryPeriod); - } - - /** - * Initializes the dashboard, with telemetry polling and updating performed on a separate thread. - * Power usage updates are still performed on the main thread. - * @param robot The robot base. - * @param telemetryPeriod The period in seconds to update telemetry at. - * @param powerUsagePeriod The period in seconds to update power usage at. - */ - public static void initAsync(TimedRobot robot, double telemetryPeriod, double powerUsagePeriod) { - init(robot, telemetryPeriod, powerUsagePeriod); - notifier = new Notifier(GRRDashboard::updateValues); - notifier.setName("GRRDashboard"); - notifier.startPeriodic(telemetryPeriod); - } - - /** - * Initializes all parts of the dashboard except for updating telemetry. - */ - private static synchronized void init(TimedRobot robot, double telemetryPeriod, double powerUsagePeriod) { - if (initialized) throw new IllegalStateException("GRRDashboard is already initialized"); - initialized = true; - - publish("Robot", new RobotSendable()); - publish("Autos", autoChooser); - publish("SystemsCheck", Commands.none().withName("SystemsCheck")); - - watchdog.setTimeout(telemetryPeriod); - robot.addPeriodic(GRRDashboard::updatePowerUsage, powerUsagePeriod); - } - - /** - * Publishes a sendable to network tables. - * @param key The key to publish under. - * @param sendable The sendable. - */ - public static void publish(String key, Sendable sendable) { - if (sendable == null) return; - try { - localTableMutex.lock(); - Sendable existing = localTable.get(key); - if (existing == null || !existing.equals(sendable)) { - localTable.put(key, sendable); - NetworkTable table = nt.getSubTable(key); - SendableBuilderImpl builder = new SendableBuilderImpl(); - builder.setTable(table); - SendableRegistry.publish(sendable, builder); - builder.startListeners(); - } - } finally { - localTableMutex.unlock(); - } - } - - /** - * Gets the currently selected auto command. - */ - public static Command getAutoCommand() { - Command selected = autoChooser.getSelected(); - return selected != null ? selected : Commands.none().withName("Do Nothing"); - } - - /** - * Sets the systems check command. - * @param command The systems check command. - */ - public static void setSystemsCheck(Command command) { - publish("SystemsCheck", command.withName("SystemsCheck")); - } - - /** - * Adds a command to the dashboard. - * @param label The label for the command. - * @param command The command to add to the dashboard. - */ - public static void addCommand(String label, Command command) { - publish("Commands/" + label, command.withName(label)); - } - - /** - * Adds an auto command to the dashboard. - * @param label The label for the command. - * @param command The command to add to the dashboard. - */ - public static void addAutoCommand(String label, Command command) { - addAutoCommand(label, new ChoreoTrajectory(), command); - } - - /** - * Adds an auto command to the dashboard. - * @param label The label for the command. - * @param trajectory The trajectory used by the auto. - * @param command The command to add to the dashboard. - */ - public static void addAutoCommand(String label, ChoreoTrajectory trajectory, Command command) { - addAutoCommand(label, new ChoreoTrajectory[] { trajectory }, command); - } - - /** - * Adds an auto command to the dashboard. - * @param label The label for the command. - * @param trajectories Trajectories used by the auto. - * @param command The command to add to the dashboard. - */ - public static void addAutoCommand(String label, List trajectories, Command command) { - autoChooser.addOption(label, trajectories.stream().toArray(ChoreoTrajectory[]::new), command); - } - - /** - * Adds an auto command to the dashboard. - * @param label The label for the command. - * @param trajectories Trajectories used by the auto. - * @param command The command to add to the dashboard. - */ - public static void addAutoCommand(String label, ChoreoTrajectory[] trajectories, Command command) { - autoChooser.addOption(label, trajectories, command); - } - - /** - * Adds a controller. - * @param label The label for the controller. - * @param controller The controller. - */ - public static void addController(Controller2 controller) { - publish("Controllers/" + controller.getLabel(), controller); - } - - /** - * Adds a subsystem to the dashboard. - * @param label The label for the subsystem. - */ - public static void addSubsystem(GRRSubsystem subsystem) { - addSubsystemSendable("Details", subsystem, subsystem); - } - - /** - * Adds a sendable property to a subsystem's directory in NT. - * @param key The key to assign the sendable to. - * @param subsystem The subsystem the sendable is associated with. - * @param sendable The Sendable. - */ - public static void addSubsystemSendable(String key, GRRSubsystem subsystem, Sendable sendable) { - publish("Subsystems/" + subsystem.getName() + "/" + key, sendable); - } - - /** - * Adds hardware to the dashboard. - * Done automatically when using hardware factories in {@link GRRSubsystem}. - * @param subsystem The subsystem the hardware is associated with. - * @param hardware A sendable to represent the hardware. - */ - static void addHardware(GRRSubsystem subsystem, HardwareSendables.Hardware hardware) { - addSubsystemSendable("Hardware/" + hardware.getKey(), subsystem, hardware); - } - - /** - * Adds hardware to the dashboard. - * Done automatically when using hardware factories in {@link GRRSubsystem}. - * @param subsystem The subsystem the hardware is associated with. - * @param hardware A sendable to represent the hardware. - */ - static void addHardware(GRRSubsystem subsystem, HardwareSendables.PoweredHardware hardware) { - addSubsystemSendable("Hardware/" + hardware.getKey(), subsystem, hardware); - powerUsageGetters.add(hardware::getPowerUsage); - powerUsageUpdaters.add(hardware::updatePowerUsage); - } - - /** - * Updates sendable data. - */ - private static void updateValues() { - watchdog.reset(); - - try { - localTableMutex.lock(); - for (Map.Entry entry : localTable.entrySet()) { - SendableRegistry.update(entry.getValue()); - watchdog.addEpoch(entry.getKey()); - } - } finally { - localTableMutex.unlock(); - } - - watchdog.disable(); - lastWatchdog = watchdog.getTime(); - if (watchdog.isExpired()) watchdog.printEpochs(); - } - - private static void updatePowerUsage() { - for (Runnable updater : powerUsageUpdaters) updater.run(); - - double usageTotal = 0.0; - for (Supplier getter : powerUsageGetters) usageTotal += getter.get(); - lastPowerUsage = usageTotal; - } - - /** - * Report an overrun. - */ - private static void reportOverrun() { - DriverStation.reportWarning( - "Dashboard update frequency of " + (watchdog != null ? watchdog.getTimeout() : 0) + "s overrun\n", - false - ); - } - - /** - * A sendable for the auto chooser. - */ - private static class AutoChooserSendable implements Sendable { - - private final ReentrantLock selectedMutex = new ReentrantLock(); - - private String defaultChoice; - private String selected; - private Command choice = Commands.none(); - private Map> options = new LinkedHashMap<>(); // { [id]: [json, command] } - - /** - * Creates the auto chooser. Automatically adds a default option that does nothing. - */ - public AutoChooserSendable() { - defaultChoice = addOption("Do Nothing", new ChoreoTrajectory[] { new ChoreoTrajectory() }, Commands.none()); - } - - /** - * Gets the command of the selected auto. - */ - public Command getSelected() { - return choice; - } - - /** - * Adds an option. - * @param label The option's label. - * @param trajectories Trajectories used by the auto. - * @param command The option's command. - * @return The option's ID. - */ - public String addOption(String label, ChoreoTrajectory[] trajectories, Command command) { - String id = UUID.randomUUID().toString(); - String json = ""; - - List points = new ArrayList<>(); - double lastTimestamp = 0.0; - - for (int i = 0; i < trajectories.length; i++) { - ChoreoTrajectoryState[] states = trajectories[i].getStates(); - if (i > 0 && trajectories[i - 1].getStates().length > 0) lastTimestamp += trajectories[i - 1].getFinalState().timestamp; - for (ChoreoTrajectoryState state : states) { - points.add(new double[] { state.x, state.y, state.heading, state.timestamp + lastTimestamp }); - } - } - - ChoreoTrajectory lastTrajectory = trajectories.length > 0 ? trajectories[trajectories.length - 1] : new ChoreoTrajectory(); - double time = lastTimestamp + (lastTrajectory.getStates().length > 0 ? lastTrajectory.getFinalState().timestamp : 0.0); - - try { - json = - new ObjectMapper() - .writeValueAsString( - new HashMap<>() { - { - put("id", id); - put("label", label); - put("points", points); - put("time", time); - } - } - ); - } catch (Exception e) { - e.printStackTrace(); - json = ""; - } - - if (json.isEmpty()) json = "{ \"id\": \"" + id + "\", \"label\": \"" + label + "\", \"points\": [] }"; - options.put(id, Map.entry(json, command)); - return id; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.addStringProperty("default", () -> defaultChoice, null); - builder.addStringArrayProperty( - "options", - () -> options.values().stream().map(entry -> entry.getKey()).toArray(String[]::new), - null - ); - builder.addStringProperty( - "active", - () -> { - selectedMutex.lock(); - try { - if (selected != null) return selected; else return defaultChoice; - } finally { - selectedMutex.unlock(); - } - }, - null - ); - builder.addStringProperty( - "selected", - null, - (String value) -> { - selectedMutex.lock(); - try { - Map.Entry entry = options.get(value); - if (entry != null) { - selected = value; - choice = entry.getValue(); - } - } finally { - selectedMutex.unlock(); - } - } - ); - } - } - - /** - * A sendable that represents the robot. - */ - private static class RobotSendable implements Sendable { - - @Override - public void initSendable(SendableBuilder builder) { - builder.addBooleanProperty("blueAlliance", Alliance::isBlue, null); - builder.addDoubleProperty("cpuTemperature", RobotController::getCPUTemp, null); - builder.addBooleanProperty("enabled", DriverStation::isEnabled, null); - builder.addDoubleProperty("matchTime", DriverStation::getMatchTime, null); - builder.addDoubleProperty("ntUpdateTime", () -> lastWatchdog, null); - builder.addDoubleProperty("powerUsage", () -> lastPowerUsage, null); - builder.addIntegerProperty("timestamp", RobotController::getFPGATime, null); - builder.addDoubleProperty("voltage", RobotController::getBatteryVoltage, null); - } - } -} diff --git a/src/main/java/org/team340/lib/GRRSubsystem.java b/src/main/java/org/team340/lib/GRRSubsystem.java deleted file mode 100644 index c54efe5..0000000 --- a/src/main/java/org/team340/lib/GRRSubsystem.java +++ /dev/null @@ -1,492 +0,0 @@ -package org.team340.lib; - -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; -import edu.wpi.first.wpilibj.CounterBase.EncodingType; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DigitalSource; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PneumaticHub; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.team340.lib.commands.CommandBuilder; - -/** - * An extension to WPILib's subsystem. - * Adds factories for creating hardware and automatically adds them to the dashboard. - */ -public abstract class GRRSubsystem extends SubsystemBase { - - protected final String label; - - /** - * Create the subsystem. - * @param label The label to give the subsystem. Shown in the dashboard. - */ - public GRRSubsystem(String label) { - super(); - this.label = label; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.publishConstString(".label", label); - builder.publishConstString(".api", "GRRSubsystem"); - builder.addStringProperty("command", () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "{None}", null); - builder.addStringProperty("default", () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "{None}", null); - builder.addBooleanProperty("hasCommand", () -> getCurrentCommand() != null, null); - builder.addBooleanProperty("hasDefault", () -> getDefaultCommand() != null, null); - } - - /** - * Adds the subsystem to the dashboard. - */ - public void addToDashboard() { - GRRDashboard.addSubsystem(this); - } - - /** - * Creates a command builder that requires this subsystem. - */ - protected CommandBuilder commandBuilder() { - return new CommandBuilder(this); - } - - /** - * Creates a command builder that requires this subsystem. - * @param name The name of the command. - */ - protected CommandBuilder commandBuilder(String name) { - return new CommandBuilder(name, this); - } - - /** - * A command that reports a warning to the driver station. - * @param warning The warning to report. - * @param printTrace If {@code true}, appends a stack trace to warning string. - */ - protected Command reportWarning(String warning, boolean printTrace) { - return runOnce(() -> DriverStation.reportWarning(warning, printTrace)); - } - - /** - * A command that reports an error to the driver station. - * @param error The error to report. - * @param printTrace If {@code true}, appends a stack trace to error string. - */ - protected Command reportError(String error, boolean printTrace) { - return runOnce(() -> DriverStation.reportError(error, printTrace)); - } - - /** - * Creates a Spark Max. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Spark Max on the CAN bus. - * @param type The motor type connected to the controller. - */ - protected CANSparkMax createSparkMax(String label, int deviceId, MotorType type) { - CANSparkMax sparkMax = new CANSparkMax(deviceId, type); - new HardwareSendables.SparkMax(label, sparkMax).addToDashboard(this); - return sparkMax; - } - - /** - * Creates a Spark Flex. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Spark Flex on the CAN bus. - * @param type The motor type connected to the controller. - */ - protected CANSparkFlex createSparkFlex(String label, int deviceId, MotorType type) { - CANSparkFlex sparkFlex = new CANSparkFlex(deviceId, type); - new HardwareSendables.SparkFlex(label, sparkFlex).addToDashboard(this); - return sparkFlex; - } - - /** - * Creates a Talon SRX. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Talon FX on the CAN bus. - */ - protected TalonSRX createTalonSRX(String label, int deviceId) { - TalonSRX talonSRX = new TalonSRX(deviceId); - new HardwareSendables.TalonSRX(label, talonSRX).addToDashboard(this); - return talonSRX; - } - - /** - * Creates a Talon FX. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Talon FX on the CAN bus. - */ - protected TalonFX createTalonFX(String label, int deviceId) { - TalonFX talonFX = new TalonFX(deviceId); - new HardwareSendables.TalonFX(label, talonFX).addToDashboard(this); - return talonFX; - } - - /** - * Creates a Talon FX. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Talon FX on the CAN bus. - * @param canBus Name of the CAN bus the Talon FX is on. - */ - protected TalonFX createTalonFX(String label, int deviceId, String canBus) { - TalonFX talonFX = new TalonFX(deviceId, canBus); - new HardwareSendables.TalonFX(label, talonFX).addToDashboard(this); - return talonFX; - } - - /** - * Creates a Spark Max attached Absolute Encoder. - * @param label The label to use. Shown in the dashboard. - * @param sparkMax The Spark Max the absolute encoder is attached to. - * @param type The encoder type. - */ - protected SparkAbsoluteEncoder createSparkMaxAbsoluteEncoder(String label, CANSparkMax sparkMax, SparkAbsoluteEncoder.Type type) { - SparkAbsoluteEncoder absoluteEncoder = sparkMax.getAbsoluteEncoder(type); - new HardwareSendables.SparkAbsoluteEncoder(label, sparkMax, absoluteEncoder).addToDashboard(this); - return absoluteEncoder; - } - - /** - * Creates a Spark Flex attached Absolute Encoder. - * @param label The label to use. Shown in the dashboard. - * @param sparkFlex The Spark Flex the absolute encoder is attached to. - * @param type The encoder type. - */ - protected SparkAbsoluteEncoder createSparkFlexAbsoluteEncoder(String label, CANSparkFlex sparkFlex, SparkAbsoluteEncoder.Type type) { - SparkAbsoluteEncoder absoluteEncoder = sparkFlex.getAbsoluteEncoder(type); - new HardwareSendables.SparkAbsoluteEncoder(label, sparkFlex, absoluteEncoder).addToDashboard(this); - return absoluteEncoder; - } - - /** - * Creates a CANcoder. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the CANcoder on the CAN bus. - */ - protected CANcoder createCANcoder(String label, int deviceId) { - CANcoder canCoder = new CANcoder(deviceId); - new HardwareSendables.CANcoder(label, canCoder).addToDashboard(this); - return canCoder; - } - - /** - * Creates a CANcoder. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the CANcoder on the CAN bus. - * @param canBus Name of the CAN bus the CANcoder is on. - */ - protected CANcoder createCANcoder(String label, int deviceId, String canBus) { - CANcoder canCoder = new CANcoder(deviceId, canBus); - new HardwareSendables.CANcoder(label, canCoder).addToDashboard(this); - return canCoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - */ - protected Encoder createDIOEncoder(String label, int channelA, int channelB) { - Encoder encoder = new Encoder(channelA, channelB); - new HardwareSendables.DIOEncoder(label, channelA, channelB, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - * @param reverseDirection The orientation of the encoder and inverts the output values if necessary so forward represents positive values. - */ - protected Encoder createDIOEncoder(String label, int channelA, int channelB, boolean reverseDirection) { - Encoder encoder = new Encoder(channelA, channelB, reverseDirection); - new HardwareSendables.DIOEncoder(label, channelA, channelB, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - * @param reverseDirection The orientation of the encoder and inverts the output values if necessary so forward represents positive values. - * @param encodingType Either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are selected, then a counter object will be used and the returned value will either exactly match the spec'd count or be double (2x) the spec'd count. - */ - protected Encoder createDIOEncoder(String label, int channelA, int channelB, boolean reverseDirection, EncodingType encodingType) { - Encoder encoder = new Encoder(channelA, channelB, reverseDirection, encodingType); - new HardwareSendables.DIOEncoder(label, channelA, channelB, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - * @param indexChannel The encoder's DIO index channel. - */ - protected Encoder createDIOEncoder(String label, int channelA, int channelB, int indexChannel) { - Encoder encoder = new Encoder(channelA, channelB, indexChannel); - new HardwareSendables.DIOEncoder(label, channelA, channelB, indexChannel, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - * @param indexChannel The encoder's DIO index channel. - * @param reverseDirection The orientation of the encoder and inverts the output values if necessary so forward represents positive values. - */ - protected Encoder createDIOEncoder(String label, int channelA, int channelB, int indexChannel, boolean reverseDirection) { - Encoder encoder = new Encoder(channelA, channelB, indexChannel, reverseDirection); - new HardwareSendables.DIOEncoder(label, channelA, channelB, indexChannel, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - */ - protected Encoder createDIOEncoder(String label, DigitalSource sourceA, DigitalSource sourceB) { - Encoder encoder = new Encoder(sourceA, sourceB); - new HardwareSendables.DIOEncoder(label, sourceA, sourceB, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - * @param reverseDirection The orientation of the encoder and inverts the output values if necessary so forward represents positive values. - */ - protected Encoder createDIOEncoder(String label, DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) { - Encoder encoder = new Encoder(sourceA, sourceB, reverseDirection); - new HardwareSendables.DIOEncoder(label, sourceA, sourceB, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - * @param reverseDirection The orientation of the encoder and inverts the output values if necessary so forward represents positive values. - * @param encodingType Either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are selected, then a counter object will be used and the returned value will either exactly match the spec'd count or be double (2x) the spec'd count. - */ - protected Encoder createDIOEncoder( - String label, - DigitalSource sourceA, - DigitalSource sourceB, - boolean reverseDirection, - EncodingType encodingType - ) { - Encoder encoder = new Encoder(sourceA, sourceB, reverseDirection, encodingType); - new HardwareSendables.DIOEncoder(label, sourceA, sourceB, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - * @param indexSource The encoder's index channel source. - */ - protected Encoder createDIOEncoder(String label, DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) { - Encoder encoder = new Encoder(sourceA, sourceB, indexSource); - new HardwareSendables.DIOEncoder(label, sourceA, sourceB, indexSource, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates a DIO encoder. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - * @param indexSource The encoder's index channel source. - * @param reverseDirection The orientation of the encoder and inverts the output values if necessary so forward represents positive values. - */ - protected Encoder createDIOEncoder( - String label, - DigitalSource sourceA, - DigitalSource sourceB, - DigitalSource indexSource, - boolean reverseDirection - ) { - Encoder encoder = new Encoder(sourceA, sourceB, indexSource, reverseDirection); - new HardwareSendables.DIOEncoder(label, sourceA, sourceB, indexSource, encoder).addToDashboard(this); - return encoder; - } - - /** - * Creates an ADIS16470. - * @param label The label to use. Shown in the dashboard. - * @param yawAxis The axis that measures the yaw. - * @param pitchAxis The axis that measures the pitch. - * @param rollAxis The axis that measures the roll. - */ - protected ADIS16470_IMU createADIS16470(String label, IMUAxis yawAxis, IMUAxis pitchAxis, IMUAxis rollAxis) { - ADIS16470_IMU adis16470 = new ADIS16470_IMU(yawAxis, pitchAxis, rollAxis); - new HardwareSendables.ADIS16470(label, adis16470).addToDashboard(this); - return adis16470; - } - - /** - * Creates an ADIS16470. - * @param label The label to use. Shown in the dashboard. - * @param yawAxis The axis that measures the yaw. - * @param pitchAxis The axis that measures the pitch. - * @param rollAxis The axis that measures the roll. - * @param port The SPI Port the gyro is plugged into. - * @param calibrationTime Calibration time. - */ - protected ADIS16470_IMU createADIS16470( - String label, - IMUAxis yawAxis, - IMUAxis pitchAxis, - IMUAxis rollAxis, - SPI.Port port, - CalibrationTime calibrationTime - ) { - ADIS16470_IMU adis16470 = new ADIS16470_IMU(yawAxis, pitchAxis, rollAxis, port, calibrationTime); - new HardwareSendables.ADIS16470(label, adis16470).addToDashboard(this); - return adis16470; - } - - /** - * Creates a Pigeon 2. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Pigeon 2 on the CAN bus. - */ - protected Pigeon2 createPigeon2(String label, int deviceId) { - Pigeon2 pigeon2 = new Pigeon2(deviceId); - new HardwareSendables.Pigeon2(label, pigeon2).addToDashboard(this); - return pigeon2; - } - - /** - * Creates a Pigeon 2. - * @param label The label to use. Shown in the dashboard. - * @param deviceId The ID of the Pigeon 2 on the CAN bus. - * @param canBus Name of the CAN bus the Pigeon 2 is on. - */ - protected Pigeon2 createPigeon2(String label, int deviceId, String canBus) { - Pigeon2 pigeon2 = new Pigeon2(deviceId, canBus); - new HardwareSendables.Pigeon2(label, pigeon2).addToDashboard(this); - return pigeon2; - } - - /** - * Creates a Digital Input. - * @param label The label to use. Shown in the dashboard. - * @param channel The DIO channel for the digital input. - */ - protected DigitalInput createDigitalInput(String label, int channel) { - DigitalInput digitalInput = new DigitalInput(channel); - new HardwareSendables.DigitalInput(label, digitalInput).addToDashboard(this); - return digitalInput; - } - - /** - * Creates a Pneumatic Hub. - * @param label The label to use. Shown in the dashboard. - */ - protected PneumaticHub createPneumaticHub(String label) { - PneumaticHub pneumaticHub = new PneumaticHub(); - new HardwareSendables.PneumaticHub(label, pneumaticHub).addToDashboard(this); - return pneumaticHub; - } - - /** - * Creates a Pneumatic Hub. - * @param label The label to use. Shown in the dashboard. - * @param module The module number to construct. - */ - protected PneumaticHub createPneumaticHub(String label, int module) { - PneumaticHub pneumaticHub = new PneumaticHub(module); - new HardwareSendables.PneumaticHub(label, pneumaticHub).addToDashboard(this); - return pneumaticHub; - } - - /** - * Creates a Solenoid. - * @param label The label to use. Shown in the dashboard. - * @param moduleType The module type to use. - * @param channel The channel the solenoid is on. - */ - protected Solenoid createSolenoid(String label, PneumaticsModuleType moduleType, int channel) { - Solenoid solenoid = new Solenoid(moduleType, channel); - new HardwareSendables.Solenoid(label, solenoid).addToDashboard(this); - return solenoid; - } - - /** - * Creates a Solenoid. - * @param label The label to use. Shown in the dashboard. - * @param module The module ID to use. - * @param moduleType The module type to use. - * @param channel The channel the solenoid is on. - */ - protected Solenoid createSolenoid(String label, int module, PneumaticsModuleType moduleType, int channel) { - Solenoid solenoid = new Solenoid(module, moduleType, channel); - new HardwareSendables.Solenoid(label, solenoid).addToDashboard(this); - return solenoid; - } - - /** - * Creates a Double Solenoid. - * @param label The label to use. Shown in the dashboard. - * @param moduleType The module type to use. - * @param forwardChannel The forward channel on the module to control. - * @param reverseChannel The reverse channel on the module to control. - */ - protected DoubleSolenoid createDoubleSolenoid(String label, PneumaticsModuleType moduleType, int forwardChannel, int reverseChannel) { - DoubleSolenoid doubleSolenoid = new DoubleSolenoid(moduleType, forwardChannel, reverseChannel); - new HardwareSendables.DoubleSolenoid(label, doubleSolenoid).addToDashboard(this); - return doubleSolenoid; - } - - /** - * Creates a Double Solenoid. - * @param label The label to use. Shown in the dashboard. - * @param module The module ID to use. - * @param moduleType The module type to use. - * @param forwardChannel The forward channel on the module to control. - * @param reverseChannel The reverse channel on the module to control. - */ - protected DoubleSolenoid createDoubleSolenoid( - String label, - int module, - PneumaticsModuleType moduleType, - int forwardChannel, - int reverseChannel - ) { - DoubleSolenoid doubleSolenoid = new DoubleSolenoid(module, moduleType, forwardChannel, reverseChannel); - new HardwareSendables.DoubleSolenoid(label, doubleSolenoid).addToDashboard(this); - return doubleSolenoid; - } -} diff --git a/src/main/java/org/team340/lib/HardwareSendables.java b/src/main/java/org/team340/lib/HardwareSendables.java deleted file mode 100644 index 34a086a..0000000 --- a/src/main/java/org/team340/lib/HardwareSendables.java +++ /dev/null @@ -1,719 +0,0 @@ -package org.team340.lib; - -import com.revrobotics.CANSparkBase.FaultID; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalSource; -import edu.wpi.first.wpilibj.Timer; -import java.util.ArrayList; -import java.util.List; -import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Supplier; - -// TODO Faults - -/** - * {@link Sendable} wrappers for hardware. - * Also provides methods for tracking faults and power usage. - */ -final class HardwareSendables { - - private HardwareSendables() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - /** - * Generic hardware. - */ - public abstract static class Hardware implements Sendable { - - private final String key; - private final String label; - private final String api; - - /** - * @param key The key to use in network tables. It is recommended to have this key be related to the bus and device ID the hardware is accessed through. For example, {@code "CAN-10"}. - * @param label The label to use. Shown in the dashboard. - * @param api The API used for interfacing with the hardware in code. - */ - public Hardware(String key, String label, Object api) { - this.key = key; - this.label = label; - this.api = api.getClass().getSimpleName(); - } - - /** - * Adds the hardware to the dashboard. - * @param subsystem The subsystem the hardware is associated with. - */ - public void addToDashboard(GRRSubsystem subsystem) { - GRRDashboard.addHardware(subsystem, this); - } - - /** - * Returns the hardware's key. - */ - public String getKey() { - return key; - } - - /** - * Gets hardware faults. - */ - public String[] getFaults() { - return new String[0]; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.publishConstString(".label", label); - builder.publishConstString(".api", api); - builder.addStringArrayProperty("faults", this::getFaults, null); - } - } - - /** - * Generic hardware that has trackable power consumption. - */ - public abstract static class PoweredHardware extends Hardware { - - private final Supplier power; - private final Supplier voltage; - private final Supplier current; - private final ReentrantLock usageMutex = new ReentrantLock(); - private final Timer usageTimer = new Timer(); - private double usage = 0.0; - private double lastPower = 0.0; - private boolean updatedPower = false; - - /** - * @param key The key to use in network tables. It is recommended to have this key be related to the bus and device ID the hardware is accessed through. For example, {@code "CAN-10"}. - * @param label The label to use. Shown in the dashboard. - * @param api The API used for interfacing with the hardware in code. - * @param voltage A supplier for the device's voltage. - * @param current A supplier for the device's current in amps. - */ - public PoweredHardware(String key, String label, Object api, Supplier voltage, Supplier current) { - this(key, label, api, () -> voltage.get() * current.get(), voltage, current); - } - - /** - * @param key The key to use in network tables. It is recommended to have this key be related to the bus and device ID the hardware is accessed through. For example, {@code "CAN-10"}. - * @param label The label to use. Shown in the dashboard. - * @param api The API used for interfacing with the hardware in code. - * @param power A supplier for the device's power in watts. - * @param voltage A supplier for the device's voltage. - * @param current A supplier for the device's current in amps. - */ - public PoweredHardware( - String key, - String label, - Object api, - Supplier power, - Supplier voltage, - Supplier current - ) { - super(key, label, api); - this.power = power; - this.voltage = voltage; - this.current = current; - } - - /** - * Adds the hardware to the dashboard. - * @param subsystem The subsystem the hardware is associated with. - */ - public void addToDashboard(GRRSubsystem subsystem) { - GRRDashboard.addHardware(subsystem, this); - } - - /** - * Updates the hardware's power usage counter. - * Call this periodically to ensure more accurate tracking. Smaller intervals results in better accuracy. - */ - public void updatePowerUsage() { - try { - usageMutex.lock(); - lastPower = power.get(); - usage += (usageTimer.get() / 3600.0) * lastPower; - usageTimer.restart(); - updatedPower = true; - } finally { - usageMutex.unlock(); - } - } - - /** - * Gets the hardware's power usage since startup in watt hours. - */ - public double getPowerUsage() { - return usage; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty( - "power", - () -> { - double p; - try { - usageMutex.lock(); - if (updatedPower) { - updatedPower = false; - p = lastPower; - } else { - p = power.get(); - } - } finally { - usageMutex.unlock(); - } - - return p; - }, - null - ); - builder.addDoubleProperty("powerUsage", () -> usage, null); - builder.addDoubleProperty("voltage", voltage::get, null); - builder.addDoubleProperty("current", current::get, null); - } - } - - /** - * A generic motor. - */ - public abstract static class Motor extends PoweredHardware { - - private final Supplier output; - private final Supplier temperature; - private final Supplier velocity; - private final Supplier position; - - /** - * @param key The key to use in network tables. It is recommended to have this key be related to the bus and device ID the hardware is accessed through. For example, {@code "CAN-10"}. - * @param label The label to use. Shown in the dashboard. - * @param api The API used for interfacing with the hardware in code. - * @param voltage A supplier for the device's voltage. - * @param current A supplier for the device's current. - * @param output A supplier for the motor's applied output. Should be a value from {@code -1.0} to {@code 1.0}. - * @param temperature A supplier for the temperature in celsius of the motor or the motor's controller (whichever is accessible, preferably the motor). - * @param velocity A supplier for the motor's velocity. - * @param position A supplier for the motor's position. - */ - public Motor( - String key, - String label, - Object api, - Supplier voltage, - Supplier current, - Supplier output, - Supplier temperature, - Supplier velocity, - Supplier position - ) { - super(key, label, api, voltage, current); - this.output = output; - this.temperature = temperature; - this.velocity = velocity; - this.position = position; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("output", output::get, null); - builder.addDoubleProperty("temperature", temperature::get, null); - builder.addDoubleProperty("velocity", velocity::get, null); - builder.addDoubleProperty("position", position::get, null); - } - } - - /** - * A generic encoder. - */ - public abstract static class Encoder extends Hardware { - - private final Supplier velocity; - private final Supplier position; - - /** - * @param key The key to use in network tables. It is recommended to have this key be related to the bus and device ID the hardware is accessed through. For example, {@code "CAN-10"}. - * @param label The label to use. Shown in the dashboard. - * @param api The API used for interfacing with the hardware in code. - * @param velocity A supplier for the encoder's velocity. - * @param position A supplier for the encoder's position. If the encoder is absolute, prefer the absolute position. - */ - public Encoder(String key, String label, Object api, Supplier velocity, Supplier position) { - super(key, label, api); - this.velocity = velocity; - this.position = position; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("velocity", velocity::get, null); - builder.addDoubleProperty("position", position::get, null); - } - } - - /** - * A generic IMU. - */ - public abstract static class IMU extends Hardware { - - private final Supplier yaw; - private final Supplier pitch; - private final Supplier roll; - - /** - * @param key The key to use in network tables. It is recommended to have this key be related to the bus and device ID the hardware is accessed through. For example, {@code "CAN-10"}. - * @param label The label to use. Shown in the dashboard. - * @param api The API used for interfacing with the hardware in code. - * @param yaw A supplier for the IMU's yaw in radians. - * @param pitch A supplier for the IMU's pitch in radians. - * @param roll A supplier for the IMU's roll in radians. - */ - public IMU(String key, String label, Object api, Supplier yaw, Supplier pitch, Supplier roll) { - super(key, label, api); - this.yaw = yaw; - this.pitch = pitch; - this.roll = roll; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("yaw", yaw::get, null); - builder.addDoubleProperty("pitch", pitch::get, null); - builder.addDoubleProperty("roll", roll::get, null); - } - } - - /** - * A Spark Max {@link Sendable}. - */ - public static final class SparkMax extends Motor { - - private final CANSparkMax sparkMax; - - /** - * Create the Spark Max sendable. - * @param label The label to use. Shown in the dashboard. - * @param sparkMax The Spark Max. - */ - public SparkMax(String label, CANSparkMax sparkMax) { - this(label, sparkMax, sparkMax.getEncoder()); - } - - private SparkMax(String label, CANSparkMax sparkMax, RelativeEncoder relativeEncoder) { - super( - "CAN-" + sparkMax.getDeviceId(), - label, - sparkMax, - () -> sparkMax.getBusVoltage(), - () -> sparkMax.getOutputCurrent(), - () -> sparkMax.getAppliedOutput(), - () -> sparkMax.getMotorTemperature(), - () -> relativeEncoder.getVelocity(), - () -> relativeEncoder.getPosition() - ); - this.sparkMax = sparkMax; - } - - @Override - public String[] getFaults() { - int faults = sparkMax.getStickyFaults() | sparkMax.getFaults(); - List faultStrings = new ArrayList<>(); - for (int i = 0; i < 16; i++) { - if (((faults >> i) & 1) == 1) { - faultStrings.add(FaultID.fromId(i).name()); - } - } - - return faultStrings.toArray(new String[faultStrings.size()]); - } - } - - /** - * A Spark Flex {@link Sendable}. - */ - public static final class SparkFlex extends Motor { - - private final CANSparkFlex sparkFlex; - - /** - * Create the Spark Flex sendable. - * @param label The label to use. Shown in the dashboard. - * @param sparkFlex The Spark Flex. - */ - public SparkFlex(String label, CANSparkFlex sparkFlex) { - this(label, sparkFlex, sparkFlex.getEncoder()); - } - - private SparkFlex(String label, CANSparkFlex sparkFlex, RelativeEncoder relativeEncoder) { - super( - "CAN-" + sparkFlex.getDeviceId(), - label, - sparkFlex, - () -> sparkFlex.getBusVoltage(), - () -> sparkFlex.getOutputCurrent(), - () -> sparkFlex.getAppliedOutput(), - () -> sparkFlex.getMotorTemperature(), - () -> relativeEncoder.getVelocity(), - () -> relativeEncoder.getPosition() - ); - this.sparkFlex = sparkFlex; - } - - @Override - public String[] getFaults() { - int faults = sparkFlex.getStickyFaults() | sparkFlex.getFaults(); - List faultStrings = new ArrayList<>(); - for (int i = 0; i < 16; i++) { - if (((faults >> i) & 1) == 1) { - faultStrings.add(FaultID.fromId(i).name()); - } - } - - return faultStrings.toArray(new String[faultStrings.size()]); - } - } - - /** - * A Talon SRX {@link Sendable}. - */ - public static final class TalonSRX extends Motor { - - /** - * Create the Talon SRX sendable. - * @param label The label to use. Shown in the dashboard. - * @param talonSRX The Talon SRX. - */ - public TalonSRX(String label, com.ctre.phoenix.motorcontrol.can.TalonSRX talonSRX) { - super( - "CAN-" + talonSRX.getDeviceID(), - label, - talonSRX, - () -> talonSRX.getBusVoltage(), - () -> talonSRX.getStatorCurrent(), - () -> talonSRX.getMotorOutputPercent() / 100.0, - () -> talonSRX.getTemperature(), - () -> talonSRX.getSelectedSensorVelocity(), - () -> talonSRX.getSelectedSensorPosition() - ); - } - } - - /** - * A Talon FX {@link Sendable}. - */ - public static final class TalonFX extends Motor { - - /** - * Create the Talon FX sendable. - * @param label The label to use. Shown in the dashboard. - * @param talonFX The Talon FX. - */ - public TalonFX(String label, com.ctre.phoenix6.hardware.TalonFX talonFX) { - super( - "CAN-" + talonFX.getDeviceID(), - label, - talonFX, - () -> talonFX.getSupplyVoltage().getValue(), - () -> talonFX.getSupplyCurrent().getValue(), - () -> talonFX.getDutyCycle().getValue(), - () -> talonFX.getDeviceTemp().getValue(), - () -> talonFX.getRotorVelocity().getValue(), - () -> talonFX.getRotorPosition().getValue() - ); - } - } - - /** - * A Spark Absolute Encoder {@link Sendable}. - */ - public static final class SparkAbsoluteEncoder extends Encoder { - - /** - * Create the Spark Max Absolute Encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param sparkMax The Spark Max the encoder is attached to. - * @param absoluteEncoder The absolute encoder. - */ - public SparkAbsoluteEncoder(String label, CANSparkMax sparkMax, com.revrobotics.SparkAbsoluteEncoder absoluteEncoder) { - this(label, sparkMax.getDeviceId(), absoluteEncoder); - } - - /** - * Create the Spark Flex Absolute Encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param sparkFlex The Spark Flex the encoder is attached to. - * @param absoluteEncoder The absolute encoder. - */ - public SparkAbsoluteEncoder(String label, CANSparkFlex sparkFlex, com.revrobotics.SparkAbsoluteEncoder absoluteEncoder) { - this(label, sparkFlex.getDeviceId(), absoluteEncoder); - } - - /** - * Create the Spark Absolute Encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param sparkDeviceId The device ID of the Spark the encoder is attached to. - * @param absoluteEncoder The absolute encoder. - */ - public SparkAbsoluteEncoder(String label, int sparkDeviceId, com.revrobotics.SparkAbsoluteEncoder absoluteEncoder) { - super( - "CAN-" + sparkDeviceId + "-AbsoluteEncoder", - label, - absoluteEncoder, - () -> absoluteEncoder.getVelocity(), - () -> absoluteEncoder.getPosition() - ); - } - } - - /** - * A CANcoder {@link Sendable}. - */ - public static final class CANcoder extends Encoder { - - /** - * Create the CANcoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param canCoder The CANcoder. - */ - public CANcoder(String label, com.ctre.phoenix6.hardware.CANcoder canCoder) { - super( - "CAN-" + canCoder.getDeviceID(), - label, - canCoder, - () -> canCoder.getVelocity().getValue(), - () -> canCoder.getAbsolutePosition().getValue() - ); - } - } - - /** - * A DIO attached Encoder {@link Sendable}. - */ - public static final class DIOEncoder extends Encoder { - - /** - * Create the DIO attached encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - * @param encoder The encoder. - */ - public DIOEncoder(String label, DigitalSource sourceA, DigitalSource sourceB, edu.wpi.first.wpilibj.Encoder encoder) { - this(label, sourceA.getChannel(), sourceB.getChannel(), encoder); - } - - /** - * Create the DIO attached encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param sourceA The encoder's A channel source. - * @param sourceB The encoder's B channel source. - * @param indexSource The encoder's index channel source. - * @param encoder The encoder. - */ - public DIOEncoder( - String label, - DigitalSource sourceA, - DigitalSource sourceB, - DigitalSource indexSource, - edu.wpi.first.wpilibj.Encoder encoder - ) { - this(label, sourceA.getChannel(), sourceB.getChannel(), indexSource.getChannel(), encoder); - } - - /** - * Create the DIO attached encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - * @param encoder The encoder. - */ - public DIOEncoder(String label, int channelA, int channelB, edu.wpi.first.wpilibj.Encoder encoder) { - super("DIO-" + channelA + "-" + channelB, label, encoder, () -> encoder.getRate(), () -> encoder.getDistance()); - } - - /** - * Create the DIO attached encoder sendable. - * @param label The label to use. Shown in the dashboard. - * @param channelA The encoder's DIO A channel. - * @param channelB The encoder's DIO B channel. - * @param indexChannel The encoder's DIO index channel. - * @param encoder The encoder. - */ - public DIOEncoder(String label, int channelA, int channelB, int indexChannel, edu.wpi.first.wpilibj.Encoder encoder) { - super( - "DIO-" + channelA + "-" + channelB + "-" + indexChannel, - label, - encoder, - () -> encoder.getRate(), - () -> encoder.getDistance() - ); - } - } - - /** - * A ADIS16470 {@link Sendable}. - */ - public static final class ADIS16470 extends IMU { - - /** - * Create the ADIS16470 sendable. - * @param label The label to use. Shown in the dashboard. - * @param adis16470 The ADIS16470. - */ - public ADIS16470(String label, edu.wpi.first.wpilibj.ADIS16470_IMU adis16470) { - super( - "SPI-" + adis16470.getPort(), - label, - adis16470, - () -> Math.toRadians(adis16470.getAngle(adis16470.getYawAxis())), - () -> Math.toRadians(adis16470.getAngle(adis16470.getPitchAxis())), - () -> Math.toRadians(adis16470.getAngle(adis16470.getRollAxis())) - ); - } - } - - /** - * A Pigeon 2 {@link Sendable}. - */ - public static final class Pigeon2 extends IMU { - - /** - * Create the Pigeon 2 sendable. - * @param label The label to use. Shown in the dashboard. - * @param pigeon2 The Pigeon 2. - */ - public Pigeon2(String label, com.ctre.phoenix6.hardware.Pigeon2 pigeon2) { - super( - "CAN-" + pigeon2.getDeviceID(), - label, - pigeon2, - () -> Math.toRadians(pigeon2.getYaw().getValue()), - () -> Math.toRadians(pigeon2.getPitch().getValue()), - () -> Math.toRadians(pigeon2.getRoll().getValue()) - ); - } - } - - /** - * A Digital Input {@link Sendable}. - */ - public static final class DigitalInput extends Hardware { - - private final Supplier value; - - /** - * Create the Digital Input sendable. - * @param label The label to use. Shown in the dashboard. - * @param digitalInput The Digital Input. - */ - public DigitalInput(String label, edu.wpi.first.wpilibj.DigitalInput digitalInput) { - super("DIO-" + digitalInput.getChannel(), label, digitalInput); - value = digitalInput::get; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("value", value::get, null); - } - } - - /** - * A Pneumatic Hub {@link Sendable}. - */ - public static final class PneumaticHub extends PoweredHardware { - - private final Supplier pressure; - private final Supplier pressureSwitchOn; - private final Supplier compressorOn; - - /** - * Create the Pneumatic Hub sendable. - * @param label The label to use. Shown in the dashboard. - * @param pneumaticHub The Pneumatic Hub. - */ - public PneumaticHub(String label, edu.wpi.first.wpilibj.PneumaticHub pneumaticHub) { - super( - "PneumaticController-" + pneumaticHub.getModuleNumber(), - label, - pneumaticHub, - () -> - (pneumaticHub.getInputVoltage() * pneumaticHub.getCompressorCurrent()) + - (pneumaticHub.getSolenoidsVoltage() * pneumaticHub.getSolenoidsTotalCurrent()), - () -> pneumaticHub.getInputVoltage(), - () -> - pneumaticHub.getCompressorCurrent() + - ((pneumaticHub.getSolenoidsVoltage() / pneumaticHub.getInputVoltage()) * pneumaticHub.getSolenoidsTotalCurrent()) - ); - pressure = () -> pneumaticHub.getPressure(0); - pressureSwitchOn = pneumaticHub::getPressureSwitch; - compressorOn = pneumaticHub::getCompressor; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("pressure", pressure::get, null); - builder.addBooleanProperty("pressureSwitchOn", pressureSwitchOn::get, null); - builder.addBooleanProperty("compressorOn", compressorOn::get, null); - } - } - - /** - * A Solenoid {@link Sendable}. - */ - public static final class Solenoid extends Hardware { - - private final Supplier value; - - /** - * Create the Solenoid sendable. - * @param label The label to use. Shown in the dashboard. - * @param solenoid The Solenoid. - */ - public Solenoid(String label, edu.wpi.first.wpilibj.Solenoid solenoid) { - super("Solenoid-" + solenoid.getChannel(), label, solenoid); - value = solenoid::get; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("value", value::get, null); - } - } - - /** - * A Double Solenoid {@link Sendable}. - */ - public static final class DoubleSolenoid extends Hardware { - - private final Supplier value; - - /** - * Create the Double Solenoid sendable. - * @param label The label to use. Shown in the dashboard. - * @param doubleSolenoid The Double Solenoid. - */ - public DoubleSolenoid(String label, edu.wpi.first.wpilibj.DoubleSolenoid doubleSolenoid) { - super("Solenoid-" + doubleSolenoid.getFwdChannel() + "-" + doubleSolenoid.getRevChannel(), label, doubleSolenoid); - value = () -> doubleSolenoid.get().ordinal(); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addIntegerProperty("value", value::get, null); - } - } -} diff --git a/src/main/java/org/team340/lib/controller/Controller.java b/src/main/java/org/team340/lib/controller/Controller.java new file mode 100644 index 0000000..0ea5202 --- /dev/null +++ b/src/main/java/org/team340/lib/controller/Controller.java @@ -0,0 +1,387 @@ +package org.team340.lib.controller; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.event.EventLoop; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** + * A modified {@link CommandXboxController}. + * Uses configured deadbands and thresholds when utilizing + * axis values, and all buttons are disabled when the robot + * is in autonomous mode. + */ +public class Controller extends CommandXboxController { + + private static final EventLoop loop = new EventLoop(); + + static { + CommandScheduler.getInstance() + .getDefaultButtonLoop() + .bind(() -> { + if (!RobotState.isAutonomous()) loop.poll(); + }); + } + + private final ControllerConfig config; + + /** + * Create the controller. + * @param config The controller's config. + */ + public Controller(ControllerConfig config) { + super(config.port); + this.config = config; + } + + /** + * Get the default button poll for controllers. + */ + public static EventLoop getDefaultButtonLoop() { + return loop; + } + + @Override + public Trigger button(int button) { + return button(button, loop); + } + + @Override + public Trigger pov(int angle) { + return pov(0, angle, loop); + } + + @Override + public Trigger povUp() { + return pov(0); + } + + @Override + public Trigger povUpRight() { + return pov(45); + } + + @Override + public Trigger povRight() { + return pov(90); + } + + @Override + public Trigger povDownRight() { + return pov(135); + } + + @Override + public Trigger povDown() { + return pov(180); + } + + @Override + public Trigger povDownLeft() { + return pov(225); + } + + @Override + public Trigger povLeft() { + return pov(270); + } + + @Override + public Trigger povUpLeft() { + return pov(315); + } + + @Override + public Trigger povCenter() { + return pov(-1); + } + + @Override + public Trigger axisLessThan(int axis, double threshold) { + return axisLessThan(axis, threshold, loop); + } + + @Override + public Trigger axisGreaterThan(int axis, double threshold) { + return axisGreaterThan(axis, threshold, loop); + } + + @Override + public Trigger a() { + return a(loop); + } + + @Override + public Trigger b() { + return b(loop); + } + + @Override + public Trigger x() { + return x(loop); + } + + @Override + public Trigger y() { + return y(loop); + } + + @Override + public Trigger leftBumper() { + return leftBumper(loop); + } + + @Override + public Trigger rightBumper() { + return rightBumper(loop); + } + + @Override + public Trigger back() { + return back(loop); + } + + @Override + public Trigger start() { + return start(loop); + } + + @Override + public Trigger leftStick() { + return leftStick(loop); + } + + @Override + public Trigger rightStick() { + return rightStick(loop); + } + + @Override + public Trigger leftTrigger(double threshold) { + return leftTrigger(threshold, loop); + } + + @Override + public Trigger rightTrigger(double threshold) { + return rightTrigger(threshold, loop); + } + + /** + * Constructs a Trigger instance around the axis value of the left trigger. + * @return A {@link Trigger} instance. + */ + @Override + public Trigger leftTrigger() { + return leftTrigger(config.triggerThreshold); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. + * @return A {@link Trigger} instance. + */ + @Override + public Trigger rightTrigger() { + return rightTrigger(config.triggerThreshold); + } + + /** + * Constructs an event instance around the up position of the left joystick. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickUp() { + return leftJoystickUp(config.joystickThreshold); + } + + /** + * Constructs an event instance around the up position of the left joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickUp(double threshold) { + return axisLessThan(Axis.kLeftY.value, -threshold); + } + + /** + * Constructs an event instance around the down position of the left joystick. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickDown() { + return leftJoystickDown(config.joystickThreshold); + } + + /** + * Constructs an event instance around the down position of the left joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickDown(double threshold) { + return axisGreaterThan(Axis.kLeftY.value, threshold); + } + + /** + * Constructs an event instance around the left position of the left joystick. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickLeft() { + return leftJoystickLeft(config.joystickThreshold); + } + + /** + * Constructs an event instance around the left position of the left joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickLeft(double threshold) { + return axisLessThan(Axis.kLeftX.value, -threshold); + } + + /** + * Constructs an event instance around the right position of the left joystick. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickRight() { + return leftJoystickRight(config.joystickThreshold); + } + + /** + * Constructs an event instance around the right position of the left joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger leftJoystickRight(double threshold) { + return axisGreaterThan(Axis.kLeftX.value, threshold); + } + + /** + * Constructs an event instance around the up position of the right joystick. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickUp() { + return rightJoystickUp(config.joystickThreshold); + } + + /** + * Constructs an event instance around the up position of the right joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickUp(double threshold) { + return axisLessThan(Axis.kRightY.value, -threshold); + } + + /** + * Constructs an event instance around the down position of the right joystick. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickDown() { + return rightJoystickDown(config.joystickThreshold); + } + + /** + * Constructs an event instance around the down position of the right joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickDown(double threshold) { + return axisGreaterThan(Axis.kRightY.value, threshold); + } + + /** + * Constructs an event instance around the left position of the right joystick. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickLeft() { + return rightJoystickLeft(config.joystickThreshold); + } + + /** + * Constructs an event instance around the left position of the right joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickLeft(double threshold) { + return axisLessThan(Axis.kRightX.value, -threshold); + } + + /** + * Constructs an event instance around the right position of the right joystick. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickRight() { + return rightJoystickRight(config.joystickThreshold); + } + + /** + * Constructs an event instance around the right position of the right joystick. + * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. + * @return A {@link Trigger} instance. + */ + public Trigger rightJoystickRight(double threshold) { + return axisGreaterThan(Axis.kRightX.value, threshold); + } + + /** + * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of left joystick. + * @return The axis value. + */ + @Override + public double getLeftX() { + return MathUtil.applyDeadband(super.getLeftX(), config.joystickDeadband); + } + + /** + * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of left joystick. + * @return The axis value. + */ + @Override + public double getLeftY() { + return MathUtil.applyDeadband(super.getLeftY(), config.joystickDeadband); + } + + /** + * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of right joystick. + * @return The axis value. + */ + @Override + public double getRightX() { + return MathUtil.applyDeadband(super.getRightX(), config.joystickDeadband); + } + + /** + * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of right joystick. + * @return The axis value. + */ + @Override + public double getRightY() { + return MathUtil.applyDeadband(super.getRightY(), config.joystickDeadband); + } + + /** + * Gets the left trigger value (no press = {@code 0.0}, full press = {@code 1.0}). + * @return The axis value. + */ + @Override + public double getLeftTriggerAxis() { + return MathUtil.applyDeadband(super.getLeftTriggerAxis(), config.triggerDeadband); + } + + /** + * Gets the right trigger value (no press = {@code 0.0}, full press = {@code 1.0}). + * @return The axis value. + */ + @Override + public double getRightTriggerAxis() { + return MathUtil.applyDeadband(super.getRightTriggerAxis(), config.triggerDeadband); + } + + /** + * Gets the difference between the trigger values ({@code left - right}). + * @return The difference between the trigger values. + */ + public double getTriggerDifference() { + return getLeftTriggerAxis() - getRightTriggerAxis(); + } +} diff --git a/src/main/java/org/team340/lib/controller/Controller2.java b/src/main/java/org/team340/lib/controller/Controller2.java deleted file mode 100644 index aeb9023..0000000 --- a/src/main/java/org/team340/lib/controller/Controller2.java +++ /dev/null @@ -1,470 +0,0 @@ -package org.team340.lib.controller; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import org.team340.lib.GRRDashboard; - -/** - * A modified {@link CommandXboxController}. - */ -public class Controller2 extends CommandXboxController implements Sendable { - - private final Controller2Config config; - private final JoystickProfile leftProfile; - private final JoystickProfile rightProfile; - - /** - * Create the controller. - * @param config The controller's config. - */ - public Controller2(Controller2Config config) { - super(config.getPort()); - this.config = config; - this.leftProfile = config.getLeftProfile().isEmpty() ? null : JoystickProfile.fromFile(config.getLeftProfile()); - this.rightProfile = config.getRightProfile().isEmpty() ? null : JoystickProfile.fromFile(config.getRightProfile()); - } - - /** - * Gets the controller's configured label. - */ - public String getLabel() { - return config.getLabel(); - } - - /** - * Constructs an event instance around the up position of the left joystick. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickUp() { - return leftJoystickUp(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the up position of the left joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickUp(double threshold) { - return new Trigger(() -> super.getLeftY() < -threshold); - } - - /** - * Constructs an event instance around the down position of the left joystick. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickDown() { - return leftJoystickDown(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the down position of the left joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickDown(double threshold) { - return new Trigger(() -> super.getLeftY() > threshold); - } - - /** - * Constructs an event instance around the left position of the left joystick. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickLeft() { - return leftJoystickLeft(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the left position of the left joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickLeft(double threshold) { - return new Trigger(() -> super.getLeftX() < -threshold); - } - - /** - * Constructs an event instance around the right position of the left joystick. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickRight() { - return leftJoystickRight(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the right position of the left joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger leftJoystickRight(double threshold) { - return new Trigger(() -> super.getLeftX() > threshold); - } - - /** - * Constructs an event instance around the up position of the right joystick. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickUp() { - return rightJoystickUp(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the up position of the right joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickUp(double threshold) { - return new Trigger(() -> super.getRightY() < -threshold); - } - - /** - * Constructs an event instance around the down position of the right joystick. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickDown() { - return rightJoystickDown(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the down position of the right joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickDown(double threshold) { - return new Trigger(() -> super.getRightY() > threshold); - } - - /** - * Constructs an event instance around the left position of the right joystick. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickLeft() { - return rightJoystickLeft(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the left position of the right joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickLeft(double threshold) { - return new Trigger(() -> super.getRightX() < -threshold); - } - - /** - * Constructs an event instance around the right position of the right joystick. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickRight() { - return rightJoystickRight(config.getJoystickThreshold()); - } - - /** - * Constructs an event instance around the right position of the right joystick. - * @param threshold The minimum axis value for the returned {@link Trigger} to be true. Should be between {@code 0.0} and {@code 1.0}. Deadband is not applied. - * @return A {@link Trigger} instance. - */ - public Trigger rightJoystickRight(double threshold) { - return new Trigger(() -> super.getRightX() > threshold); - } - - /** - * Constructs a Trigger instance around the axis value of the left trigger. - * @return A {@link Trigger} instance. - */ - @Override - public Trigger leftTrigger() { - return super.leftTrigger(config.getTriggerThreshold()); - } - - /** - * Constructs a Trigger instance around the axis value of the right trigger. - * @return A {@link Trigger} instance. - */ - @Override - public Trigger rightTrigger() { - return super.rightTrigger(config.getTriggerThreshold()); - } - - /**` - * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of left joystick. - * @return The axis value. - */ - @Override - public double getLeftX() { - double raw = super.getLeftX(); - return MathUtil.applyDeadband(leftProfile == null ? raw : leftProfile.getX(raw, super.getLeftY()), config.getJoystickDeadband()); - } - - /** - * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of left joystick. - * @param multiplier A multiplier for the value of the joystick. - * @return The axis value. - */ - public double getLeftX(double multiplier) { - return getLeftX() * multiplier; - } - - /** - * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of left joystick. - * @param multiplier A multiplier for the value of the joystick. - * @param exp An exponential factor to apply to the value of the joystick. Applied before the multiplier. - * @return The axis value. - */ - public double getLeftX(double multiplier, double exp) { - if (exp == 1.0) return getLeftX(multiplier); - - double val = getLeftX(); - if (val == 0.0) return val; - - double norm = Math.hypot(val, getLeftY()); - double expMultiplier = exp == 2.0 ? norm : (exp == 3.0 ? norm * norm : Math.pow(norm, exp - 1.0)); - - return val * expMultiplier * multiplier; - } - - /** - * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of left joystick. - * @return The axis value. - */ - @Override - public double getLeftY() { - double raw = super.getLeftY(); - return MathUtil.applyDeadband(leftProfile == null ? raw : leftProfile.getY(super.getLeftX(), raw), config.getJoystickDeadband()); - } - - /** - * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of left joystick. - * @param multiplier A multiplier for the value of the joystick. - * @return The axis value. - */ - public double getLeftY(double multiplier) { - return getLeftY() * multiplier; - } - - /** - * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of left joystick. - * @param multiplier A multiplier for the value of the joystick. - * @param exp An exponential factor to apply to the value of the joystick. Applied before the multiplier. - * @return The axis value. - */ - public double getLeftY(double multiplier, double exp) { - if (exp == 1.0) return getLeftY(multiplier); - - double val = getLeftY(); - if (val == 0.0) return val; - - double norm = Math.hypot(val, getLeftX()); - double expMultiplier = exp == 2.0 ? norm : (exp == 3.0 ? norm * norm : Math.pow(norm, exp - 1.0)); - - return val * expMultiplier * multiplier; - } - - /** - * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of right joystick. - * @return The axis value. - */ - @Override - public double getRightX() { - double raw = super.getRightX(); - return MathUtil.applyDeadband(rightProfile == null ? raw : rightProfile.getX(raw, super.getRightY()), config.getJoystickDeadband()); - } - - /** - * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of right joystick. - * @param multiplier A multiplier for the value of the joystick. - * @return The axis value. - */ - public double getRightX(double multiplier) { - return getRightX() * multiplier; - } - - /** - * Gets the X axis value (left = {@code -1.0}, right = {@code 1.0}) of right joystick. - * @param multiplier A multiplier for the value of the joystick. - * @param exp An exponential factor to apply to the value of the joystick. Applied before the multiplier. - * @return The axis value. - */ - public double getRightX(double multiplier, double exp) { - if (exp == 1.0) return getRightX(multiplier); - - double val = getRightX(); - if (val == 0.0) return val; - - double norm = Math.hypot(val, getRightY()); - double expMultiplier = exp == 2.0 ? norm : (exp == 3.0 ? norm * norm : Math.pow(norm, exp - 1.0)); - - return val * expMultiplier * multiplier; - } - - /** - * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of right joystick. - * @return The axis value. - */ - @Override - public double getRightY() { - double raw = super.getRightY(); - return MathUtil.applyDeadband(rightProfile == null ? raw : rightProfile.getY(super.getRightX(), raw), config.getJoystickDeadband()); - } - - /** - * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of right joystick. - * @param multiplier A multiplier for the value of the joystick. - * @return The axis value. - */ - public double getRightY(double multiplier) { - return getRightY() * multiplier; - } - - /** - * Gets the Y axis value (up = {@code -1.0}, down = {@code 1.0}) of right joystick. - * @param multiplier A multiplier for the value of the joystick. - * @param exp An exponential factor to apply to the value of the joystick. Applied before the multiplier. - * @return The axis value. - */ - public double getRightY(double multiplier, double exp) { - if (exp == 1.0) return getRightY(multiplier); - - double val = getRightY(); - if (val == 0.0) return val; - - double norm = Math.hypot(val, getRightX()); - double expMultiplier = exp == 2.0 ? norm : (exp == 3.0 ? norm * norm : Math.pow(norm, exp - 1.0)); - - return val * expMultiplier * multiplier; - } - - /** - * Gets the left trigger value (no press = {@code 0.0}, full press = {@code 1.0}). - * @return The axis value. - */ - @Override - public double getLeftTriggerAxis() { - return Math.max(MathUtil.applyDeadband(super.getLeftTriggerAxis(), config.getTriggerDeadband()), 0.0); - } - - /** - * Gets the left trigger value (no press = {@code 0.0}, full press = {@code 1.0}). - * @param multiplier A multiplier for the value of the trigger. - * @return The axis value. - */ - public double getLeftTriggerAxis(double multiplier) { - return getLeftTriggerAxis() * multiplier; - } - - /** - * Gets the left trigger value (no press = {@code 0.0}, full press = {@code 1.0}). - * @param multiplier A multiplier for the value of the trigger. - * @param exp An exponential factor to apply to the value of the trigger. Applied before the multiplier. - * @return The axis value. - */ - public double getLeftTriggerAxis(double multiplier, double exp) { - if (exp == 1.0) return getLeftTriggerAxis(multiplier); - double raw = getLeftTriggerAxis(); - return (exp == 2.0 ? raw * raw : (exp == 3.0 ? raw * raw * raw : Math.pow(raw, exp))) * multiplier; - } - - /** - * Gets the right trigger value (no press = {@code 0.0}, full press = {@code 1.0}). - * @return The axis value. - */ - @Override - public double getRightTriggerAxis() { - return Math.max(MathUtil.applyDeadband(super.getRightTriggerAxis(), config.getTriggerDeadband()), 0.0); - } - - /** - * Gets the right trigger value (no press = {@code 0.0}, full press = {@code 1.0}). - * @param multiplier A multiplier for the value of the trigger. - * @return The axis value. - */ - public double getRightTriggerAxis(double multiplier) { - return getRightTriggerAxis() * multiplier; - } - - /** - * Gets the right trigger value (no press = {@code 0.0}, full press = {@code 1.0}). - * @param multiplier A multiplier for the value of the trigger. - * @param exp An exponential factor to apply to the value of the trigger. Applied before the multiplier. - * @return The axis value. - */ - public double getRightTriggerAxis(double multiplier, double exp) { - if (exp == 1.0) return getRightTriggerAxis(multiplier); - double raw = getRightTriggerAxis(); - return (exp == 2.0 ? raw * raw : (exp == 3.0 ? raw * raw * raw : Math.pow(raw, exp))) * multiplier; - } - - /** - * Gets the difference between the trigger values ({@code left - right}). - * @return The difference between the trigger values. - */ - public double getTriggerDifference() { - return getLeftTriggerAxis() - getRightTriggerAxis(); - } - - /** - * Gets the difference between the trigger values ({@code left - right}). - * @param multiplier A multiplier for the difference of the triggers. - * @return The difference between the trigger values. - */ - public double getTriggerDifference(double multiplier) { - return getTriggerDifference() * multiplier; - } - - /** - * Gets the difference between the trigger values ({@code left - right}). - * @param multiplier A multiplier for the difference of the triggers. - * @param exp An exponential factor to apply to the difference of the triggers. Applied before the multiplier. - * @return The difference between the trigger values. - */ - public double getTriggerDifference(double multiplier, double exp) { - if (exp == 1.0) return getTriggerDifference(multiplier); - double val = getTriggerDifference(); - if (exp == 2.0) return Math.copySign(val * val, val) * multiplier; - if (exp == 3.0) return val * val * val * multiplier; - return Math.copySign(Math.pow(Math.abs(val), exp), val) * multiplier; - } - - /** - * Adds the controller to the dashboard. - */ - public void addToDashboard() { - GRRDashboard.addController(this); - } - - @Override - public void initSendable(SendableBuilder builder) { - XboxController hid = getHID(); - - builder.publishConstString(".api", "Controller2"); - - builder.addBooleanProperty("a", hid::getAButton, null); - builder.addBooleanProperty("b", hid::getBButton, null); - builder.addBooleanProperty("x", hid::getXButton, null); - builder.addBooleanProperty("y", hid::getYButton, null); - - builder.addBooleanProperty("back", hid::getBackButton, null); - builder.addBooleanProperty("start", hid::getStartButton, null); - - builder.addIntegerProperty("pov", hid::getPOV, null); - - builder.addBooleanProperty("ls", hid::getLeftStickButton, null); - builder.addDoubleProperty("lx", this::getLeftX, null); - builder.addDoubleProperty("ly", this::getLeftY, null); - builder.addDoubleProperty("ln", () -> Math.hypot(getLeftX(), getLeftY()), null); - builder.addDoubleProperty("lns", () -> Math.hypot(super.getLeftX(), super.getLeftY()), null); - - builder.addBooleanProperty("rs", hid::getRightStickButton, null); - builder.addDoubleProperty("rx", this::getRightX, null); - builder.addDoubleProperty("ry", this::getRightY, null); - - builder.addBooleanProperty("lb", hid::getLeftBumper, null); - builder.addBooleanProperty("rb", hid::getRightBumper, null); - - builder.addDoubleProperty("lt", this::getLeftTriggerAxis, null); - builder.addDoubleProperty("rt", this::getRightTriggerAxis, null); - } -} diff --git a/src/main/java/org/team340/lib/controller/Controller2Config.java b/src/main/java/org/team340/lib/controller/Controller2Config.java deleted file mode 100644 index b9e9fca..0000000 --- a/src/main/java/org/team340/lib/controller/Controller2Config.java +++ /dev/null @@ -1,147 +0,0 @@ -package org.team340.lib.controller; - -/** - * Config builder for {@link Controller2}. - */ -public class Controller2Config { - - private String label = "Controller"; - private int port = -1; - private double joystickDeadband = 0.0; - private double joystickThreshold = 0.0; - private double triggerDeadband = 0.0; - private double triggerThreshold = 0.0; - private String leftProfile = ""; - private String rightProfile = ""; - - /** - * Sets the controller's label. - * Used in network tables. - * @param label The label to use. - */ - public Controller2Config setLabel(String label) { - this.label = label; - return this; - } - - /** - * Gets the controller's configured label. - */ - public String getLabel() { - return label; - } - - /** - * Sets the port the controller is connected to. - * @param port The port the controller is connected to. - */ - public Controller2Config setPort(int port) { - this.port = port; - return this; - } - - /** - * Gets the controller's configured port. - */ - public int getPort() { - return port; - } - - /** - * Sets the deadband of the controller's joysticks. - * @param joystickDeadband The deadband to use. Should be a value from {@code 0.0} to {@code 1.0}. - */ - public Controller2Config setJoystickDeadband(double joystickDeadband) { - this.joystickDeadband = joystickDeadband; - return this; - } - - /** - * Gets the configured joystick deadband. - */ - public double getJoystickDeadband() { - return joystickDeadband; - } - - /** - * Sets the threshold of the controller's joysticks when being used as "buttons". - * @param joystickThreshold The threshold to use. Should be a value from {@code 0.0} to {@code 1.0}. - */ - public Controller2Config setJoystickThreshold(double joystickThreshold) { - this.joystickThreshold = joystickThreshold; - return this; - } - - /** - * Gets the configured joystick threshold. - */ - public double getJoystickThreshold() { - return joystickThreshold; - } - - /** - * Sets the deadband of the controller's triggers. - * @param triggerDeadband The deadband to use. Should be a value from {@code 0.0} to {@code 1.0}. - */ - public Controller2Config setTriggerDeadband(double triggerDeadband) { - this.triggerDeadband = triggerDeadband; - return this; - } - - /** - * Gets the configured trigger deadband. - */ - public double getTriggerDeadband() { - return triggerDeadband; - } - - /** - * Sets the threshold of the controller's triggers when being used as "buttons". - * @param triggerThreshold The threshold to use. Should be a value from {@code 0.0} to {@code 1.0}. - */ - public Controller2Config setTriggerThreshold(double triggerThreshold) { - this.triggerThreshold = triggerThreshold; - return this; - } - - /** - * Gets the configured trigger threshold. - */ - public double getTriggerThreshold() { - return triggerThreshold; - } - - /** - * Sets the profile file for the controller's left joystick. - * An empty string disables the joystick profile. - * @param leftProfile The file path for the controller's left joystick profile. - */ - public Controller2Config setLeftProfile(String leftProfile) { - this.leftProfile = leftProfile; - return this; - } - - /** - * Gets the file path of the controller's left joystick profile. - */ - public String getLeftProfile() { - return leftProfile; - } - - /** - * Sets the profile file for the controller's right joystick. - * An empty string disables the joystick profile. - * @param rightProfile The file path for the controller's left joystick profile. - */ - public Controller2Config setRightProfile(String rightProfile) { - this.rightProfile = rightProfile; - return this; - } - - /** - * Gets the file path of the controller's right joystick profile. - */ - public String getRightProfile() { - return rightProfile; - } -} diff --git a/src/main/java/org/team340/lib/controller/ControllerConfig.java b/src/main/java/org/team340/lib/controller/ControllerConfig.java new file mode 100644 index 0000000..6d8b18b --- /dev/null +++ b/src/main/java/org/team340/lib/controller/ControllerConfig.java @@ -0,0 +1,49 @@ +package org.team340.lib.controller; + +/** + * Config builder for {@link Controller}. + */ +public class ControllerConfig { + + /** The port number the controller is connected to in the DS. */ + public int port = 0; + /** The configured deadband for the controller's joysticks. */ + public double joystickDeadband = 0.1; + /** The configured deadband for the controller's triggers. */ + public double triggerDeadband = 0.1; + /** The configured threshold for "button" usage of the controller's joysticks. */ + public double joystickThreshold = 0.5; + /** The configured threshold for "button" usage of the controller's triggers. */ + public double triggerThreshold = 0.5; + + /** + * Sets the port the controller is connected to in the DS. + * @param port The port the controller is connected to. + */ + public ControllerConfig setPort(int port) { + this.port = port; + return this; + } + + /** + * Sets the deadband of the controller's joysticks and triggers. + * @param joysticks The deadband to use for joysticks. Should be a value from {@code 0.0} to {@code 1.0}. + * @param triggers The deadband to use for triggers. Should be a value from {@code 0.0} to {@code 1.0}. + */ + public ControllerConfig setDeadbands(double joysticks, double triggers) { + joystickDeadband = joysticks; + triggerDeadband = triggers; + return this; + } + + /** + * Sets the threshold of the controller's joysticks and triggers when being used as "buttons". + * @param joysticks The threshold to use for joysticks. Should be a value from {@code 0.0} to {@code 1.0}. + * @param triggers The threshold to use for triggers. Should be a value from {@code 0.0} to {@code 1.0}. + */ + public ControllerConfig setThresholds(double joysticks, double triggers) { + joystickThreshold = joysticks; + triggerThreshold = triggers; + return this; + } +} diff --git a/src/main/java/org/team340/lib/controller/ControllerLogger.java b/src/main/java/org/team340/lib/controller/ControllerLogger.java new file mode 100644 index 0000000..1bf58ac --- /dev/null +++ b/src/main/java/org/team340/lib/controller/ControllerLogger.java @@ -0,0 +1,18 @@ +package org.team340.lib.controller; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(Controller.class) +public class ControllerLogger extends ClassSpecificLogger { + + public ControllerLogger() { + super(Controller.class); + } + + @Override + public void update(DataLogger logger, Controller controller) { + // No-op, use DriverStation.startDataLog(DataLog) + } +} diff --git a/src/main/java/org/team340/lib/controller/JoystickProfile.java b/src/main/java/org/team340/lib/controller/JoystickProfile.java deleted file mode 100644 index d704533..0000000 --- a/src/main/java/org/team340/lib/controller/JoystickProfile.java +++ /dev/null @@ -1,56 +0,0 @@ -package org.team340.lib.controller; - -import com.fasterxml.jackson.core.type.TypeReference; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.File; -import java.util.List; -import org.team340.lib.util.Polar2d; - -public class JoystickProfile { - - private InterpolatingDoubleTreeMap profile; - - private JoystickProfile(InterpolatingDoubleTreeMap profile) { - this.profile = profile; - } - - public double getX(double rawX, double rawY) { - double theta = Math.atan2(rawY, rawX); - return rawX / profile.get(theta); - } - - public double getY(double rawX, double rawY) { - double theta = Math.atan2(rawY, rawX); - return rawY / profile.get(theta); - } - - public static JoystickProfile fromFile(String filePath) { - InterpolatingDoubleTreeMap profile = new InterpolatingDoubleTreeMap(); - - try { - File file = new File(Filesystem.getDeployDirectory(), filePath); - List> points = new ObjectMapper().readValue(file, new TypeReference>>() {}); - - for (List point : points) { - profile.put(point.get(0), point.get(1)); - } - } catch (Exception e) { - e.printStackTrace(); - profile.clear(); - profile.put(0.0, 1.0); - } - - return new JoystickProfile(profile); - } - - public static JoystickProfile fromPolar2dList(List list) { - InterpolatingDoubleTreeMap profile = new InterpolatingDoubleTreeMap(); - for (Polar2d point : list) { - profile.put(point.getTheta(), point.getR()); - } - - return new JoystickProfile(profile); - } -} diff --git a/src/main/java/org/team340/lib/controller/JoystickProfiler.java b/src/main/java/org/team340/lib/controller/JoystickProfiler.java deleted file mode 100644 index 7fd59fa..0000000 --- a/src/main/java/org/team340/lib/controller/JoystickProfiler.java +++ /dev/null @@ -1,107 +0,0 @@ -package org.team340.lib.controller; - -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.ArrayList; -import java.util.List; -import org.team340.lib.commands.CommandBuilder; -import org.team340.lib.util.Math2; -import org.team340.lib.util.Polar2d; - -/** - * Records a controller's joystick outputs and generates a corresponding JSON string. - * For use in correcting irregular joystick output shape. - */ -public class JoystickProfiler { - - private final GenericHID controller; - private final InterpolatingDoubleTreeMap data = new InterpolatingDoubleTreeMap(); - private final int xAxis; - private final int yAxis; - - /** - * Create the profiler. - * @param controller The controller to profile. - * @param xAxis The index of the controller's X axis. - * @param yAxis The index of the controller's Y axis. - */ - public JoystickProfiler(GenericHID controller, int xAxis, int yAxis) { - this.controller = controller; - this.xAxis = xAxis; - this.yAxis = yAxis; - } - - /** - * Clears currently saved data. - */ - public void clearData() { - data.clear(); - } - - /** - * Collects data from the joystick. Should be run periodically. - */ - public void pollData() { - double x = controller.getRawAxis(xAxis); - double y = controller.getRawAxis(yAxis); - double theta = Math.atan2(y, x); - double r = Math.hypot(x, y); - System.out.println("T: " + Math2.toFixed(theta) + " R: " + Math2.toFixed(r)); - data.put(theta, r); - } - - /** - * Generates the profile using data collected by {@link JoystickProfiler#pollData() pollData()}. - * @param samplePoints Number of points to sample. - * @return The joystick profile. - */ - public List generateProfile(int samplePoints) { - List profile = new ArrayList<>(); - - for (int i = 0; i < samplePoints; i++) { - double theta = ((double) i / (double) samplePoints * Math2.TWO_PI) - Math.PI; - profile.add(new Polar2d(theta, data.get(theta))); - } - return profile; - } - - /** - * Write profile generated by {@link JoystickProfiler#generateProfile(int samplePoints) generateProfile()} to console as a JSON string. - * The output can be saved to a file and loaded by {@link JoystickProfile}. - * @param rawProfile Profile to write to file. - * @return The JSON string. - */ - public static String writeToConsole(List rawProfile) { - List> profile = new ArrayList>(); - for (Polar2d polar2d : rawProfile) { - profile.add(List.of(polar2d.getTheta(), polar2d.getR())); - } - - try { - String json = new ObjectMapper().writeValueAsString(profile); - System.out.println(json); - return json; - } catch (Exception e) { - e.printStackTrace(); - return "[]"; - } - } - - /** - * Command to profile a controller's joystick and print to console. - * @param controller Controller to profile. - * @param xAxis X-Axis of controller joystick. - * @param yAxis Y-Axis of controller joystick. - * @param samplePoints Number of points to sample. - */ - public static Command run(GenericHID controller, int xAxis, int yAxis, int samplePoints) { - JoystickProfiler profiler = new JoystickProfiler(controller, xAxis, yAxis); - return new CommandBuilder() - .onInitialize(profiler::clearData) - .onExecute(profiler::pollData) - .onEnd(() -> writeToConsole(profiler.generateProfile(samplePoints))) - .ignoringDisable(true); - } -} diff --git a/src/main/java/org/team340/lib/dashboard/GRRDashboard.java b/src/main/java/org/team340/lib/dashboard/GRRDashboard.java new file mode 100644 index 0000000..fb6fdfa --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/GRRDashboard.java @@ -0,0 +1,180 @@ +package org.team340.lib.dashboard; + +import com.choreo.lib.ChoreoTrajectory; +import com.choreo.lib.ChoreoTrajectoryState; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.Pair; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayPublisher; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.event.EventLoop; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import java.math.BigDecimal; +import java.math.RoundingMode; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import java.util.UUID; +import org.team340.lib.util.Alliance; + +/** + * Utility class for interfacing with GRRDashboard. + */ +public final class GRRDashboard { + + private GRRDashboard() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("GRRDashboard"); + private static final EventLoop periodic = new EventLoop(); + + private static final BooleanSubscriber allianceOverrideActiveSub = nt + .getBooleanTopic("AllianceOverride/active") + .subscribe(false); + private static final BooleanSubscriber allianceOverrideIsBlueSub = nt + .getBooleanTopic("AllianceOverride/isBlue") + .subscribe(false); + + private static final Map> autoOptions = new LinkedHashMap<>(); // { id: [command, json] } + private static final StringArrayPublisher autoOptionsPub = nt.getStringArrayTopic("Autos/options").publish(); + private static final StringPublisher activeAutoPub = nt.getStringTopic("Autos/active").publish(); + private static final StringSubscriber selectedAutoSub; + + private static Command selectedAuto = Commands.none(); + + static { + String defaultAuto = addAuto("Do Nothing", selectedAuto); + selectedAutoSub = nt.getStringTopic("Autos/selected").subscribe(defaultAuto); + activeAutoPub.set(defaultAuto); + } + + /** + * Adds an auto to the dashboard. + * @param label The label for the auto. + * @param command The auto's command. + */ + public static String addAuto(String label, Command command) { + return addAuto(label, command, new ChoreoTrajectory[] {}); + } + + /** + * Adds an auto to the dashboard. + * @param label The label for the auto. + * @param command The auto's command. + * @param trajectory The trajectory utilized by the auto. + */ + public static String addAuto(String label, Command command, ChoreoTrajectory trajectory) { + return addAuto(label, command, new ChoreoTrajectory[] { trajectory }); + } + + /** + * Adds an auto to the dashboard. + * @param label The label for the auto. + * @param command The auto's command. + * @param trajectories Trajectories utilized by the auto. + */ + public static String addAuto(String label, Command command, List trajectories) { + return addAuto(label, command, trajectories.stream().toArray(ChoreoTrajectory[]::new)); + } + + /** + * Adds an auto to the dashboard. + * @param label The label for the auto. + * @param command The auto's command. + * @param trajectories Trajectories utilized by the auto. + */ + public static String addAuto(String label, Command command, ChoreoTrajectory[] trajectories) { + String id = UUID.randomUUID().toString(); + List points = new ArrayList<>(); + + double lastTimestamp = 0.0; + for (int i = 0; i < trajectories.length; i++) { + ChoreoTrajectoryState[] states = trajectories[i].getStates(); + if (i > 0 && trajectories[i - 1].getStates().length > 0) lastTimestamp += + trajectories[i - 1].getFinalState().timestamp; + for (ChoreoTrajectoryState state : states) { + points.add( + new BigDecimal[] { + new BigDecimal(state.x).setScale(3, RoundingMode.HALF_UP), + new BigDecimal(state.y).setScale(3, RoundingMode.HALF_UP), + new BigDecimal(state.heading).setScale(2, RoundingMode.HALF_UP), + new BigDecimal(state.timestamp + lastTimestamp).setScale(3, RoundingMode.HALF_UP) + } + ); + } + } + + ChoreoTrajectory lastTrajectory = trajectories.length > 0 + ? trajectories[trajectories.length - 1] + : new ChoreoTrajectory(); + double time = + lastTimestamp + (lastTrajectory.getStates().length > 0 ? lastTrajectory.getFinalState().timestamp : 0.0); + + String json = ""; + try { + json = new ObjectMapper() + .writeValueAsString( + new HashMap<>() { + { + put("id", id); + put("label", label); + put("points", points); + put("time", time); + } + } + ); + } catch (Exception e) { + e.printStackTrace(); + json = ""; + } + + if (json.isEmpty()) json = "{ \"id\": \"" + id + "\", \"label\": \"" + label + "\", \"points\": [] }"; + autoOptions.put(id, Pair.of(command, json)); + autoOptionsPub.set(autoOptions.values().stream().map(entry -> entry.getSecond()).toArray(String[]::new)); + return id; + } + + /** + * Gets the command of the selected auto. + */ + public static Command getSelectedAuto() { + return selectedAuto; + } + + /** + * Syncs data with the dashboard. Must be called + * periodically in order for this class to function. + */ + public static void update() { + if (allianceOverrideActiveSub.get()) { + Alliance.enableOverride(allianceOverrideIsBlueSub.get()); + } else { + Alliance.disableOverride(); + } + + for (String id : selectedAutoSub.readQueueValues()) { + var entry = autoOptions.get(id); + if (entry != null) { + activeAutoPub.set(id); + selectedAuto = entry.getFirst(); + } + } + + periodic.poll(); + } + + /** + * Binds an action to the dashboard's update loop. + * @param action The action to bind. + */ + static void bind(Runnable action) { + periodic.bind(action); + } +} diff --git a/src/main/java/org/team340/lib/dashboard/Tunable.java b/src/main/java/org/team340/lib/dashboard/Tunable.java new file mode 100644 index 0000000..ff1904a --- /dev/null +++ b/src/main/java/org/team340/lib/dashboard/Tunable.java @@ -0,0 +1,254 @@ +package org.team340.lib.dashboard; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.Logged.Strategy; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.Subscriber; +import java.util.function.Consumer; +import java.util.function.Supplier; + +/** + * The Tunable class is used to construct tunable properties of the robot to be modified + * via NetworkTables, as well as automatically displayed and edited in the dashboard. + */ +@Logged(strategy = Strategy.OPT_IN) +public final class Tunable implements Supplier, AutoCloseable { + + private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("GRRDashboard/Tunables"); + + private final Subscriber sub; + private final Supplier getter; + private long lastSet; + + private Tunable(Subscriber sub, Supplier getter, Consumer onChange) { + this.sub = sub; + this.getter = getter; + + if (onChange != null) { + GRRDashboard.bind(() -> { + long lastChange = sub.getLastChange(); + if (lastChange != lastSet) { + lastSet = lastChange; + onChange.accept(getter.get()); + } + }); + } + } + + /** + * Gets the current value of the tunable. + */ + @Override + public T get() { + return getter.get(); + } + + @Override + public void close() { + sub.close(); + } + + /** + * Creates a tunable boolean. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static Tunable booleanValue(String name, boolean defaultValue) { + return booleanValue(name, defaultValue, null); + } + + /** + * Creates a tunable boolean. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static Tunable booleanValue(String name, boolean defaultValue, Consumer onChange) { + var entry = nt.getBooleanTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + return new Tunable<>(entry, () -> entry.get(), onChange); + } + + /** + * Creates a tunable integer. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static Tunable intValue(String name, int defaultValue) { + return intValue(name, defaultValue, null); + } + + /** + * Creates a tunable integer. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static Tunable intValue(String name, int defaultValue, Consumer onChange) { + var entry = nt.getIntegerTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + return new Tunable<>(entry, () -> (int) entry.get(), onChange); + } + + /** + * Creates a tunable float. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static Tunable floatValue(String name, float defaultValue) { + return floatValue(name, defaultValue, null); + } + + /** + * Creates a tunable float. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static Tunable floatValue(String name, float defaultValue, Consumer onChange) { + var entry = nt.getFloatTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + return new Tunable<>(entry, () -> entry.get(), onChange); + } + + /** + * Creates a tunable double. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static Tunable doubleValue(String name, double defaultValue) { + return doubleValue(name, defaultValue, null); + } + + /** + * Creates a tunable double. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static Tunable doubleValue(String name, double defaultValue, Consumer onChange) { + var entry = nt.getDoubleTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + return new Tunable<>(entry, () -> entry.get(), onChange); + } + + /** + * Creates a tunable string. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + */ + public static Tunable stringValue(String name, String defaultValue) { + return stringValue(name, defaultValue, null); + } + + /** + * Creates a tunable string. + * @param name The name for the tunable. Must be unique. + * @param defaultValue The default value of the tunable (e.g. a programmed constant). + * @param onChange A consumer that is invoked when the value of the tunable is modified. + */ + public static Tunable stringValue(String name, String defaultValue, Consumer onChange) { + var entry = nt.getStringTopic(name).getEntry(defaultValue); + entry.setDefault(defaultValue); + return new Tunable<>(entry, () -> entry.get(), onChange); + } + + /** + * Wraps a WPILib {@link PIDController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + */ + public static void pidController(String name, PIDController controller) { + doubleValue(name + "/kP", controller.getP(), v -> controller.setP(v)); + doubleValue(name + "/kI", controller.getI(), v -> controller.setI(v)); + doubleValue(name + "/kD", controller.getD(), v -> controller.setD(v)); + doubleValue(name + "/iZone", controller.getIZone(), v -> controller.setIZone(v)); + } + + /** + * Wraps a WPILib {@link ProfiledPIDController} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + */ + public static void pidController(String name, ProfiledPIDController controller) { + doubleValue(name + "/kP", controller.getP(), v -> controller.setP(v)); + doubleValue(name + "/kI", controller.getI(), v -> controller.setI(v)); + doubleValue(name + "/kD", controller.getD(), v -> controller.setD(v)); + doubleValue(name + "/iZone", controller.getIZone(), v -> controller.setIZone(v)); + doubleValue(name + "/maxVelocity", controller.getConstraints().maxVelocity, v -> + controller.setConstraints(new TrapezoidProfile.Constraints(v, controller.getConstraints().maxAcceleration)) + ); + doubleValue(name + "/maxAcceleration", controller.getConstraints().maxAcceleration, v -> + controller.setConstraints(new TrapezoidProfile.Constraints(controller.getConstraints().maxVelocity, v)) + ); + } + + /** + * Wraps a {@link SparkPIDController REV Spark PID Controller} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + */ + public static void pidController(String name, SparkPIDController controller) { + pidController(name, controller, 0); + } + + /** + * Wraps a {@link SparkPIDController REV Spark PID Controller} to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + * @param slotId The slot of the PID controller to use. + */ + public static void pidController(String name, SparkPIDController controller, int slotId) { + doubleValue(name + "/kP", controller.getP(slotId), v -> controller.setP(v, slotId)); + doubleValue(name + "/kI", controller.getI(slotId), v -> controller.setI(v, slotId)); + doubleValue(name + "/kD", controller.getD(slotId), v -> controller.setD(v, slotId)); + doubleValue(name + "/iZone", controller.getIZone(slotId), v -> controller.setIZone(v, slotId)); + doubleValue(name + "/kFF", controller.getFF(slotId), v -> controller.setFF(slotId)); + } + + /** + * Wraps a {@link TalonFX} PID controller to be tunable. + * @param name The name for the tunable. Must be unique. + * @param controller The PID controller. + */ + public static void pidController(String name, TalonFX controller) { + Slot0Configs config = new Slot0Configs(); + controller.getConfigurator().refresh(config); + + doubleValue(name + "/kP", config.kP, v -> { + config.kP = v; + controller.getConfigurator().apply(config); + }); + doubleValue(name + "/kI", config.kI, v -> { + config.kI = v; + controller.getConfigurator().apply(config); + }); + doubleValue(name + "/kD", config.kD, v -> { + config.kD = v; + controller.getConfigurator().apply(config); + }); + doubleValue(name + "/kS", config.kS, v -> { + config.kS = v; + controller.getConfigurator().apply(config); + }); + doubleValue(name + "/kV", config.kV, v -> { + config.kV = v; + controller.getConfigurator().apply(config); + }); + doubleValue(name + "/kA", config.kA, v -> { + config.kA = v; + controller.getConfigurator().apply(config); + }); + doubleValue(name + "/kG", config.kG, v -> { + config.kG = v; + controller.getConfigurator().apply(config); + }); + } +} diff --git a/src/main/java/org/team340/lib/logging/ADIS16470Logger.java b/src/main/java/org/team340/lib/logging/ADIS16470Logger.java new file mode 100644 index 0000000..3906029 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/ADIS16470Logger.java @@ -0,0 +1,28 @@ +package org.team340.lib.logging; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; + +@CustomLoggerFor(ADIS16470_IMU.class) +public class ADIS16470Logger extends ClassSpecificLogger { + + public ADIS16470Logger() { + super(ADIS16470_IMU.class); + } + + @Override + public void update(DataLogger logger, ADIS16470_IMU adis16470) { + logger.log("accelerationX", adis16470.getAccelX()); + logger.log("accelerationY", adis16470.getAccelY()); + logger.log("accelerationZ", adis16470.getAccelZ()); + logger.log("velocityX", adis16470.getRate(IMUAxis.kX)); + logger.log("velocityY", adis16470.getRate(IMUAxis.kY)); + logger.log("velocityZ", adis16470.getRate(IMUAxis.kZ)); + logger.log("yaw", adis16470.getAngle(IMUAxis.kYaw)); + logger.log("pitch", adis16470.getAngle(IMUAxis.kPitch)); + logger.log("roll", adis16470.getAngle(IMUAxis.kRoll)); + } +} diff --git a/src/main/java/org/team340/lib/logging/CANcoderLogger.java b/src/main/java/org/team340/lib/logging/CANcoderLogger.java new file mode 100644 index 0000000..aa99a05 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/CANcoderLogger.java @@ -0,0 +1,22 @@ +package org.team340.lib.logging; + +import com.ctre.phoenix6.hardware.CANcoder; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(CANcoder.class) +public class CANcoderLogger extends ClassSpecificLogger { + + public CANcoderLogger() { + super(CANcoder.class); + } + + @Override + public void update(DataLogger logger, CANcoder canCoder) { + logger.log("absolutePosition", canCoder.getAbsolutePosition().getValue()); + logger.log("magnetHealth", canCoder.getMagnetHealth().getValue().name()); + logger.log("position", canCoder.getPosition().getValue()); + logger.log("velocity", canCoder.getVelocity().getValue()); + } +} diff --git a/src/main/java/org/team340/lib/logging/DigitalInputLogger.java b/src/main/java/org/team340/lib/logging/DigitalInputLogger.java new file mode 100644 index 0000000..2a09395 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/DigitalInputLogger.java @@ -0,0 +1,19 @@ +package org.team340.lib.logging; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.wpilibj.DigitalInput; + +@CustomLoggerFor(DigitalInput.class) +public class DigitalInputLogger extends ClassSpecificLogger { + + public DigitalInputLogger() { + super(DigitalInput.class); + } + + @Override + public void update(DataLogger logger, DigitalInput digitalInput) { + logger.log("value", digitalInput.get()); + } +} diff --git a/src/main/java/org/team340/lib/logging/EncoderLogger.java b/src/main/java/org/team340/lib/logging/EncoderLogger.java new file mode 100644 index 0000000..cf39b73 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/EncoderLogger.java @@ -0,0 +1,20 @@ +package org.team340.lib.logging; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.wpilibj.Encoder; + +@CustomLoggerFor(Encoder.class) +public class EncoderLogger extends ClassSpecificLogger { + + public EncoderLogger() { + super(Encoder.class); + } + + @Override + public void update(DataLogger logger, Encoder encoder) { + logger.log("distance", encoder.getDistance()); + logger.log("rate", encoder.getRate()); + } +} diff --git a/src/main/java/org/team340/lib/logging/Pigeon2Logger.java b/src/main/java/org/team340/lib/logging/Pigeon2Logger.java new file mode 100644 index 0000000..80b7bab --- /dev/null +++ b/src/main/java/org/team340/lib/logging/Pigeon2Logger.java @@ -0,0 +1,27 @@ +package org.team340.lib.logging; + +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(Pigeon2.class) +public class Pigeon2Logger extends ClassSpecificLogger { + + public Pigeon2Logger() { + super(Pigeon2.class); + } + + @Override + public void update(DataLogger logger, Pigeon2 pigeon2) { + logger.log("accelerationX", pigeon2.getAccelerationX().getValue()); + logger.log("accelerationY", pigeon2.getAccelerationY().getValue()); + logger.log("accelerationZ", pigeon2.getAccelerationZ().getValue()); + logger.log("velocityX", pigeon2.getAngularVelocityXWorld().getValue()); + logger.log("velocityY", pigeon2.getAngularVelocityYWorld().getValue()); + logger.log("velocityZ", pigeon2.getAngularVelocityZWorld().getValue()); + logger.log("yaw", pigeon2.getYaw().getValue()); + logger.log("pitch", pigeon2.getPitch().getValue()); + logger.log("roll", pigeon2.getRoll().getValue()); + } +} diff --git a/src/main/java/org/team340/lib/logging/RelativeEncoderLogger.java b/src/main/java/org/team340/lib/logging/RelativeEncoderLogger.java new file mode 100644 index 0000000..9e40987 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/RelativeEncoderLogger.java @@ -0,0 +1,20 @@ +package org.team340.lib.logging; + +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(RelativeEncoder.class) +public class RelativeEncoderLogger extends ClassSpecificLogger { + + public RelativeEncoderLogger() { + super(RelativeEncoder.class); + } + + @Override + public void update(DataLogger logger, RelativeEncoder relativeEncoder) { + logger.log("position", relativeEncoder.getPosition()); + logger.log("velocity", relativeEncoder.getVelocity()); + } +} diff --git a/src/main/java/org/team340/lib/logging/SparkAbsoluteEncoderLogger.java b/src/main/java/org/team340/lib/logging/SparkAbsoluteEncoderLogger.java new file mode 100644 index 0000000..21a395c --- /dev/null +++ b/src/main/java/org/team340/lib/logging/SparkAbsoluteEncoderLogger.java @@ -0,0 +1,20 @@ +package org.team340.lib.logging; + +import com.revrobotics.SparkAbsoluteEncoder; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(SparkAbsoluteEncoder.class) +public class SparkAbsoluteEncoderLogger extends ClassSpecificLogger { + + public SparkAbsoluteEncoderLogger() { + super(SparkAbsoluteEncoder.class); + } + + @Override + public void update(DataLogger logger, SparkAbsoluteEncoder absoluteEncoder) { + logger.log("position", absoluteEncoder.getPosition()); + logger.log("velocity", absoluteEncoder.getVelocity()); + } +} diff --git a/src/main/java/org/team340/lib/logging/SparkFlexLogger.java b/src/main/java/org/team340/lib/logging/SparkFlexLogger.java new file mode 100644 index 0000000..1ff7954 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/SparkFlexLogger.java @@ -0,0 +1,24 @@ +package org.team340.lib.logging; + +import com.revrobotics.CANSparkFlex; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(CANSparkFlex.class) +public class SparkFlexLogger extends ClassSpecificLogger { + + public SparkFlexLogger() { + super(CANSparkFlex.class); + } + + @Override + public void update(DataLogger logger, CANSparkFlex sparkFlex) { + logger.log("appliedOutput", sparkFlex.getAppliedOutput()); + logger.log("busVoltage", sparkFlex.getBusVoltage()); + logger.log("motorTemperature", sparkFlex.getMotorTemperature()); + logger.log("outputCurrent", sparkFlex.getOutputCurrent()); + logger.log("position", sparkFlex.getEncoder().getPosition()); + logger.log("velocity", sparkFlex.getEncoder().getVelocity()); + } +} diff --git a/src/main/java/org/team340/lib/logging/SparkLimitSwitchLogger.java b/src/main/java/org/team340/lib/logging/SparkLimitSwitchLogger.java new file mode 100644 index 0000000..4182045 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/SparkLimitSwitchLogger.java @@ -0,0 +1,19 @@ +package org.team340.lib.logging; + +import com.revrobotics.SparkLimitSwitch; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(SparkLimitSwitch.class) +public class SparkLimitSwitchLogger extends ClassSpecificLogger { + + public SparkLimitSwitchLogger() { + super(SparkLimitSwitch.class); + } + + @Override + public void update(DataLogger logger, SparkLimitSwitch limitSwitch) { + logger.log("isPressed", limitSwitch.isPressed()); + } +} diff --git a/src/main/java/org/team340/lib/logging/SparkMaxLogger.java b/src/main/java/org/team340/lib/logging/SparkMaxLogger.java new file mode 100644 index 0000000..2973da6 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/SparkMaxLogger.java @@ -0,0 +1,24 @@ +package org.team340.lib.logging; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(CANSparkMax.class) +public class SparkMaxLogger extends ClassSpecificLogger { + + public SparkMaxLogger() { + super(CANSparkMax.class); + } + + @Override + public void update(DataLogger logger, CANSparkMax sparkMax) { + logger.log("appliedOutput", sparkMax.getAppliedOutput()); + logger.log("busVoltage", sparkMax.getBusVoltage()); + logger.log("motorTemperature", sparkMax.getMotorTemperature()); + logger.log("outputCurrent", sparkMax.getOutputCurrent()); + logger.log("position", sparkMax.getEncoder().getPosition()); + logger.log("velocity", sparkMax.getEncoder().getVelocity()); + } +} diff --git a/src/main/java/org/team340/lib/logging/SparkPIDControllerLogger.java b/src/main/java/org/team340/lib/logging/SparkPIDControllerLogger.java new file mode 100644 index 0000000..ee01be7 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/SparkPIDControllerLogger.java @@ -0,0 +1,19 @@ +package org.team340.lib.logging; + +import com.revrobotics.SparkPIDController; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(SparkPIDController.class) +public class SparkPIDControllerLogger extends ClassSpecificLogger { + + public SparkPIDControllerLogger() { + super(SparkPIDController.class); + } + + @Override + public void update(DataLogger logger, SparkPIDController pidController) { + // No-op + } +} diff --git a/src/main/java/org/team340/lib/logging/SparkRelativeEncoderLogger.java b/src/main/java/org/team340/lib/logging/SparkRelativeEncoderLogger.java new file mode 100644 index 0000000..ed66bf5 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/SparkRelativeEncoderLogger.java @@ -0,0 +1,20 @@ +package org.team340.lib.logging; + +import com.revrobotics.SparkRelativeEncoder; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(SparkRelativeEncoder.class) +public class SparkRelativeEncoderLogger extends ClassSpecificLogger { + + public SparkRelativeEncoderLogger() { + super(SparkRelativeEncoder.class); + } + + @Override + public void update(DataLogger logger, SparkRelativeEncoder relativeEncoder) { + logger.log("position", relativeEncoder.getPosition()); + logger.log("velocity", relativeEncoder.getVelocity()); + } +} diff --git a/src/main/java/org/team340/lib/logging/TalonFXLogger.java b/src/main/java/org/team340/lib/logging/TalonFXLogger.java new file mode 100644 index 0000000..ed29c64 --- /dev/null +++ b/src/main/java/org/team340/lib/logging/TalonFXLogger.java @@ -0,0 +1,27 @@ +package org.team340.lib.logging; + +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; + +@CustomLoggerFor(TalonFX.class) +public class TalonFXLogger extends ClassSpecificLogger { + + public TalonFXLogger() { + super(TalonFX.class); + } + + @Override + public void update(DataLogger logger, TalonFX talonFX) { + logger.log("acceleration", talonFX.getAcceleration().getValue()); + logger.log("closedLoopError", talonFX.getClosedLoopError().getValue()); + logger.log("deviceTemp", talonFX.getDeviceTemp().getValue()); + logger.log("motorVoltage", talonFX.getMotorVoltage().getValue()); + logger.log("position", talonFX.getPosition().getValue()); + logger.log("statorCurrent", talonFX.getStatorCurrent().getValue()); + logger.log("supplyCurrent", talonFX.getSupplyCurrent().getValue()); + logger.log("supplyVoltage", talonFX.getSupplyVoltage().getValue()); + logger.log("velocity", talonFX.getVelocity().getValue()); + } +} diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java new file mode 100644 index 0000000..1d5d5ed --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -0,0 +1,581 @@ +package org.team340.lib.swerve; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Threads; +import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; +import org.team340.lib.swerve.config.SwerveConfig; +import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU; +import org.team340.lib.util.Alliance; +import org.team340.lib.util.Math2; +import org.team340.lib.util.Sleep; +import org.team340.robot.Robot; + +public class SwerveAPI implements AutoCloseable { + + public final SwerveState state; + + final SwerveIMU imu; + final SwerveModule[] modules; + final SwerveConfig config; + + private final int moduleCount; + private final double farthestModule; + private final Translation2d[] moduleLocations; + private final SwerveModuleState[] lockedStates; + + private final SwerveDriveKinematics kinematics; + private final SwerveDrivePoseEstimator poseEstimator; + + private final Lock odometryMutex = new ReentrantLock(); + private final SwerveOdometryThread odometryThread; + + private Rotation2d lastRobotAngle = Math2.kZeroRotation2d; + private double lastRatelimit = 0.0; + + private Consumer imuSimHook = s -> {}; + + public SwerveAPI(SwerveConfig config) { + this.config = config; + + moduleCount = config.modules.length; + modules = new SwerveModule[moduleCount]; + moduleLocations = new Translation2d[moduleCount]; + lockedStates = new SwerveModuleState[moduleCount]; + double farthest = 0.0; + for (int i = 0; i < moduleCount; i++) { + var moduleConfig = config.modules[i]; + modules[i] = new SwerveModule(config, moduleConfig); + moduleLocations[i] = moduleConfig.location; + lockedStates[i] = new SwerveModuleState(0.0, moduleLocations[i].getAngle()); + farthest = Math.max(farthest, moduleLocations[i].getNorm()); + } + + farthestModule = farthest; + + state = new SwerveState(modules); + kinematics = new SwerveDriveKinematics(moduleLocations); + poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + Math2.kZeroRotation2d, + state.modules.positions, + Math2.kZeroPose2d, + config.odometryStdDevs, + VecBuilder.fill(0.0, 0.0, 0.0) + ); + + imu = SwerveIMU.construct(config.imu, config, hook -> imuSimHook = hook); + odometryThread = new SwerveOdometryThread(); + } + + /** + * Refreshes inputs from all swerve hardware. This must be called periodically + * in order for the API to function. Typically, this method is called at the + * start of the swerve subsystem's {@code periodic()} method. + */ + public void refresh() { + odometryMutex.lock(); + try { + odometryThread.run(true); + state.odometry.timesync = odometryThread.timesync; + state.odometry.successes = odometryThread.successes; + state.odometry.failures = odometryThread.failures; + + odometryThread.successes = 0; + odometryThread.failures = 0; + + for (int i = 0; i < moduleCount; i++) { + Math2.copyInto(modules[i].getPosition(), state.modules.positions[i]); + Math2.copyInto(modules[i].getState(), state.modules.states[i]); + } + + state.pose = poseEstimator.getEstimatedPosition(); + } finally { + odometryMutex.unlock(); + } + + state.pitch = imu.getPitch(); + state.roll = imu.getRoll(); + state.speeds = kinematics.toChassisSpeeds(state.modules.states); + state.velocity = Math.hypot(state.speeds.vxMetersPerSecond, state.speeds.vyMetersPerSecond); + + imuSimHook.accept(state.speeds); + } + + /** + * Tares the rotation of the robot. Useful for fixing an out of sync or drifting IMU. In + * most cases, a forward perspective of {@link ForwardPerspective#OPERATOR OPERATOR} is + * desirable. {@link ForwardPerspective#ROBOT ROBOT} will no-op. + * @param forwardPerspective The perspective to tare the rotation to. + */ + public void tareRotation(ForwardPerspective forwardPerspective) { + if (forwardPerspective.equals(ForwardPerspective.ROBOT)) return; + var rotation = forwardPerspective.getTareRotation(); + resetPose(new Pose2d(state.pose.getTranslation(), rotation)); + } + + /** + * Resets the pose of the robot, inherently seeding field-relative movement. + * Additionally, odometry and vision measurement history is flushed. This + * method is typically invoked at the start of a match to set the robot's + * position to the starting location of an autonomous mode. The supplied + * pose is expected to be blue origin relative. + * @param pose The new blue origin relative pose to apply to the pose estimator. + */ + public void resetPose(Pose2d pose) { + odometryMutex.lock(); + try { + poseEstimator.resetPosition(odometryThread.lastYaw, state.modules.positions, pose); + state.pose = poseEstimator.getEstimatedPosition(); + } finally { + odometryMutex.unlock(); + } + } + + /** + * Adds a vision measurement to the pose estimator. + * @see {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. + * @param visionPose The pose of the robot as measured by the vision camera. + * @param timestamp The timestamp of the vision measurement in seconds. + * @param stdDevs Standard deviations of the vision pose measurement (x position in meters, y position in meters, and yaw in radians). + */ + public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix stdDevs) { + odometryMutex.lock(); + try { + poseEstimator.addVisionMeasurement(visionPose, timestamp, stdDevs); + state.pose = poseEstimator.getEstimatedPosition(); + } finally { + odometryMutex.unlock(); + } + } + + /** + * Drives using inputs from the driver's controller. The {@code x} and {@code y} parameters + * expect the controller's NED (north-east-down) convention, and will automatically convert + * to WPILib's typical NWU (north-west-up) convention when applying chassis speeds. + * @param x The X value of the driver's joystick, from {@code [-1.0, 1.0]}. + * @param y The Y value of the driver's joystick, from {@code [-1.0, 1.0]}. + * @param angular The CCW+ angular speed to apply, from {@code [-1.0, 1.0]}. + * @param forwardPerspective The forward perspective for the chassis speeds. + * @param discretize If the generated speeds should be discretized. + * @param ratelimit If the robot's acceleration should be constrained. + */ + public void applyDriverInput( + double x, + double y, + double angular, + ForwardPerspective forwardPerspective, + boolean discretize, + boolean ratelimit + ) { + double angularVel = + config.driverAngularVel * Math.copySign(Math.pow(angular, config.driverAngularVelExp), angular); + applyDriverXY(x, y, angularVel, forwardPerspective, discretize, ratelimit); + } + + /** + * Drives using inputs from the driver's controller, with a specified angular velocity in radians/second. + * Use this method as opposed to {@link SwerveAPI#applyDriverInput(double, double, double)} if the driver's + * input is desired only for x/y movement, and not heading. Use cases include locking the robot's heading by + * passing the output of a PID controller as the angular velocity. The {@code x} and {@code y} parameters + * expect the controller's NED (north-east-down) convention, and will automatically convert to WPILib's + * typical NWU (north-west-up) convention when applying chassis speeds. + * @param x The X value of the driver's joystick, from {@code [-1.0, 1.0]}. + * @param y The Y value of the driver's joystick, from {@code [-1.0, 1.0]}. + * @param angularVel The CCW+ angular velocity to apply, in radians/second. + * @param forwardPerspective The forward perspective for the chassis speeds. + * @param discretize If the generated speeds should be discretized. + * @param ratelimit If the robot's acceleration should be constrained. + */ + public void applyDriverXY( + double x, + double y, + double angularVel, + ForwardPerspective forwardPerspective, + boolean discretize, + boolean ratelimit + ) { + double norm = Math.hypot(x, y); + if (norm > 1.0) { + x /= norm; + y /= norm; + } + double xyMult = config.driverVel * Math.pow(Math.hypot(x, y), config.driverVelExp - 1.0); + applySpeeds(new ChassisSpeeds(-y * xyMult, -x * xyMult, angularVel), forwardPerspective, discretize, ratelimit); + } + + /** + * Drives using chassis speeds. + * @param speeds The chassis speeds to apply. Note that the provided {@link ChassisSpeeds} object may be mutated. + * @param forwardPerspective The forward perspective for the chassis speeds. + * @param discretize If the speeds should be discretized. + * @param ratelimit If the robot's acceleration should be constrained. + */ + public void applySpeeds( + ChassisSpeeds speeds, + ForwardPerspective forwardPerspective, + boolean discretize, + boolean ratelimit + ) { + double w_max = config.velocity / farthestModule; + if (Math.abs(speeds.omegaRadiansPerSecond) >= w_max) { + speeds.vxMetersPerSecond = 0.0; + speeds.vyMetersPerSecond = 0.0; + speeds.omegaRadiansPerSecond = Math.copySign(w_max, speeds.omegaRadiansPerSecond); + } else { + double vx = speeds.vxMetersPerSecond; + double vy = speeds.vyMetersPerSecond; + double w = speeds.omegaRadiansPerSecond; + + double k = 1.0; + double v_max2 = config.velocity * config.velocity; + + for (var r : moduleLocations) { + double vx_w = -w * r.getY(); + double vy_w = w * r.getX(); + + double vx_m = (vx + vx_w); + double vy_m = (vy + vy_w); + + if ((vx_m * vx_m + vy_m * vy_m) > v_max2) { + double a = vx * vx + vy * vy; + double b = 2 * vx * vx_w + 2 * vy * vy_w; + double c = vx_w * vx_w + vy_w * vy_w - v_max2; + k = Math.min(k, (2 * c) / (-b - Math.sqrt(b * b - 4 * a * c))); + } + } + + speeds.vxMetersPerSecond *= k; + speeds.vyMetersPerSecond *= k; + } + + if (ratelimit) { + double now = Timer.getFPGATimestamp(); + if (now - lastRatelimit > config.period * 2.0) { + Math2.copyInto(state.speeds, state.targetSpeeds); + } + lastRatelimit = now; + + forwardPerspective.toPerspectiveSpeeds(state.targetSpeeds, lastRobotAngle); + + double vx_l = state.targetSpeeds.vxMetersPerSecond; + double vy_l = state.targetSpeeds.vyMetersPerSecond; + double v_l = Math.hypot(vx_l, vy_l); + double w_l = state.targetSpeeds.omegaRadiansPerSecond; + + double dx = speeds.vxMetersPerSecond - vx_l; + double dy = speeds.vyMetersPerSecond - vy_l; + double a_slip = config.slipAccel * config.period; + if (dx * dx + dy * dy > a_slip * a_slip) { + double s = a_slip / Math.hypot(dx, dy); + speeds.vxMetersPerSecond = vx_l + (s * dx); + speeds.vyMetersPerSecond = vy_l + (s * dy); + } + + double v = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + double a_torque = (config.torqueAccel * config.period) * (1.0 - (v_l / config.velocity)); + if (v - v_l > a_torque) { + double s = (v_l + a_torque) / v; + speeds.vxMetersPerSecond *= s; + speeds.vyMetersPerSecond *= s; + } + + double dw = speeds.omegaRadiansPerSecond - w_l; + double a_angular = config.angularAccel * config.period; + speeds.omegaRadiansPerSecond = w_l + ((Math.abs(dw) > a_angular ? a_angular / Math.abs(dw) : 1.0) * dw); + } + + if (discretize) ChassisSpeeds.discretize(speeds, config.discretizationPeriod); + + lastRobotAngle = state.pose.getRotation(); + forwardPerspective.toRobotSpeeds(speeds, lastRobotAngle); + Math2.copyInto(speeds, state.targetSpeeds); + + applyStates(kinematics.toSwerveModuleStates(speeds)); + } + + /** + * Drives the modules to an X formation to stop the robot from moving. + */ + public void applyLockedWheels() { + applyStates(lockedStates); + } + + /** + * Drives using module states. + * @param states The states to apply. + */ + public void applyStates(SwerveModuleState[] states) { + if (moduleCount != states.length) { + throw new IllegalArgumentException( + "Requested " + states.length + " states be applied to " + moduleCount + " modules" + ); + } + + for (int i = 0; i < moduleCount; i++) { + modules[i].applyState(states[i]); + } + } + + /** + * Drives the robot using open-loop voltage. Intended for characterization. + * Plumbing for recording device voltage via their Java API is intentionally + * unavailable, as GC pressure and CAN latency will result in inaccurate data. + * Use Phoenix Signal Logging or URCL instead. + * @param voltage The voltage to apply to the move motors. + * @param angle The robot-relative angle to apply to the turn motors. + */ + public void applyVoltage(double voltage, Rotation2d angle) { + for (int i = 0; i < moduleCount; i++) { + modules[i].applyVoltage(voltage, angle); + } + } + + /** + * Enables publishing tunables for adjustment of the API's constants. + * @param name The parent name for the tunables in NetworkTables. + */ + public void enableTunables(String name) { + SwerveTunables.initialize(name, this); + } + + @Override + public void close() { + try { + odometryThread.close(); + for (var module : modules) module.close(); + imu.close(); + } catch (Exception e) {} + } + + /** + * Specifies the X+ direction of chassis speeds. + */ + public static enum ForwardPerspective { + // TODO Remove copyInto, mutate in place: blocked by upstream PR https://github.com/wpilibsuite/allwpilib/pull/7115 + + /** + * The speeds are relative to the operator's perspective. If the robot + * is on the blue alliance, X+ drives towards the red alliance. If the + * robot is on the red alliance, X+ drives towards the blue alliance. + */ + OPERATOR { + @Override + void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { + (Alliance.isBlue() ? BLUE_ALLIANCE : RED_ALLIANCE).toRobotSpeeds(speeds, robotAngle); + } + + @Override + void toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { + (Alliance.isBlue() ? BLUE_ALLIANCE : RED_ALLIANCE).toPerspectiveSpeeds(speeds, robotAngle); + } + + @Override + Rotation2d getTareRotation() { + return (Alliance.isBlue() ? BLUE_ALLIANCE : RED_ALLIANCE).getTareRotation(); + } + }, + + /** + * The speeds are relative to the blue alliance perspective. + * X+ drives towards the red alliance. + */ + BLUE_ALLIANCE { + @Override + void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { + var newSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotAngle); + Math2.copyInto(newSpeeds, speeds); + } + + @Override + void toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { + var newSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, robotAngle); + Math2.copyInto(newSpeeds, speeds); + } + + @Override + Rotation2d getTareRotation() { + return Math2.kZeroRotation2d; + } + }, + + /** + * The speeds are relative to the red alliance perspective. + * X+ drives towards the blue alliance. + */ + RED_ALLIANCE { + @Override + void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { + var newSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotAngle.rotateBy(Math2.kPiRotation2d)); + Math2.copyInto(newSpeeds, speeds); + } + + @Override + void toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) { + var newSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, robotAngle.rotateBy(Math2.kPiRotation2d)); + Math2.copyInto(newSpeeds, speeds); + } + + @Override + Rotation2d getTareRotation() { + return Math2.kPiRotation2d; + } + }, + + /** + * The speeds are relative to the robot's perspective. + * X+ drives forwards relative to the chassis. + */ + ROBOT { + @Override + void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) {} + + @Override + void toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle) {} + + @Override + Rotation2d getTareRotation() { + // Will no-op downstream. + return null; + } + }; + + /** + * Converts perspective relative speeds to robot relative speeds. + * @param speeds The perspective relative speeds to convert. + * @param robotAngle The blue origin relative angle of the robot. + */ + abstract void toRobotSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle); + + /** + * Converts robot relative speeds to the perspective relative speeds. + * @param speeds The robot relative speeds to convert. + * @param robotAngle The blue origin relative angle of the robot. + */ + abstract void toPerspectiveSpeeds(ChassisSpeeds speeds, Rotation2d robotAngle); + + /** + * Gets the rotation to apply as the new zero when + * taring the robot's rotation to the perspective. + */ + abstract Rotation2d getTareRotation(); + } + + /** + * Manages swerve odometry. Will run asynchronously at the configured odometry update + * period, unless the configured period is the same or more than the main robot loop + * period. The {@link SwerveOdometryThread#run(boolean)} method is also invoked in + * {@link SwerveAPI#refresh()}, to ensure the latest measurements are applied to + * pose estimation when executing user logic in the main loop. + */ + private final class SwerveOdometryThread implements AutoCloseable { + + public Rotation2d lastYaw = Math2.kZeroRotation2d; + public boolean timesync = false; + public int successes = 0; + public int failures = 0; + + private final SwerveModulePosition[] positionCache; + private final BaseStatusSignal[] signals; + private final Thread thread; + + private volatile boolean active = false; + private double lastTime = 0.0; + + public SwerveOdometryThread() { + List signalList = new ArrayList<>(); + signalList.addAll(imu.getSignals()); + for (var module : modules) signalList.addAll(module.getSignals()); + signals = signalList.stream().toArray(BaseStatusSignal[]::new); + positionCache = new SwerveModulePosition[moduleCount]; + for (int i = 0; i < moduleCount; i++) { + positionCache[i] = modules[i].getPosition(); + } + + if (config.odometryPeriod < config.period) { + thread = new Thread(() -> { + Threads.setCurrentThreadPriority(true, 1); + while (active) this.run(false); + }); + thread.setName("SwerveAPI"); + thread.setDaemon(true); + timesync = config.phoenixPro && CANBus.isNetworkFD(config.phoenixCanBus); + active = true; + thread.start(); + } else { + thread = null; + } + } + + /** + * Runs an odometry update. + * @param sync If the update is being invoked from the main loop. + */ + public void run(boolean sync) { + StatusCode phoenixStatus = StatusCode.OK; + if (!sync) { + if (timesync) { + phoenixStatus = BaseStatusSignal.waitForAll(config.odometryPeriod * 2.0, signals); + } else { + Sleep.seconds(Math.max(0.0, config.odometryPeriod - (Timer.getFPGATimestamp() - lastTime))); + lastTime = Timer.getFPGATimestamp(); + } + } + + odometryMutex.lock(); + try { + if (!timesync && signals.length > 0) phoenixStatus = BaseStatusSignal.refreshAll(signals); + + lastYaw = imu.getYaw(); + + boolean readError = !phoenixStatus.isOK() || imu.readError(); + for (var module : modules) { + if (!module.refresh()) readError = true; + } + + if (readError) { + failures++; + if (!Robot.isSimulation()) return; + } + + poseEstimator.update(lastYaw, positionCache); + successes++; + } finally { + odometryMutex.unlock(); + } + } + + @Override + public void close() { + if (active) { + active = false; + if (thread != null && thread.isAlive()) { + try { + thread.interrupt(); + thread.join(); + } catch (Exception e) { + Thread.currentThread().interrupt(); + } + } + } + } + } +} diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java new file mode 100644 index 0000000..c55f5ce --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java @@ -0,0 +1,55 @@ +package org.team340.lib.swerve; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +@CustomLoggerFor(SwerveAPI.class) +public class SwerveAPILogger extends ClassSpecificLogger { + + public SwerveAPILogger() { + super(SwerveAPI.class); + } + + @Override + public void update(DataLogger logger, SwerveAPI swerveAPI) { + logState(logger.getSubLogger("state"), swerveAPI.state); + var hardwareLogger = logger.getSubLogger("hardware"); + + ErrorHandler errorHandler = Epilogue.getConfig().errorHandler; + swerveAPI.imu.log(hardwareLogger.getSubLogger("imu"), errorHandler); + for (var module : swerveAPI.modules) { + var moduleLogger = hardwareLogger.getSubLogger(module.getName()); + module.moveMotor.log(moduleLogger.getSubLogger("moveMotor"), errorHandler); + module.turnMotor.log(moduleLogger.getSubLogger("turnMotor"), errorHandler); + module.encoder.log(moduleLogger.getSubLogger("encoder"), errorHandler); + } + } + + private void logState(DataLogger logger, SwerveState state) { + logger.log("pitch", state.pitch, Rotation2d.struct); + logger.log("roll", state.roll, Rotation2d.struct); + logger.log("pose", state.pose, Pose2d.struct); + logger.log("speeds", state.speeds, ChassisSpeeds.struct); + logger.log("targetSpeeds", state.targetSpeeds, ChassisSpeeds.struct); + logger.log("velocity", state.velocity); + + var modules = logger.getSubLogger("modules"); + modules.log("positions", state.modules.positions, SwerveModulePosition.struct); + modules.log("states", state.modules.states, SwerveModuleState.struct); + modules.log("nextTarget", state.modules.nextTarget, SwerveModuleState.struct); + modules.log("lastTarget", state.modules.lastTarget, SwerveModuleState.struct); + + var odometry = logger.getSubLogger("odometry"); + odometry.log("timesync", state.odometry.timesync); + odometry.log("successes", state.odometry.successes); + odometry.log("failures", state.odometry.failures); + } +} diff --git a/src/main/java/org/team340/lib/swerve/SwerveBase.java b/src/main/java/org/team340/lib/swerve/SwerveBase.java deleted file mode 100644 index 6a45614..0000000 --- a/src/main/java/org/team340/lib/swerve/SwerveBase.java +++ /dev/null @@ -1,683 +0,0 @@ -package org.team340.lib.swerve; - -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.units.Distance; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Velocity; -import edu.wpi.first.units.Voltage; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; -import java.util.function.Consumer; -import org.team340.lib.GRRDashboard; -import org.team340.lib.GRRSubsystem; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; -import org.team340.lib.swerve.hardware.encoders.vendors.SwerveCANcoder; -import org.team340.lib.swerve.hardware.encoders.vendors.SwerveSparkEncoder; -import org.team340.lib.swerve.hardware.imu.SwerveIMU; -import org.team340.lib.swerve.hardware.imu.SwerveIMUSim; -import org.team340.lib.swerve.hardware.imu.vendors.SwerveADIS16470; -import org.team340.lib.swerve.hardware.imu.vendors.SwervePigeon2; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; -import org.team340.lib.swerve.hardware.motors.vendors.SwerveSparkFlex; -import org.team340.lib.swerve.hardware.motors.vendors.SwerveSparkMax; -import org.team340.lib.swerve.hardware.motors.vendors.SwerveTalonFX; -import org.team340.lib.swerve.util.SwerveConversions; -import org.team340.lib.swerve.util.SwerveOdometryThread; -import org.team340.lib.swerve.util.SwerveRatelimiter; -import org.team340.lib.swerve.util.SwerveVisualizer; -import org.team340.lib.util.Math2; -import org.team340.lib.util.SendableFactory; -import org.team340.lib.util.StringUtil; - -/** - * A general implementation of swerve drive. - * This class consists of protected members that must be exposed in a subclassed implementation. - * - * Supported motors: - *
      - *
    • Brushed Spark Max
      • - *
    • Brushless Spark Max
      • - *
    • Brushed Spark Flex
      • - *
    • Brushless Spark Flex
      • - *
    • Talon FX
      • - *
    - * - * Supported encoders: - *
      - *
    • CANcoder
      • - *
    • Spark Attached Encoders
      • - *
    - * - * Supported IMUs: - *
      - *
    • ADIS16470
      • - *
    • Pigeon 2
      • - *
    - */ -public abstract class SwerveBase extends GRRSubsystem { - - protected final SwerveConfig config; - protected final SwerveConversions conversions; - protected final SwerveIMU imu; - protected final SwerveDriveKinematics kinematics; - protected final SwerveModule[] modules; - protected final SwerveDrivePoseEstimator poseEstimator; - protected final SysIdRoutine sysIdRoutine; - protected final SwerveVisualizer visualizer; - - private final Translation2d[] moduleTranslations; - private final SwerveOdometryThread odometryThread; - private final SwerveRatelimiter ratelimiter; - - private final MutableMeasure sysIdAppliedVoltage = mutable(Volts.of(0)); - private final MutableMeasure sysIdDistance = mutable(Meters.of(0)); - private final MutableMeasure> sysIdVelocity = mutable(MetersPerSecond.of(0)); - - /** - * Create the swerve subsystem. - * @param label The label for the subsystem. Shown in the dashboard. - * @param config Swerve config, use {@link SwerveConfig} as a builder for generating configs. - */ - public SwerveBase(String label, SwerveConfig config) { - super(label); - config.verify(); - this.config = config; - - imu = createIMU(); - - modules = new SwerveModule[config.getModules().size()]; - moduleTranslations = new Translation2d[modules.length]; - for (int i = 0; i < config.getModules().size(); i++) { - SwerveModuleConfig moduleConfig = config.getModules().get(i); - modules[i] = createModule(moduleConfig); - moduleTranslations[i] = moduleConfig.getPosition(); - } - - conversions = new SwerveConversions(config); - kinematics = new SwerveDriveKinematics(moduleTranslations); - odometryThread = new SwerveOdometryThread(modules, imu, config); - ratelimiter = new SwerveRatelimiter(config, kinematics, getModuleStates()); - visualizer = new SwerveVisualizer(this::getPosition, this::getModuleStates, this::getDesiredModuleStates); - - double[] odometryStd = config.getOdometryStd(); - double[] visionStd = config.getVisionStd(); - poseEstimator = - new SwerveDrivePoseEstimator( - kinematics, - imu.getYaw(), - getModulePositions(), - Math2.POSE2D_0, - VecBuilder.fill(odometryStd[0], odometryStd[1], odometryStd[2]), - VecBuilder.fill(visionStd[0], visionStd[1], visionStd[2]) - ); - - sysIdRoutine = - new SysIdRoutine( - config.getSysIdConfig(), - new Mechanism( - (Measure volts) -> { - driveVoltage(volts.in(Volts), Math2.ROTATION2D_0); - }, - log -> { - for (SwerveModule module : modules) { - log - .motor("module-" + StringUtil.toCamelCase(module.getLabel())) - .voltage( - sysIdAppliedVoltage.mut_replace(module.getMoveDutyCycle() * RobotController.getBatteryVoltage(), Volts) - ) - .linearPosition(sysIdDistance.mut_replace(module.getDistance(), Meters)) - .linearVelocity(sysIdVelocity.mut_replace(module.getVelocity(), MetersPerSecond)); - } - }, - this, - "Swerve" - ) - ); - - imu.setZero(Math2.ROTATION2D_0); - - System.out.println( - "\nSwerve Conversions:" + - "\n\tModule Count: " + - modules.length + - "\n\tMove Rotations/Meter: " + - Math2.toFixed(conversions.moveRotationsPerMeter()) + - "\n\tTurn Rotations/Radian: " + - Math2.toFixed(conversions.turnRotationsPerRadian()) - ); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - - builder.addDoubleProperty("velocityX", () -> getVelocity(true).vxMetersPerSecond, null); - builder.addDoubleProperty("velocityY", () -> getVelocity(true).vyMetersPerSecond, null); - builder.addDoubleProperty("velocityRot", () -> getVelocity(true).omegaRadiansPerSecond, null); - builder.addDoubleProperty( - "velocityNorm", - () -> { - ChassisSpeeds speeds = getVelocity(true); - return Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - }, - null - ); - - builder.addDoubleProperty("odometryX", () -> getPosition().getX(), null); - builder.addDoubleProperty("odometryY", () -> getPosition().getY(), null); - builder.addDoubleProperty("odometryRot", () -> getPosition().getRotation().getRadians(), null); - - builder.addIntegerProperty("readErrors", odometryThread::readErrorCount, null); - - for (SwerveModule module : modules) { - GRRDashboard.addSubsystemSendable( - "Modules/" + StringUtil.toPascalCase(module.getLabel()), - this, - SendableFactory.create(moduleBuilder -> { - moduleBuilder.publishConstString(".label", module.getLabel()); - moduleBuilder.addDoubleProperty("velocity", module::getVelocity, null); - moduleBuilder.addDoubleProperty("distance", module::getDistance, null); - moduleBuilder.addDoubleProperty("heading", module::getHeading, null); - moduleBuilder.addDoubleProperty("desiredVelocity", () -> module.getDesiredState().speedMetersPerSecond, null); - moduleBuilder.addDoubleProperty("desiredHeading", () -> module.getDesiredState().angle.getRadians(), null); - }) - ); - } - - GRRDashboard.addSubsystemSendable("Visualizations", this, visualizer); - } - - /** - * Gets the robot's velocity. - * @param fieldRelative If the returned velocity should be field relative. - */ - protected ChassisSpeeds getVelocity(boolean fieldRelative) { - if (fieldRelative) { - return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), imu.getYaw().unaryMinus()); - } else { - return kinematics.toChassisSpeeds(getModuleStates()); - } - } - - /** - * Gets the robot's position. - */ - protected Pose2d getPosition() { - return poseEstimator.getEstimatedPosition(); - } - - /** - * Gets the desired states of all swerve modules. - */ - protected SwerveModuleState[] getDesiredModuleStates() { - SwerveModuleState[] desiredModuleStates = new SwerveModuleState[modules.length]; - for (int i = 0; i < desiredModuleStates.length; i++) desiredModuleStates[i] = modules[i].getDesiredState(); - return desiredModuleStates; - } - - /** - * Gets the states of all swerve modules. - */ - protected SwerveModuleState[] getModuleStates() { - SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; - for (int i = 0; i < moduleStates.length; i++) moduleStates[i] = modules[i].getModuleState(); - return moduleStates; - } - - /** - * Gets the positions of all swerve modules. - */ - protected SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; - for (int i = 0; i < modulePositions.length; i++) modulePositions[i] = modules[i].getModulePosition(); - return modulePositions; - } - - /** - * Gets an array of the modules' move motor distance in radians. - */ - protected double[] getModuleDistanceRad() { - return Arrays - .stream(modules) - .mapToDouble(module -> module.getDistance() * (conversions.moveRotationsPerMeter() / config.getMoveGearRatio()) * Math2.TWO_PI) - .toArray(); - } - - /** - * Resets odometry. - * @param newPose The new pose. - */ - protected void resetOdometry(Pose2d newPose) { - poseEstimator.resetPosition(newPose.getRotation(), getModulePositions(), newPose); - kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, newPose.getRotation())); - } - - /** - * Updates odometry. - * Should be ran periodically. - */ - protected void updateOdometry() { - updateOdometry(poseEstimator -> {}); - } - - /** - * Updates odometry. - * Should be ran periodically. - * @param poseEstimatorConsumer A consumer that accepts the pose estimator. Should be used for applying field-relative poses from vision data. Note that it is expected that standard deviations are specified when using {@link SwerveDrivePoseEstimator#addVisionMeasurement}, as the initial standard deviations are set to {@code 0.0}. - */ - protected void updateOdometry(Consumer poseEstimatorConsumer) { - poseEstimatorConsumer.accept(poseEstimator); - odometryThread.update(poseEstimator); - } - - /** - * Generates a trajectory using the configured constraints of the robot. - * Always starts at the robot's position, with a starting velocity of the - * robot's velocity, and an ending velocity of {@code 0.0}. - * @param points The points in the trajectory. - */ - protected Trajectory generateTrajectory(Pose2d... points) { - ChassisSpeeds speeds = getVelocity(true); - return generateTrajectory(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond), 0.0, points); - } - - /** - * Generates a trajectory using the configured constraints of the robot. - * Always starts at the robot's position. - * @param startVelocity The start velocity of the trajectory. - * @param endVelocity The end velocity of the trajectory. - * @param points The points in the trajectory. - */ - protected Trajectory generateTrajectory(double startVelocity, double endVelocity, Pose2d... points) { - TrajectoryConfig trajectoryConfig = new TrajectoryConfig(config.getTrajectoryVelocity(), config.getTrajectoryAcceleration()); - trajectoryConfig.setStartVelocity(startVelocity); - trajectoryConfig.setEndVelocity(endVelocity); - trajectoryConfig.setKinematics(kinematics); - List pointsList = new ArrayList<>(Arrays.asList(points)); - Pose2d position = getPosition(); - pointsList.add( - 0, - new Pose2d(position.getX(), position.getY(), points[0].getTranslation().minus(position.getTranslation()).getAngle()) - ); - try { - return TrajectoryGenerator.generateTrajectory(pointsList, trajectoryConfig); - } catch (Exception e) { - DriverStation.reportError(e.getMessage(), true); - return new Trajectory(); - } - } - - /** - * Drives the modules into an X formation to prevent the robot from moving. - */ - protected void lockWheels() { - SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; - for (int i = 0; i < moduleStates.length; i++) { - moduleStates[i] = new SwerveModuleState(0.0, moduleTranslations[i].getAngle()); - } - - ratelimiter.setLastState(Math2.CHASSIS_SPEEDS_0, moduleStates); - driveStates(moduleStates); - } - - /** - * Stops the modules. - */ - protected void stop() { - driveSpeeds(Math2.CHASSIS_SPEEDS_0, false, false); - } - - /** - * Drives the robot using percents of its calculated max velocity. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}. - * @param rot The desired rotational speed from {@code -1.0} to {@code 1.0}. - * @param fieldRelative If the robot should drive field relative. - */ - protected void drive(double x, double y, double rot, boolean fieldRelative) { - driveVelocity(x * config.getVelocity(), y * config.getVelocity(), rot * config.getRotationalVelocity(), fieldRelative); - } - - /** - * Drives the robot using velocity. - * @param xV The desired {@code x} velocity in meters/second. - * @param yV The desired {@code y} velocity in meters/second. - * @param rotV The desired rotational velocity in radians/second. - * @param fieldRelative If the robot should drive field relative. - */ - protected void driveVelocity(double xV, double yV, double rotV, boolean fieldRelative) { - ChassisSpeeds chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xV, yV, rotV, imu.getYaw()) - : new ChassisSpeeds(xV, yV, rotV); - - driveSpeeds(chassisSpeeds); - } - - /** - * Drives the robot using percents of its calculated max velocity while locked at a field relative angle. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}. - * @param angle The desired field relative angle to point at in radians. - * @param controller A profiled PID controller to use for translating to and maintaining the angle. - * @param useIMU If the IMU should be used for determining the robot's angle. If {@code false}, the pose estimator is used. - */ - protected void driveAngle(double x, double y, double angle, ProfiledPIDController controller, boolean useIMU) { - driveAngleVelocity(x * config.getVelocity(), y * config.getVelocity(), angle, controller, useIMU); - } - - /** - * Drives the robot using velocity while locked at a field relative angle. - * @param xV The desired {@code x} velocity in meters/second. - * @param yV The desired {@code y} velocity in meters/second. - * @param angle The desired field relative angle to point at in radians. - * @param controller A profiled PID controller to use for translating to and maintaining the angle. - * @param useIMU If the IMU should be used for determining the robot's angle. If {@code false}, the pose estimator is used. - */ - protected void driveAngleVelocity(double xV, double yV, double angle, ProfiledPIDController controller, boolean useIMU) { - Rotation2d yaw = useIMU ? imu.getYaw() : getPosition().getRotation(); - ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - xV, - yV, - controller.calculate(MathUtil.angleModulus(yaw.getRadians()), angle), - yaw - ); - - driveSpeeds(chassisSpeeds); - } - - /** - * Drives the robot to a field relative pose. - * @param pose The pose to drive to. - * @param xController A PID controller to use for translating to and maintaining pose's {@code x} position. - * @param yController The PID controller to use for translating to and maintaining pose's {@code y} position. - * @param rotController The profiled PID controller to use for translating to and maintaining the pose's angle. - * @param useIMU If the IMU should be used for determining the robot's angle. If {@code false}, the pose estimator is used. - */ - protected void driveToPose( - Pose2d pose, - PIDController xController, - PIDController yController, - ProfiledPIDController rotController, - boolean useIMU - ) { - Rotation2d yaw = useIMU ? imu.getYaw() : getPosition().getRotation(); - Pose2d position = getPosition(); - ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - xController.calculate(position.getX(), pose.getX()), - yController.calculate(position.getY(), pose.getY()), - rotController.calculate(MathUtil.angleModulus(yaw.getRadians()), pose.getRotation().getRadians()), - yaw - ); - - driveSpeeds(chassisSpeeds); - } - - /** - * Drives using chassis speeds. - * Speeds are discretized, then the ratelimiter calculates module states. - * @param chassisSpeeds The chassis speeds to drive with. - */ - protected void driveSpeeds(ChassisSpeeds chassisSpeeds) { - driveSpeeds(chassisSpeeds, true, true); - } - - /** - * Drives using chassis speeds. - * @param chassisSpeeds The chassis speeds to drive with. - * @param discretize If chassis speeds should be discretized. - * @param withRatelimiter If the ratelimiter should be used to calculate module states. - */ - protected void driveSpeeds(ChassisSpeeds chassisSpeeds, boolean discretize, boolean withRatelimiter) { - if (discretize) { - // Calculate how much the robot will have turned in configured lookahead number of seconds. - double dtheta = chassisSpeeds.omegaRadiansPerSecond * config.getDiscretizationLookahead(); - - // Find the coefficients of the twist experienced by the robot. - double sin = -dtheta / 2.0; - double cos = Math2.epsilonEquals(Math.cos(dtheta) - 1.0, 0.0) - ? 1.0 - ((1.0 / 12.0) * dtheta * dtheta) - : (sin * Math.sin(dtheta)) / (Math.cos(dtheta) - 1.0); - - // Find distance traveled over lookahead period. - double dt = config.getPeriod(); - double dx = chassisSpeeds.vxMetersPerSecond * dt; - double dy = chassisSpeeds.vyMetersPerSecond * dt; - - // Apply the found twist to the chassis speeds. - chassisSpeeds = - new ChassisSpeeds(((dx * cos) - (dy * sin)) / dt, ((dx * sin) + (dy * cos)) / dt, chassisSpeeds.omegaRadiansPerSecond); - } - - if (withRatelimiter) { - driveStates(ratelimiter.calculate(chassisSpeeds).moduleStates()); - } else { - SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(states, config.getVelocity()); - ratelimiter.setLastState(chassisSpeeds, states); - driveStates(states); - } - } - - /** - * Drives the robot's move motors at a specified voltage. - * Useful for feedforward characterization. - * @param voltage The voltage to apply to the move motors. - * @param heading A robot relative heading for the modules to point towards. - */ - protected void driveVoltage(double voltage, Rotation2d heading) { - for (int i = 0; i < modules.length; i++) { - modules[i].setVoltage(voltage, heading); - } - SwerveModuleState[] states = new SwerveModuleState[modules.length]; - for (int i = 0; i < states.length; i++) { - states[i] = new SwerveModuleState(0, heading); - } - ratelimiter.setLastState(new SwerveRatelimiter.SwerveState(new ChassisSpeeds(), states)); - } - - /** - * Drives using raw swerve module states. - * @param states The states to drive with. - */ - protected void driveStates(SwerveModuleState[] states) { - if (RobotBase.isSimulation()) { - ((SwerveIMUSim) imu).updateSim(kinematics.toChassisSpeeds(states)); - } - - for (int i = 0; i < states.length; i++) { - SwerveModule module = modules[i]; - if (module != null) { - module.setDesiredState(states[i]); - } - } - } - - /** - * Creates an IMU. - */ - private SwerveIMU createIMU() { - if (RobotBase.isSimulation()) return new SwerveIMUSim(); - - Object[] imuArgs = config.getImuArgs(); - - switch (config.getImuType()) { - case ADIS16470: - return new SwerveADIS16470( - createADIS16470( - "Swerve IMU", - (ADIS16470_IMU.IMUAxis) imuArgs[0], - (ADIS16470_IMU.IMUAxis) imuArgs[1], - (ADIS16470_IMU.IMUAxis) imuArgs[2], - (SPI.Port) imuArgs[3], - (ADIS16470_IMU.CalibrationTime) imuArgs[4] - ) - ); - case PIGEON2: - return new SwervePigeon2(createPigeon2("Swerve IMU", (int) imuArgs[0], (String) imuArgs[1]), config); - default: - throw new UnsupportedOperationException("Invalid IMU type"); - } - } - - /** - * Creates a swerve module. - * @param moduleConfig The module's config. - */ - private SwerveModule createModule(SwerveModuleConfig moduleConfig) { - SwerveEncoder encoder = null; - CANSparkMax turnSparkMax = null; - CANSparkFlex turnSparkFlex = null; - - switch (moduleConfig.getEncoderType()) { - case CANCODER: - encoder = - new SwerveCANcoder( - createCANcoder( - moduleConfig.getLabel() + " Absolute Encoder", - moduleConfig.getEncoderDeviceId(), - moduleConfig.getEncoderCanBus() - ), - config, - moduleConfig - ); - break; - case SPARK_ENCODER: - if ( - config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHED) || - config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) - ) { - turnSparkMax = - createSparkMax( - moduleConfig.getLabel() + "Turn Motor", - moduleConfig.getTurnMotorDeviceId(), - config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) - ? MotorType.kBrushless - : MotorType.kBrushed - ); - - encoder = - new SwerveSparkEncoder( - turnSparkMax, - createSparkMaxAbsoluteEncoder( - moduleConfig.getLabel() + " Absolute Encoder", - turnSparkMax, - SparkAbsoluteEncoder.Type.kDutyCycle - ) - ); - } else { - turnSparkFlex = - createSparkFlex( - moduleConfig.getLabel() + "Turn Motor", - moduleConfig.getTurnMotorDeviceId(), - config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) - ? MotorType.kBrushless - : MotorType.kBrushed - ); - - encoder = - new SwerveSparkEncoder( - turnSparkFlex, - createSparkFlexAbsoluteEncoder( - moduleConfig.getLabel() + " Absolute Encoder", - turnSparkFlex, - SparkAbsoluteEncoder.Type.kDutyCycle - ) - ); - } - break; - default: - throw new UnsupportedOperationException("Invalid encoder type"); - } - - SwerveMotor moveMotor = createMotor(true, null, moduleConfig); - SwerveMotor turnMotor = turnSparkMax != null - ? new SwerveSparkMax(false, turnSparkMax, encoder, config, moduleConfig) - : ( - turnSparkFlex != null - ? new SwerveSparkFlex(false, turnSparkFlex, encoder, config, moduleConfig) - : createMotor(false, encoder, moduleConfig) - ); - - return new SwerveModule(moveMotor, turnMotor, encoder, config, moduleConfig); - } - - /** - * Creates a swerve motor. - * @param isMoveMotor If the motor is a move motor. - * @param encoder If the motor is a turn motor, the absolute encoder. Otherwise {@code null}. - * @param moduleConfig The module's config. - */ - private SwerveMotor createMotor(boolean isMoveMotor, SwerveEncoder encoder, SwerveModuleConfig moduleConfig) { - String label = moduleConfig.getLabel() + " " + (isMoveMotor ? "Move" : "Turn") + " Motor"; - int deviceId = isMoveMotor ? moduleConfig.getMoveMotorDeviceId() : moduleConfig.getTurnMotorDeviceId(); - String canBus = isMoveMotor ? moduleConfig.getMoveMotorCanBus() : moduleConfig.getTurnMotorCanBus(); - - switch (config.getMoveMotorType()) { - case SPARK_MAX_BRUSHED: - case SPARK_MAX_BRUSHLESS: - return new SwerveSparkMax( - isMoveMotor, - createSparkMax( - label, - deviceId, - config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed - ), - encoder, - config, - moduleConfig - ); - case SPARK_FLEX_BRUSHED: - case SPARK_FLEX_BRUSHLESS: - return new SwerveSparkFlex( - isMoveMotor, - createSparkFlex( - label, - deviceId, - config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed - ), - encoder, - config, - moduleConfig - ); - case TALONFX: - return new SwerveTalonFX(isMoveMotor, createTalonFX(label, deviceId, canBus), config, moduleConfig); - default: - throw new UnsupportedOperationException("Invalid move motor type"); - } - } -} diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index c37e462..9fb86df 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -1,217 +1,202 @@ package org.team340.lib.swerve; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import com.ctre.phoenix6.BaseStatusSignal; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.util.Units; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; +import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder; +import org.team340.lib.swerve.hardware.SwerveMotors.SwerveMotor; import org.team340.lib.util.Math2; /** - * A swerve module for {@link SwerveBase} + * An encapsulation of all hardware for a swerve module. */ -public class SwerveModule { +class SwerveModule implements AutoCloseable { + + public final SwerveMotor moveMotor; + public final SwerveMotor turnMotor; + public final SwerveEncoder encoder; private final SwerveConfig config; private final SwerveModuleConfig moduleConfig; - private final SwerveMotor moveMotor; - private final SwerveMotor turnMotor; - private final SwerveEncoder encoder; - private final SimpleMotorFeedforward moveFF; - private final Timer controlTimer = new Timer(); - - private SwerveModuleState desiredState = new SwerveModuleState(); - private double lastMoveSpeed = 0.0; - private double simDistance = 0.0; - private double simHeading = 0.0; - private double simVelocity = 0.0; + private final double moveRotationsPerMeter; + private final boolean hookStatus; + + private final SwerveModulePosition position = new SwerveModulePosition(); + private final SwerveModuleState state = new SwerveModuleState(); + private final SwerveModuleState nextTarget = new SwerveModuleState(); + private final SwerveModuleState lastTarget = new SwerveModuleState(); + + private Lock cacheMutex = new ReentrantLock(); + private Rotation2d cachedAngle = Math2.kZeroRotation2d; + private double cachedTurnPosition = 0.0; /** * Create the swerve module. - * @param moveMotor The module's move motor. - * @param turnMotor The module's turn motor. - * @param encoder The module's encoder. - * @param config The general swerve config. - * @param moduleConfig The module's config. + * @param config The general swerve API configuration. + * @param moduleConfig The module's configuration. */ - SwerveModule( - SwerveMotor moveMotor, - SwerveMotor turnMotor, - SwerveEncoder encoder, - SwerveConfig config, - SwerveModuleConfig moduleConfig - ) { + public SwerveModule(SwerveConfig config, SwerveModuleConfig moduleConfig) { this.config = config; this.moduleConfig = moduleConfig; - this.moveMotor = moveMotor; - this.turnMotor = turnMotor; - this.encoder = encoder; - moveFF = config.getMoveFF().simpleMotorFeedForward(); - } + moveMotor = SwerveMotor.construct(moduleConfig.moveMotor, config, true); + turnMotor = SwerveMotor.construct(moduleConfig.turnMotor, config, false); + encoder = SwerveEncoder.construct(moduleConfig.encoder, config, turnMotor); - /** - * Configures the current limit of the move motor. - * @param newLimit The new current limit. - */ - public void configMoveCurrentLimit(double newLimit) { - moveMotor.configCurrentLimit(newLimit); + moveRotationsPerMeter = config.moveGearRatio / (config.wheelDiameter * Math.PI); + hookStatus = encoder.hookStatus(); } /** - * Configures the current limit of the turn motor. - * @param newLimit The new current limit. + * Gets the module's configured name. */ - public void configTurnCurrentLimit(double newLimit) { - turnMotor.configCurrentLimit(newLimit); + public String getName() { + return moduleConfig.name; } /** - * Gets the module's label. + * Returns all Phoenix status signals in use by the module. */ - public String getLabel() { - return moduleConfig.getLabel(); + public List getSignals() { + List signals = new ArrayList<>(); + signals.addAll(moveMotor.getSignals()); + signals.addAll(turnMotor.getSignals()); + signals.addAll(encoder.getSignals()); + return signals; } /** - * Gets current duty cycle of move motor. + * Refreshes the cached position and state of the module. + * @return {@code true} on success, {@code false} if a read error ocurred. */ - public double getMoveDutyCycle() { - if (RobotBase.isSimulation()) { - return (simVelocity / config.getVelocity()) * 12.0; - } else { - return moveMotor.getDutyCycle(); + public boolean refresh() { + double turnPosition = turnMotor.getPosition(); + double movePosition = moveMotor.getPosition() - (turnPosition * config.couplingRatio); + Rotation2d angle = Rotation2d.fromRotations(hookStatus ? turnPosition : encoder.getPosition()); + + position.distanceMeters = movePosition / moveRotationsPerMeter; + position.angle = angle; + + state.speedMetersPerSecond = moveMotor.getVelocity() / moveRotationsPerMeter; + state.angle = angle; + + cacheMutex.lock(); + try { + cachedAngle = angle; + cachedTurnPosition = turnPosition; + } finally { + cacheMutex.unlock(); } - } - /** - * Gets the velocity of the swerve module in meters/second. - */ - public double getVelocity() { - if (RobotBase.isSimulation()) { - return simVelocity; - } else { - return moveMotor.getVelocity(); - } + return !moveMotor.readError() && !turnMotor.readError() && !encoder.readError(); } /** - * Gets the distance traveled by the swerve module in meters. + * Gets the module's position. The returned {@link SwerveModulePosition} + * object is final and can be cached, but is volatile, as it may be + * asynchronously refreshed by the odometry thread. */ - public double getDistance() { - if (RobotBase.isSimulation()) { - return simDistance; - } else { - return moveMotor.getPosition(); - } + public SwerveModulePosition getPosition() { + return position; } /** - * Gets the heading of the swerve module in radians. + * Gets the module's state. The returned {@link SwerveModuleState} + * object is final and can be cached, but is volatile, as it may + * be asynchronously refreshed by the odometry thread. */ - public double getHeading() { - if (RobotBase.isSimulation()) { - return simHeading; - } else { - return encoder.getPosition(); - } + public SwerveModuleState getState() { + return state; } /** - * Gets the desired state of the swerve module. + * Gets the module's next target state. The returned {@link SwerveModuleState} + * object is final and can be cached, and is not volatile. */ - public SwerveModuleState getDesiredState() { - return desiredState; + public SwerveModuleState getNextTarget() { + return nextTarget; } /** - * Gets the current state of the swerve module. + * Gets the module's last target state. The returned {@link SwerveModuleState} + * object is final and can be cached, and is not volatile. */ - public SwerveModuleState getModuleState() { - return new SwerveModuleState(getVelocity(), Rotation2d.fromRadians(getHeading())); + public SwerveModuleState getLastTarget() { + return lastTarget; } /** - * Gets the current position of the swerve module. + * Sets the target state of the swerve module. + * @param state The state to apply to the module. */ - public SwerveModulePosition getModulePosition() { - return new SwerveModulePosition(getDistance(), Rotation2d.fromRadians(getHeading())); - } + public void applyState(SwerveModuleState state) { + Rotation2d angleDelta; + double turnPosition; + cacheMutex.lock(); + try { + angleDelta = state.angle.minus(cachedAngle); + turnPosition = cachedTurnPosition; + } finally { + cacheMutex.unlock(); + } - /** - * Sets the desired state of the swerve module. - * @param state The new state. - */ - public void setDesiredState(SwerveModuleState state) { - double moveSpeed = state.speedMetersPerSecond; - double angleDiff = state.angle.rotateBy(Rotation2d.fromRadians(getHeading()).times(-1.0)).getRadians(); boolean flipped = false; - if (Math.abs(angleDiff) > (Math2.HALF_PI)) { - if (angleDiff > 0) angleDiff -= Math.PI; else angleDiff += Math.PI; - moveSpeed *= -1.0; + if (Math.abs(angleDelta.getRadians()) > Math2.HALF_PI) { + state.speedMetersPerSecond *= -1.0; + state.angle = state.angle.rotateBy(Math2.kPiRotation2d); flipped = true; } - moveMotor.setReference(moveSpeed, moveFF.calculate(moveSpeed)); - turnMotor.setReference(turnMotor.getPosition() + angleDiff, 0.0); + moveMotor.setVelocity(state.speedMetersPerSecond * moveRotationsPerMeter); - if (RobotBase.isSimulation()) { - simDistance += simVelocity * controlTimer.get(); - simHeading = - Math.signum(moveSpeed) == Math.signum(state.speedMetersPerSecond) - ? state.angle.getRadians() - : state.angle.minus(Math2.ROTATION2D_PI).getRadians(); - simVelocity = moveSpeed; + if (hookStatus) { + turnMotor.setPosition(state.angle.getRotations()); + } else { + double optimizedDelta = + angleDelta.getRadians() - (flipped ? Math.copySign(Math.PI, angleDelta.getRadians()) : 0.0); + turnMotor.setPosition(turnPosition + (Units.radiansToRotations(optimizedDelta) * config.turnGearRatio)); } - desiredState = flipped ? new SwerveModuleState(-state.speedMetersPerSecond, state.angle.rotateBy(Math2.ROTATION2D_PI)) : state; - lastMoveSpeed = moveSpeed; - controlTimer.restart(); + Math2.copyInto(nextTarget, lastTarget); + Math2.copyInto(state, nextTarget); } /** - * Drives the swerve module using voltage. + * Drives the swerve module using open-loop voltage. Intended for characterization. * @param voltage The voltage to apply to the move motor. - * @param heading The desired heading of the turn motor. + * @param angle The angle to apply to the turn motor. */ - public void setVoltage(double voltage, Rotation2d heading) { - double turnTarget = turnMotor.getPosition() + heading.rotateBy(Rotation2d.fromRadians(getHeading()).times(-1.0)).getRadians(); - - moveMotor.setVoltage(voltage); - turnMotor.setReference(turnTarget, 0.0); - - double expectedVelocity = - moveFF.maxAchievableVelocity( - config.getOptimalVoltage(), - controlTimer.get() == 0 ? 0.0 : (simVelocity - lastMoveSpeed) / controlTimer.get() - ) * - (voltage / config.getOptimalVoltage()); - - if (RobotBase.isSimulation()) { - simDistance += simVelocity * controlTimer.get(); - simHeading = heading.getRadians(); - simVelocity = expectedVelocity; + public void applyVoltage(double voltage, Rotation2d angle) { + double target; + cacheMutex.lock(); + try { + if (hookStatus) { + target = angle.getRotations(); + } else { + target = cachedTurnPosition + (angle.minus(cachedAngle).getRotations() * config.turnGearRatio); + } + } finally { + cacheMutex.unlock(); } - desiredState = new SwerveModuleState(expectedVelocity, heading); - lastMoveSpeed = RobotBase.isSimulation() ? simVelocity : getVelocity(); - controlTimer.restart(); + moveMotor.setVoltage(voltage); + turnMotor.setPosition(target); } - /** - * Returns an integer representing the number of devices with a read error. - * Minimum of {@code 0}, maximum of {@code 3} (Move Motor, Turn Motor, Encoder). - */ - public int readErrorCount() { - int errors = 0; - if (moveMotor.readError()) errors++; - if (turnMotor.readError()) errors++; - if (encoder.readError()) errors++; - return errors; + @Override + public void close() { + try { + moveMotor.close(); + turnMotor.close(); + encoder.close(); + } catch (Exception e) {} } } diff --git a/src/main/java/org/team340/lib/swerve/SwerveState.java b/src/main/java/org/team340/lib/swerve/SwerveState.java new file mode 100644 index 0000000..1f86a0d --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/SwerveState.java @@ -0,0 +1,84 @@ +package org.team340.lib.swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.team340.lib.util.Math2; + +/** + * Represents the state of the robot's drivetrain. + */ +public final class SwerveState { + + /** + * Contains information about swerve module states and positions. + */ + public static final class Modules { + + /** The current measured module positions. */ + public final SwerveModulePosition[] positions; + /** The current measured module states. */ + public final SwerveModuleState[] states; + /** The next target states of the modules. */ + public final SwerveModuleState[] nextTarget; + /** The last target states of the modules. */ + public final SwerveModuleState[] lastTarget; + + public Modules(SwerveModule[] modules) { + positions = new SwerveModulePosition[modules.length]; + states = new SwerveModuleState[modules.length]; + nextTarget = new SwerveModuleState[modules.length]; + lastTarget = new SwerveModuleState[modules.length]; + for (int i = 0; i < modules.length; i++) { + positions[i] = new SwerveModulePosition(); + states[i] = new SwerveModuleState(); + nextTarget[i] = modules[i].getNextTarget(); + lastTarget[i] = modules[i].getLastTarget(); + } + } + } + + /** + * Represents the state of the odometry. + */ + public static final class Odometry { + + /** If Phoenix timesync is being utilized. */ + public boolean timesync; + /** The number of successful odometry measurements since the last loop. */ + public int successes; + /** The number of failing odometry measurements since the last loop. */ + public int failures; + + private Odometry() {} + } + + /** Information about module states and positions. */ + public final Modules modules; + /** The state of the odometry. */ + public final Odometry odometry; + /** The robot's pitch as reported by the IMU. */ + public Rotation2d pitch; + /** The robot's roll as reported by the IMU. */ + public Rotation2d roll; + /** The current blue origin relative pose of the robot. */ + public Pose2d pose; + /** The current measured robot-relative speeds. */ + public ChassisSpeeds speeds; + /** The target robot-relative speeds when using {@code applySpeeds()}. */ + public ChassisSpeeds targetSpeeds; + /** The directionless measured velocity of the robot. */ + public double velocity; + + SwerveState(SwerveModule[] modules) { + this.modules = new Modules(modules); + odometry = new Odometry(); + pitch = Math2.kZeroRotation2d; + roll = Math2.kZeroRotation2d; + pose = Math2.kZeroPose2d; + speeds = new ChassisSpeeds(); + targetSpeeds = new ChassisSpeeds(); + } +} diff --git a/src/main/java/org/team340/lib/swerve/SwerveTunables.java b/src/main/java/org/team340/lib/swerve/SwerveTunables.java new file mode 100644 index 0000000..023430b --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/SwerveTunables.java @@ -0,0 +1,97 @@ +package org.team340.lib.swerve; + +import org.team340.lib.dashboard.Tunable; +import org.team340.lib.swerve.config.SwerveConfig; + +/** + * Utility class for constructing tunables for the {@link SwerveAPI}. + */ +final class SwerveTunables { + + private SwerveTunables() { + throw new AssertionError("This is a utility class!"); + } + + /** + * Enables publishing tunables for adjustment of the API's constants. + * @param name The parent name for the tunables in NetworkTables. + * @param api The swerve API. + */ + public static void initialize(String name, SwerveAPI api) { + SwerveConfig config = api.config; + + Tunable.doubleValue(name + "/velocity", config.velocity, v -> config.velocity = v); + Tunable.doubleValue(name + "/slipAccel", config.slipAccel, v -> config.slipAccel = v); + Tunable.doubleValue(name + "/torqueAccel", config.torqueAccel, v -> config.torqueAccel = v); + Tunable.doubleValue(name + "/angularAccel", config.angularAccel, v -> config.angularAccel = v); + Tunable.doubleValue(name + "/driverVel", config.driverVel, v -> config.driverVel = v); + Tunable.doubleValue(name + "/driverVelExp", config.driverVelExp, v -> config.driverVelExp = v); + Tunable.doubleValue(name + "/driverAngularVel", config.driverAngularVel, v -> config.driverAngularVel = v); + Tunable.doubleValue(name + "/driverAngularVelExp", config.driverAngularVelExp, v -> + config.driverAngularVelExp = v + ); + Tunable.doubleValue(name + "/discretizationPeriod", config.discretizationPeriod, v -> + config.discretizationPeriod = v + ); + + Tunable.doubleValue(name + "/moveMotors/kP", config.movePID[0], v -> { + System.out.println(v); + config.movePID[0] = v; + reapplyGains(true, api); + }); + Tunable.doubleValue(name + "/moveMotors/kI", config.movePID[1], v -> { + config.movePID[1] = v; + reapplyGains(true, api); + }); + Tunable.doubleValue(name + "/moveMotors/kD", config.movePID[2], v -> { + config.movePID[2] = v; + reapplyGains(true, api); + }); + Tunable.doubleValue(name + "/moveMotors/iZone", config.movePID[3], v -> { + config.movePID[3] = v; + reapplyGains(true, api); + }); + + Tunable.doubleValue(name + "/moveMotors/kS", config.moveFF[0], v -> { + config.moveFF[0] = v; + reapplyGains(true, api); + }); + Tunable.doubleValue(name + "/moveMotors/kV", config.moveFF[1], v -> { + config.moveFF[1] = v; + reapplyGains(true, api); + }); + + Tunable.doubleValue(name + "/turnMotors/kP", config.turnPID[0], v -> { + config.turnPID[0] = v; + reapplyGains(false, api); + }); + Tunable.doubleValue(name + "/turnMotors/kI", config.turnPID[1], v -> { + config.turnPID[1] = v; + reapplyGains(false, api); + }); + Tunable.doubleValue(name + "/turnMotors/kD", config.turnPID[2], v -> { + config.turnPID[2] = v; + reapplyGains(false, api); + }); + Tunable.doubleValue(name + "/turnMotors/iZone", config.turnPID[3], v -> { + config.turnPID[3] = v; + reapplyGains(false, api); + }); + } + + /** + * Re-applies PID and FF gains to motors from the swerve config. + * Used for setting new gains after the config has been mutated. + * @param moveMotors {@code true} reapplies to all move motors, {@code false} reapplies to all turn motors. + * @param api The swerve API. + */ + private static void reapplyGains(boolean moveMotors, SwerveAPI api) { + for (var module : api.modules) { + if (moveMotors) { + module.moveMotor.reapplyGains(); + } else { + module.turnMotor.reapplyGains(); + } + } + } +} diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java index 2a37e1d..62dce28 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java @@ -1,607 +1,292 @@ package org.team340.lib.swerve.config; -import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; import java.util.ArrayList; import java.util.List; -import java.util.MissingResourceException; -import java.util.function.Consumer; -import org.team340.lib.swerve.SwerveBase; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; -import org.team340.lib.swerve.hardware.imu.SwerveIMU; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; -import org.team340.lib.util.config.FeedForwardConfig; -import org.team340.lib.util.config.PIDConfig; +import org.team340.lib.swerve.hardware.SwerveIMUs; +import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU; /** * Config builder for {@link SwerveBase}. */ public class SwerveConfig { - private SwerveIMU.Type imuType; - private Object[] imuArgs; - private double period = -1.0; - private PIDConfig movePID; - private FeedForwardConfig moveFF; - private PIDConfig turnPID; - private double moveRampRate = -1.0; - private double turnRampRate = -1.0; - private SwerveMotor.Type moveMotorType; - private SwerveMotor.Type turnMotorType; - private double velocity = -1.0; - private double rotationalVelocity = -1.0; - private double acceleration = -1.0; - private double moduleRotationalVelocity = -1.0; - private double trajectoryVelocity = -1.0; - private double trajectoryAcceleration = -1.0; - private double optimalVoltage = -1.0; - private double moveCurrentLimit = -1.0; - private double turnCurrentLimit = -1.0; - private double moveGearRatio = -1.0; - private double turnGearRatio = -1.0; - private double wheelDiameterInches = -1.0; - private double discretizationLookahead = -1.0; - private double odometryPeriod = -1.0; - private double[] odometryStd; - private double[] visionStd; - private Config sysIdConfig = null; - private double fieldLength = -1.0; - private double fieldWidth = -1.0; - private List modules = new ArrayList<>(); - - /** - * Use an ADIS16470 IMU. - * @param yawAxis The axis to use for yaw. - * @param pitchAxis The axis to use for pitch. - * @param rollAxis The axis to use for roll. - * @param port The SPI port used. - * @param calibrationTime The time frame to calibrate for. - */ - public SwerveConfig useADIS16470( - ADIS16470_IMU.IMUAxis yawAxis, - ADIS16470_IMU.IMUAxis pitchAxis, - ADIS16470_IMU.IMUAxis rollAxis, - SPI.Port port, - ADIS16470_IMU.CalibrationTime calibrationTime - ) { - imuType = SwerveIMU.Type.ADIS16470; - imuArgs = new Object[] { yawAxis, pitchAxis, rollAxis, port, calibrationTime }; - return this; - } - - /** - * Use a Pigeon 2 IMU. - * @param deviceId The device's ID on the CAN bus. - */ - public SwerveConfig usePigeon2(int deviceId) { - return usePigeon2(deviceId, ""); - } - - /** - * Use a Pigeon 2 IMU. - * @param deviceId The device's ID on the CAN bus. - * @param canBus The name of the CAN bus being used. - */ - public SwerveConfig usePigeon2(int deviceId, String canBus) { - imuType = SwerveIMU.Type.PIGEON2; - imuArgs = new Object[] { deviceId, canBus }; - return this; - } - - /** - * Gets the selected IMU's type. - */ - public SwerveIMU.Type getImuType() { - return imuType; - } - - /** - * Gets arguments for the selected IMU. - */ - public Object[] getImuArgs() { - return imuArgs; - } - - /** - * Sets the loop period used. - * By default, {@code TimedRobot} uses a period of {@code 0.020} seconds. - * @param period The loop period in seconds. - */ - public SwerveConfig setPeriod(double period) { + /** The robot's main loop period in seconds. */ + public double period = -1.0; + /** The period to update odometry in seconds. */ + public double odometryPeriod = -1.0; + /** The period to look ahead for discretizing chassis speeds in seconds. */ + public double discretizationPeriod = -1.0; + /** PID gains for move motors, as a tuple of {@code [kP, kI, kD, iZone]}. */ + public double[] movePID; + /** Feed forward gains for move motors, as a tuple of {@code [kS, kV]}. */ + public double[] moveFF; + /** PID gains for turn motors, as a tuple of {@code [kP, kI, kD, iZone]}. */ + public double[] turnPID; + /** If the move motors should have brake mode enabled. */ + public boolean moveBrakeMode = false; + /** If the turn motors should have brake mode enabled. */ + public boolean turnBrakeMode = false; + /** The maximum forward velocity the robot is capable of in meters/second. */ + public double velocity = -1.0; + /** The maximum acceleration the robot is capable of relevant to carpet slip in meters/second/second. */ + public double slipAccel = -1.0; + /** The maximum acceleration the robot is capable of relevant to motor torque in meters/second/second. */ + public double torqueAccel = -1.0; + /** The maximum angular acceleration the robot is capable of in radians/second/second. */ + public double angularAccel = -1.0; + /** The maximum velocity for driver input in meters/second. */ + public double driverVel = -1.0; + /** Exponential power to apply to driver velocity. */ + public double driverVelExp = -1.0; + /** The maximum angular velocity for driver input in radians/second. */ + public double driverAngularVel = -1.0; + /** Exponential power to apply to driver angular velocity. */ + public double driverAngularVelExp = -1.0; + /** The robot's nominal voltage. Typically {@code 12.0}. */ + public double voltage = -1.0; + /** The current limit in amps for move motors. */ + public double moveCurrentLimit = -1.0; + /** The current limit in amps for turn motors. */ + public double turnCurrentLimit = -1.0; + /** The move gear ratio, in motor rotations/wheel rotation. */ + public double moveGearRatio = -1.0; + /** The turn gear ratio, in motor rotations/module rotation. */ + public double turnGearRatio = -1.0; + /** The ratio between the wheel and the module's angle. */ + public double couplingRatio = -1.0; + /** The wheel diameter in meters. */ + public double wheelDiameter = -1.0; + /** Standard deviations for odometry. */ + public Vector odometryStdDevs; + /** The IMU to use, generated by the {@link SwerveIMUs} class. */ + public SwerveIMU.Ctor imu; + /** Name of the CAN bus utilized by Phoenix devices. */ + public String phoenixCanBus = ""; + /** {@code true} if Phoenix Pro is active on all devices used by the swerve API. */ + public boolean phoenixPro = false; + /** If move motors should enable FOC. */ + public boolean phoenixMoveFOC = false; + /** If turn motors should enable FOC. */ + public boolean phoenixTurnFOC = false; + /** Configurations for swerve modules. */ + public SwerveModuleConfig[] modules; + + /** + * Sets various timings utilized by the robot. + * @param period The robot's main loop period in seconds. + * @param odometry The period to update odometry in seconds. + * @param discretization The period to look ahead for discretizing chassis speeds in seconds. + */ + public SwerveConfig setTimings(double period, double odometry, double discretization) { this.period = period; + odometryPeriod = odometry; + discretizationPeriod = discretization; return this; } /** - * Gets the configured loop period. - */ - public double getPeriod() { - return period; - } - - /** - * Sets PID constants for move motors. - * A good starting point is a {@code p} value of {@code 0.001}, and {@code i} and {@code d} values of {@code 0.0}. - * @param p Proportional gain constant. - * @param i Integral gain constant. - * @param d Derivative gain constant. + * Sets PID gains for move motors. Note that {@link TalonFX} motors do not support {@code iZone}. + * @param kP Proportional gain constant. + * @param kI Integral gain constant. + * @param kD Derivative gain constant. * @param iZone Integral range. */ - public SwerveConfig setMovePID(double p, double i, double d, double iZone) { - movePID = new PIDConfig(p, i, d, iZone); + public SwerveConfig setMovePID(double kP, double kI, double kD, double iZone) { + movePID = new double[] { kP, kI, kD, iZone }; return this; } /** - * Gets the configured PID constants for move motors. + * Sets feed forward constants for move motors. A good starting point is a {@code kV} + * value of {@code / }. These values can be obtained + * via characterization using sysID with a real robot. + * @param kS The static gain. + * @param kV The velocity gain. */ - public PIDConfig getMovePID() { - return movePID; - } - - /** - * Sets feed forward constants for move motors. Note that turn motors don't use feed forward, as it typically causes the motor to burn out. - * A good starting point is a {@code v} value of {@code / }, and {@code s} and {@code v} values of {@code 0.0}. - * These values can be obtained via characterization using sysID. - * @param s The static gain. - * @param v The velocity gain. - * @param a The acceleration gain. - */ - public SwerveConfig setMoveFF(double s, double v, double a) { - moveFF = new FeedForwardConfig(s, v, a); + public SwerveConfig setMoveFF(double kS, double kV) { + moveFF = new double[] { kS, kV }; return this; } /** - * Gets the configured feed forward constants for move motors. - */ - public FeedForwardConfig getMoveFF() { - return moveFF; - } - - /** - * Sets PID constants for turn motors. - * A good starting point is a {@code p} value of {@code 0.5}, a {@code i} value of {@code 0.0}, and a {@code d} value of {@code 15.0}. - * @param p Proportional gain constant. - * @param i Integral gain constant. - * @param d Derivative gain constant. + * Sets PID gains for turn motors. Note that {@link TalonFX} motors do not support {@code iZone}. + * @param kP Proportional gain constant. + * @param kI Integral gain constant. + * @param kD Derivative gain constant. * @param iZone Integral range. */ - public SwerveConfig setTurnPID(double p, double i, double d, double iZone) { - this.turnPID = new PIDConfig(p, i, d, iZone); + public SwerveConfig setTurnPID(double kP, double kI, double kD, double iZone) { + this.turnPID = new double[] { kP, kI, kD, iZone }; return this; } /** - * Gets the configured PID constants for turn motors. - */ - public PIDConfig getTurnPID() { - return turnPID; - } - - /** - * Sets the motor ramp rate. - * Increase this to reduce the load on the motors. - * @param moveRampRate Time in seconds to go from {@code 0.0} to full throttle on the move motors. - * @param turnRampRate Time in seconds to go from {@code 0.0} to full throttle on the turn motors. + * Sets motor brake modes. + * @param move If the move motors should have brake mode enabled. + * @param turn If the turn motors should have brake mode enabled. */ - public SwerveConfig setRampRate(double moveRampRate, double turnRampRate) { - this.moveRampRate = moveRampRate; - this.turnRampRate = turnRampRate; + public SwerveConfig setBrakeMode(boolean move, boolean turn) { + moveBrakeMode = move; + turnBrakeMode = turn; return this; } /** - * Gets the move ramp rate in seconds. - */ - public double getMoveRampRate() { - return moveRampRate; - } - - /** - * Gets the turn ramp rate in seconds. - */ - public double getTurnRampRate() { - return turnRampRate; - } - - /** - * Sets the motor types used. - * @param moveMotorType The move motor type. - * @param turnMotorType The turn motor type. - */ - public SwerveConfig setMotorTypes(SwerveMotor.Type moveMotorType, SwerveMotor.Type turnMotorType) { - this.moveMotorType = moveMotorType; - this.turnMotorType = turnMotorType; - return this; - } - - /** - * Gets the move motor type. - */ - public SwerveMotor.Type getMoveMotorType() { - return moveMotorType; - } - - /** - * Gets the turn motor type. - */ - public SwerveMotor.Type getTurnMotorType() { - return turnMotorType; - } - - /** - * Sets max speed constraints. - * These are used for constraining the requested velocity commanded to swerve modules, as well as scaling when driving by a percent of max speed. - * - *

    - * You may find more predictable behavior by setting these values lower than the actual maximum capabilities of your robot. - * It is recommended that these values are tested for using an actual robot. An easy way to do so is to set these values to an impossibly high value, then examine the outputs in network tables. - * Initial theoretical values can be estimated using the following formulas: - * - *

    - * Max Robot Velocity: {@code ( * 0.80 / 60) / ( / ( * PI))} + * Sets limits for the drivetrain. * *

    - * Max Robot Rotational Velocity: {@code ( / (^2 + ^2)^0.5) * 2} + * You may find more predictable behavior by setting these values slightly lower than the actual maximum capabilities of your robot. + * It is recommended that these values are found empirically using an actual robot. An easy way to do so is to configure infeasible limits, then analyze telemetry. * - * @param velocity The maximum velocity the robot is capable of in meters/second. - * @param rotationalVelocity The maximum rotational velocity the robot is capable of in radians/second. + * @param velocity The maximum forward velocity the robot is capable of in meters/second. More specifically, the maximum velocity a move motor is capable of. + * @param slipAccel The maximum acceleration the robot is capable of relevant to carpet slip in meters/second/second. + * @param torqueAccel The maximum acceleration the robot is capable of relevant to motor torque in meters/second/second. + * @param angularAccel The maximum angular acceleration the robot is capable of in radians/second/second. */ - public SwerveConfig setMaxSpeeds(double velocity, double rotationalVelocity) { + public SwerveConfig setLimits(double velocity, double slipAccel, double torqueAccel, double angularAccel) { this.velocity = velocity; - this.rotationalVelocity = rotationalVelocity; + this.slipAccel = slipAccel; + this.torqueAccel = torqueAccel; + this.angularAccel = angularAccel; return this; } /** - * Gets the configured maximum robot velocity in meters/second. - */ - public double getVelocity() { - return velocity; - } - - /** - * Gets the configured maximum robot rotational velocity in radians/second. - */ - public double getRotationalVelocity() { - return rotationalVelocity; - } - - /** - * Sets constraints for the ratelimiter. - * - *

    - * You may find more predictable behavior by setting these values lower than the actual maximum capabilities of your robot. - * It is recommended that these values are tested for using an actual robot. An easy way to do so is to set these values to an impossibly high value, then examine the outputs in network tables. - * Initial theoretical values can be estimated using the following formulas: - * - *

    - * Max Robot Acceleration: {@code * 2} (VERY much an estimate, typical ballpark acceleration for robots weighing ~120 pounds) - * Can also be pulled from Choreo. - * - *

    - * Max Module Rotational Velocity: {@code ( / 60) / ( / (PI * 2)) * 0.7} - * - * @param acceleration The maximum acceleration the robot is capable of in meters/second/second. - * @param moduleRotationalVelocity The maximum module rotational velocity the robot is capable of in radians/second. - */ - public SwerveConfig setRatelimits(double acceleration, double moduleRotationalVelocity) { - this.acceleration = acceleration; - this.moduleRotationalVelocity = moduleRotationalVelocity; - return this; - } - - /** - * Gets the configured maximum robot acceleration in meters/second/second. - */ - public double getAcceleration() { - return acceleration; - } - - /** - * Gets the configured maximum module rotational velocity in radians/second. - */ - public double getModuleRotationalVelocity() { - return moduleRotationalVelocity; - } - - /** - * Sets constraints for trajectory generation. - * @param trajectoryVelocity Max trajectory velocity in meters/second. - * @param trajectoryAcceleration Max trajectory acceleration in meters/second/second. + * Configures the profile to apply to speeds calculated using the driver's controller input. + * @param velocity The maximum velocity for driver input in meters/second. + * @param velocityExp Exponential power to apply to driver velocity. {@code 1.0} effectively disables. + * @param angularVel The maximum angular velocity for driver input in radians/second. + * @param angularVelExp Exponential power to apply to driver angular velocity. {@code 1.0} effectively disables. */ - public SwerveConfig setTrajectoryConstraints(double trajectoryVelocity, double trajectoryAcceleration) { - this.trajectoryVelocity = trajectoryVelocity; - this.trajectoryAcceleration = trajectoryAcceleration; + public SwerveConfig setDriverProfile(double velocity, double velocityExp, double angularVel, double angularVelExp) { + driverVel = velocity; + driverVelExp = velocityExp; + driverAngularVel = angularVel; + driverAngularVelExp = angularVelExp; return this; } - /** - * Gets the configured maximum trajectory velocity in meters/second. - */ - public double getTrajectoryVelocity() { - return trajectoryVelocity; - } - - /** - * Gets te configured maximum trajectory acceleration in meters/second/second. - */ - public double getTrajectoryAcceleration() { - return trajectoryAcceleration; - } - /** * Sets power properties. - * @param optimalVoltage Optimal voltage to run at. Should be {@code 12}. - * @param moveCurrentLimit A current limit in amps for move motors. This is typically {@code 40}. - * @param turnCurrentLimit A current limit in amps for turn motors. This is typically {@code 30}. + * @param voltage The robot's nominal voltage. Typically {@code 12.0}. + * @param moveCurrentLimit The current limit in amps for move motors. + * @param turnCurrentLimit The current limit in amps for turn motors. */ - public SwerveConfig setPowerProperties(double optimalVoltage, double moveCurrentLimit, double turnCurrentLimit) { - this.optimalVoltage = optimalVoltage; + public SwerveConfig setPowerProperties(double voltage, double moveCurrentLimit, double turnCurrentLimit) { + this.voltage = voltage; this.moveCurrentLimit = moveCurrentLimit; this.turnCurrentLimit = turnCurrentLimit; return this; } - /** - * Gets the configured optimal voltage. - */ - public double getOptimalVoltage() { - return optimalVoltage; - } - - /** - * Gets the configured current limit for move motors in amps. - */ - public double getMoveCurrentLimit() { - return moveCurrentLimit; - } - - /** - * Gets the configured current limit for turn motors in amps. - */ - public double getTurnCurrentLimit() { - return turnCurrentLimit; - } - /** * Sets swerve gearing properties. - * @param moveGearRatio The move gear ratio (inverse of the gearing reduction). - * @param turnGearRatio The turn gear ratio (inverse of the gearing reduction). - * @param wheelDiameterInches The wheel diameter in inches. - */ - public SwerveConfig setMechanicalProperties(double moveGearRatio, double turnGearRatio, double wheelDiameterInches) { - this.moveGearRatio = moveGearRatio; - this.turnGearRatio = turnGearRatio; - this.wheelDiameterInches = wheelDiameterInches; - return this; - } - - /** - * Gets the configured gear ratio for the move motors. - */ - public double getMoveGearRatio() { - return moveGearRatio; - } - - /** - * Gets the configured gear ratio for the turn motors. - */ - public double getTurnGearRatio() { - return turnGearRatio; - } - - /** - * Gets the configured wheel diameter. - */ - public double getWheelDiameterInches() { - return wheelDiameterInches; - } - - /** - * Sets the discretization lookahead in seconds. - * Used for countering drift caused by translating and rotating simultaneously. This - * should be characterized on the robot, as it effectively models lag in the physical - * swerve system which is difficult to simulate. A good starting value is around double - * the robot's loop period. Values below the robot's loop period is not recommended. - * @param discretizationLookahead Lookahead in seconds. - */ - public SwerveConfig setDiscretizationLookahead(double discretizationLookahead) { - this.discretizationLookahead = discretizationLookahead; - return this; - } - - /** - * Gets the configured discretization lookahead in seconds. - */ - public double getDiscretizationLookahead() { - return discretizationLookahead; - } - - /** - * Sets period in seconds between odometry samples. - * @param odometryPeriod Period in seconds. - */ - public SwerveConfig setOdometryPeriod(double odometryPeriod) { - this.odometryPeriod = odometryPeriod; + * @param moveRatio The move gear ratio, in motor rotations/wheel rotation. + * @param turnRatio The turn gear ratio, in motor rotations/module rotation. + * @param couplingRatio The ratio between the wheel and the module's angle. Used as a compensation factor for odometry, set to {@code 0.0} to disable. + * @param wheelDiameter The wheel diameter in meters. + */ + public SwerveConfig setMechanicalProperties( + double moveRatio, + double turnRatio, + double couplingRatio, + double wheelDiameter + ) { + moveGearRatio = moveRatio; + turnGearRatio = turnRatio; + this.couplingRatio = couplingRatio; + this.wheelDiameter = wheelDiameter; return this; } - /** - * Gets period in seconds between odometry samples. - */ - public double getOdometryPeriod() { - return odometryPeriod; - } - /** * Sets the standard deviations for pose estimation from module odometry. - * A good starting configuration is all axis with a magnitude of {@code 0.1}. - * @param x The X axis standard deviation in meters. - * @param y The Y axis standard deviation in meters. - * @param rot The rotational standard deviation in radians. - */ - public SwerveConfig setOdometryStd(double x, double y, double rot) { - this.odometryStd = new double[] { x, y, rot }; - return this; - } - - /** - * Gets the configured standard deviations for odometry, as an array of {@code [x, y, rot]}. - */ - public double[] getOdometryStd() { - return odometryStd; - } - - /** - * Sets the standard deviations for pose estimation from vision. - * A good starting configuration is all axis with a magnitude of {@code 0.1}. * @param x The X axis standard deviation in meters. * @param y The Y axis standard deviation in meters. - * @param rot The rotational standard deviation in radians. + * @param angular The angular standard deviation in radians. */ - public SwerveConfig setVisionStd(double x, double y, double rot) { - this.visionStd = new double[] { x, y, rot }; + public SwerveConfig setOdometryStd(double x, double y, double angular) { + this.odometryStdDevs = VecBuilder.fill(x, y, angular); return this; } /** - * Gets the configured standard deviations for vision, as an array of {@code [x, y, rot]}. - */ - public double[] getVisionStd() { - return visionStd; - } - - /** - * Sets config for SysId. + * Sets the IMU to use. + * @param imu The IMU to use, generated by the {@link SwerveIMUs} class. */ - public SwerveConfig setSysIdConfig(Config sysIdConfig) { - this.sysIdConfig = sysIdConfig; + public SwerveConfig setIMU(SwerveIMU.Ctor imu) { + this.imu = imu; return this; } /** - * Gets config for SysId. - */ - public Config getSysIdConfig() { - return sysIdConfig; - } - - /** - * Sets the field size. - * @param fieldLength The field's length in meters. Typically {@code 16.5417}. - * @param fieldWidth The field's width in meters. Typically {@code 8.0136}. + * Sets available features for Phoenix devices. For Phoenix Pro features to be utilized by the Swerve API, + * all devices must be activated, and intrinsically must all be Phoenix devices (Pigeon2 + TalonFX + CANcoder). + * This configuration option can be omitted safely if its features are not needed. + * @param canBus Name of the CAN bus utilized by Phoenix devices. + * @param pro {@code true} if Phoenix Pro is active on all devices used by the swerve API. + * @param moveFOC If move motors should enable FOC. {@code pro} can still be {@code false} while enabling FOC, provided all move motors are still licensed. + * @param turnFOC If turn motors should enable FOC. {@code pro} can still be {@code false} while enabling FOC, provided all turn motors are still licensed. */ - public SwerveConfig setFieldSize(double fieldLength, double fieldWidth) { - this.fieldLength = fieldLength; - this.fieldWidth = fieldWidth; + public SwerveConfig setPhoenixFeatures(String canBus, boolean pro, boolean moveFOC, boolean turnFOC) { + phoenixCanBus = canBus; + phoenixPro = pro; + phoenixMoveFOC = moveFOC; + phoenixTurnFOC = turnFOC; return this; } /** - * Gets the configured field length in meters. - */ - public double getFieldLength() { - return fieldLength; - } - - /** - * Gets the configured field width in meters. - */ - public double getFieldWidth() { - return fieldWidth; - } - - /** - * Adds a module to the config. + * Sets module configs. * See {@link SwerveModuleConfig}. - * @param moduleConfig The config of module to add. + * @param modules The configs of the swerve modules. */ - public SwerveConfig addModule(SwerveModuleConfig moduleConfig) { - modules.add(moduleConfig); + public SwerveConfig setModules(SwerveModuleConfig... modules) { + this.modules = modules; return this; } - /** - * Adds a module to the config. - * See {@link SwerveModuleConfig}. - * @param moduleConfigConsumer A consumer for the module config. - */ - public SwerveConfig addModule(Consumer moduleConfigConsumer) { - SwerveModuleConfig moduleConfig = new SwerveModuleConfig(); - moduleConfigConsumer.accept(moduleConfig); - modules.add(moduleConfig); - return this; - } - - /** - * Gets the configured modules. - */ - public List getModules() { - return modules; - } - /** * Verifies the config as well as the config's modules. * Throws an error if an issue is found. */ public void verify() { - if (imuType == null) throwMissing("IMU"); - if (imuArgs == null) throwMissing("IMU Args"); - if (period == -1.0) throwMissing("Period"); - if (movePID == null) throwMissing("Move PID"); - if (moveFF == null) throwMissing("Move FF"); - if (turnPID == null) throwMissing("Turn PID"); - if (moveRampRate == -1.0) throwMissing("MoveRamp Rate"); - if (turnRampRate == -1.0) throwMissing("Turn Ramp Rate"); - if (moveMotorType == null) throwMissing("Move Motor Type"); - if (turnMotorType == null) throwMissing("Turn Motor Type"); - if (velocity == -1.0) throwMissing("Velocity"); - if (rotationalVelocity == -1.0) throwMissing("Rotational Velocity"); - if (acceleration == -1.0) throwMissing("Acceleration"); - if (moduleRotationalVelocity == -1.0) throwMissing("Module Rotational Velocity"); - if (trajectoryVelocity == -1.0) throwMissing("Trajectory Velocity"); - if (trajectoryAcceleration == -1.0) throwMissing("Trajectory Acceleration"); - if (optimalVoltage == -1.0) throwMissing("Optimal Voltage"); - if (moveCurrentLimit == -1.0) throwMissing("Move Current Limit"); - if (turnCurrentLimit == -1.0) throwMissing("Turn Current Limit"); - if (moveGearRatio == -1.0) throwMissing("Move Gear Ratio"); - if (turnGearRatio == -1.0) throwMissing("Turn Gear Ratio"); - if (wheelDiameterInches == -1.0) throwMissing("Wheel Diameter"); - if (discretizationLookahead == -1.0) throwMissing("Discretization Lookahead"); - if (odometryPeriod == -1.0) throwMissing("Odometry Period"); - if (odometryStd == null) throwMissing("Odometry Standard Deviations"); - if (visionStd == null) throwMissing("Vision Standard Deviations"); - if (sysIdConfig == null) throwMissing("SysId Config"); - if (fieldLength == -1.0) throwMissing("Field Length"); - if (fieldWidth == -1.0) throwMissing("Field Width"); - if (modules.size() == 0) throwMissing("Modules"); + List missing = new ArrayList<>(); + if (period == -1.0) missing.add("Period"); + if (odometryPeriod == -1.0) missing.add("Odometry Period"); + if (discretizationPeriod == -1.0) missing.add("Discretization Period"); + if (movePID == null) missing.add("Move PID"); + if (moveFF == null) missing.add("Move FF"); + if (turnPID == null) missing.add("Turn PID"); + if (velocity == -1.0) missing.add("Velocity"); + if (slipAccel == -1.0) missing.add("Slip Acceleration"); + if (torqueAccel == -1.0) missing.add("Torque Acceleration"); + if (angularAccel == -1.0) missing.add("Angular Acceleration"); + if (driverVel == -1.0) missing.add("Driver Velocity"); + if (driverVelExp == -1.0) missing.add("Driver Velocity Exponential"); + if (driverAngularVel == -1.0) missing.add("Driver Angular Velocity"); + if (driverAngularVelExp == -1.0) missing.add("Driver Angular Velocity Exponential"); + if (voltage == -1.0) missing.add("Voltage"); + if (moveCurrentLimit == -1.0) missing.add("Move Current Limit"); + if (turnCurrentLimit == -1.0) missing.add("Turn Current Limit"); + if (moveGearRatio == -1.0) missing.add("Move Gear Ratio"); + if (turnGearRatio == -1.0) missing.add("Turn Gear Ratio"); + if (couplingRatio == -1.0) missing.add("Coupling Ratio"); + if (wheelDiameter == -1.0) missing.add("Wheel Diameter"); + if (odometryStdDevs == null) missing.add("Odometry Standard Deviations"); + if (imu == null) missing.add("IMU"); + if (modules == null) missing.add("Modules"); + + if (!missing.isEmpty()) { + throw new IllegalArgumentException("SwerveConfig missing values: " + String.join(", ", missing)); + } for (SwerveModuleConfig module : modules) { module.verify(); - - if ( - ( - !turnMotorType.equals(SwerveMotor.Type.SPARK_MAX_BRUSHED) && - !turnMotorType.equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) && - !turnMotorType.equals(SwerveMotor.Type.SPARK_FLEX_BRUSHED) && - !turnMotorType.equals(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) - ) && - module.getEncoderType().equals(SwerveEncoder.Type.SPARK_ENCODER) - ) throw new UnsupportedOperationException("Cannot use Spark attached encoder on non-Spark motor"); - - if ( - !module.getMoveMotorCanBus().isEmpty() && !moveMotorType.equals(SwerveMotor.Type.TALONFX) - ) throw new UnsupportedOperationException("Cannot set custom CAN bus for non-Talon FX motor"); - if ( - !module.getTurnMotorCanBus().isEmpty() && !turnMotorType.equals(SwerveMotor.Type.TALONFX) - ) throw new UnsupportedOperationException("Cannot set custom CAN bus for non-Talon FX motor"); } } - - private void throwMissing(String key) { - throw new MissingResourceException("Missing value: " + key, this.getClass().getSimpleName(), key); - } } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java index 73d1a6e..8250df5 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java @@ -1,269 +1,88 @@ package org.team340.lib.swerve.config; import edu.wpi.first.math.geometry.Translation2d; -import java.util.MissingResourceException; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; +import java.util.ArrayList; +import java.util.List; +import org.team340.lib.swerve.hardware.SwerveEncoders; +import org.team340.lib.swerve.hardware.SwerveEncoders.SwerveEncoder; +import org.team340.lib.swerve.hardware.SwerveMotors; +import org.team340.lib.swerve.hardware.SwerveMotors.SwerveMotor; /** * Config builder for {@link SwerveBase} modules. */ public class SwerveModuleConfig { - private String label; - private SwerveEncoder.Type encoderType; - private int encoderDeviceId = -1; - private String encoderCanBus; - private double encoderOffset = 0; - private boolean encoderInverted = false; - private Translation2d position; - private int moveMotorDeviceId = -1; - private String moveMotorCanBus = ""; - private boolean moveMotorBrake; - private boolean moveMotorInverted; - private int turnMotorDeviceId = -1; - private String turnMotorCanBus = ""; - private boolean turnMotorBrake; - private boolean turnMotorInverted; + /** The name of the module. */ + public String name; + /** The module's location relative to the center of the robot. */ + public Translation2d location; + /** The move motor to use, generated by the {@link SwerveMotors} class. */ + public SwerveMotor.Ctor moveMotor; + /** The turn motor to use, generated by the {@link SwerveMotors} class. */ + public SwerveMotor.Ctor turnMotor; + /** The encoder to use, generated by the {@link SwerveEncoders} class. */ + public SwerveEncoder.Ctor encoder; /** - * Sets the module's label. - * @param label The label to use. Shown in the dashboard. + * Sets the module's name. Utilized by telemetry. + * @param name The name of the module. */ - public SwerveModuleConfig setLabel(String label) { - this.label = label; + public SwerveModuleConfig setName(String name) { + this.name = name; return this; } /** - * Gets the configured label. + * Sets the module's location relative to the center of the robot (in meters). + * @param x The module's X location in meters. + * @param y The module's Y location in meters. */ - public String getLabel() { - return label; - } - - /** - * Use a CANcoder. - * A simple method for finding {@code offset} is starting with a value of {@code 0.0}, turn the modules manually to face forward, then directly copy the values found in network tables. - * @param deviceId The device's ID on the CAN bus. - * @param offset The encoder's offset in radians. - * @param inverted If the encoder is inverted. - */ - public SwerveModuleConfig useCANcoder(int deviceId, double offset, boolean flipped) { - return useCANcoder(deviceId, "", offset, flipped); - } - - /** - * Use a CANcoder. - * A simple method for finding {@code offset} is starting with a value of {@code 0.0}, turn the modules manually to face forward, then directly copy the values found in network tables. - * @param deviceId The device's ID on the CAN bus. - * @param canBus The name of the CAN bus being used. - * @param offset The encoder's offset in radians. - * @param inverted If the encoder is inverted. - */ - public SwerveModuleConfig useCANcoder(int deviceId, String canBus, double offset, boolean inverted) { - encoderType = SwerveEncoder.Type.CANCODER; - encoderDeviceId = deviceId; - encoderCanBus = canBus; - encoderOffset = offset; - encoderInverted = inverted; + public SwerveModuleConfig setLocation(double x, double y) { + location = new Translation2d(x, y); return this; } /** - * Use a Spark Attached Absolute Encoder. - * A simple method for finding {@code offset} is starting with a value of {@code 0.0}, turn the modules manually to face forward, then directly copy the values found in network tables. - * @param offset The encoder's offset in radians. - * @param inverted If the encoder is inverted. + * Sets the move motor to use. + * @param motor The move motor to use, generated by the {@link SwerveMotors} class. */ - public SwerveModuleConfig useSparkAttachedEncoder(double offset, boolean inverted) { - encoderType = SwerveEncoder.Type.SPARK_ENCODER; - encoderOffset = offset; - encoderInverted = inverted; + public SwerveModuleConfig setMoveMotor(SwerveMotor.Ctor motor) { + moveMotor = motor; return this; } /** - * Gets the selected absolute encoder's type. + * Sets the turn motor to use. + * @param motor The turn motor to use, generated by the {@link SwerveMotors} class. */ - public SwerveEncoder.Type getEncoderType() { - return encoderType; - } - - /** - * Gets the configured absolute encoder's device ID on the CAN bus if applicable. - */ - public int getEncoderDeviceId() { - return encoderDeviceId; - } - - /** - * Gets the name of the CAN bus being used by the absolute encoder if applicable. - */ - public String getEncoderCanBus() { - return encoderCanBus; - } - - /** - * Gets the configured absolute encoder's offset in radians. - */ - public double getEncoderOffset() { - return encoderOffset; - } - - /** - * Gets the configured inverted state of the absolute encoder. - */ - public boolean getEncoderInverted() { - return encoderInverted; - } - - /** - * Sets the module's position relative to the center of the robot (in meters). - * Reminder that relative to the front of the robot, X is forwards and Y is sideways. - * This can be found from CAD. - * @param x The module's X position in meters. - * @param y The module's Y position in meters. - */ - public SwerveModuleConfig setPosition(double x, double y) { - position = new Translation2d(x, y); - return this; - } - - /** - * Gets the module's configured position. - */ - public Translation2d getPosition() { - return position; - } - - /** - * Sets the move motor for the module. - * @param deviceId The device's ID on the CAN bus. - * @param brake If brake mode should be enabled. Brake mode is typically enabled. - * @param inverted If the motor's output is inverted. - */ - public SwerveModuleConfig setMoveMotor(int deviceId, boolean brake, boolean inverted) { - moveMotorDeviceId = deviceId; - moveMotorBrake = brake; - moveMotorInverted = inverted; + public SwerveModuleConfig setTurnMotor(SwerveMotor.Ctor motor) { + turnMotor = motor; return this; } /** - * Sets the move motor for the module. - * @param deviceId The device's ID on the CAN bus. - * @param canBus The name of the CAN bus being used. - * @param brake If brake mode should be enabled. Brake mode is typically enabled. - * @param inverted If the motor's output is inverted. + * Sets the encoder to use. + * @param encoder The encoder to use, generated by the {@link SwerveEncoders} class. */ - public SwerveModuleConfig setMoveMotor(int deviceId, String canBus, boolean brake, boolean inverted) { - moveMotorDeviceId = deviceId; - moveMotorCanBus = canBus; - moveMotorBrake = brake; - moveMotorInverted = inverted; + public SwerveModuleConfig setEncoder(SwerveEncoder.Ctor encoder) { + this.encoder = encoder; return this; } - /** - * Gets the configured move motor's device ID on the CAN bus. - */ - public int getMoveMotorDeviceId() { - return moveMotorDeviceId; - } - - /** - * Gets the string of the CAN bus configured to be utilized by the move motor. - */ - public String getMoveMotorCanBus() { - return moveMotorCanBus; - } - - /** - * Gets the configured brake mode of the move motor. - */ - public boolean getMoveMotorBrake() { - return moveMotorBrake; - } - - /** - * Gets the configured inverted state of the move motor. - */ - public boolean getMoveMotorInverted() { - return moveMotorInverted; - } - - /** - * Sets the turn motor for the module. - * @param deviceId The device's ID on the CAN bus. - * @param brake If brake mode should be enabled. Brake mode is typically disabled. - * @param inverted If the motor's output is inverted. - */ - public SwerveModuleConfig setTurnMotor(int deviceId, boolean brake, boolean inverted) { - turnMotorDeviceId = deviceId; - turnMotorBrake = brake; - turnMotorInverted = inverted; - return this; - } - - /** - * Sets the turn motor for the module. - * @param deviceId The device's ID on the CAN bus. - * @param canBus The name of the CAN bus being used. - * @param brake If brake mode should be enabled. Brake mode is typically disabled. - * @param inverted If the motor's output is inverted. - */ - public SwerveModuleConfig setTurnMotor(int deviceId, String canBus, boolean brake, boolean inverted) { - turnMotorDeviceId = deviceId; - turnMotorCanBus = canBus; - turnMotorBrake = brake; - turnMotorInverted = inverted; - return this; - } - - /** - * Gets the configured turn motor's device ID on the CAN bus. - */ - public int getTurnMotorDeviceId() { - return turnMotorDeviceId; - } - - /** - * Gets the string of the CAN bus configured to be utilized by the turn motor. - */ - public String getTurnMotorCanBus() { - return turnMotorCanBus; - } - - /** - * Gets the configured brake mode of the turn motor. - */ - public boolean getTurnMotorBrake() { - return turnMotorBrake; - } - - /** - * Gets the configured inverted state of the turn motor. - */ - public boolean getTurnMotorInverted() { - return turnMotorInverted; - } - /** * Verifies the config. */ public void verify() { - if (label == null) throwMissing("Label"); - if (encoderType == null) throwMissing("Encoder Type"); - if (encoderType.equals(SwerveEncoder.Type.CANCODER)) { - if (encoderDeviceId == -1) throwMissing("CANcoder Device ID"); - if (encoderCanBus == null) throwMissing("CANcoder CAN Bus"); + List missing = new ArrayList<>(); + if (name == null) missing.add("Name"); + if (location == null) missing.add("Position"); + if (moveMotor == null) missing.add("Move Motor"); + if (turnMotor == null) missing.add("Turn Motor"); + if (encoder == null) missing.add("Encoder"); + + if (!missing.isEmpty()) { + throw new IllegalArgumentException("SwerveModuleConfig missing values: " + String.join(", ", missing)); } - if (position == null) throwMissing("Position"); - if (moveMotorDeviceId == -1) throwMissing("Move Motor Device ID"); - if (turnMotorDeviceId == -1) throwMissing("Turn Motor Device ID"); - } - - private void throwMissing(String key) { - throw new MissingResourceException("Missing value", this.getClass().getSimpleName(), key); } } diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java new file mode 100644 index 0000000..fb901e9 --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveBaseHardware.java @@ -0,0 +1,50 @@ +package org.team340.lib.swerve.hardware; + +import com.ctre.phoenix6.BaseStatusSignal; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import java.util.List; +import org.team340.lib.swerve.SwerveAPI; + +interface SwerveBaseHardware extends AutoCloseable { + /** + * The CAN frame period in seconds to use for frames containing data + * utilized for telemetry that is not necessarily required for swerve + * to function. + */ + public static final double TELEMETRY_CAN_PERIOD = 0.2; + + /** + * Returns the device's underlying API. + */ + public abstract Object getAPI(); + + /** + * Logs the device via Epilogue. + * @param logger The logger to log data to. + * @param errorHandler The handler to use if logging raised an exception. + */ + public abstract void log(DataLogger logger, ErrorHandler errorHandler); + + /** + * Returns all Phoenix status signals in use by the hardware. Phoenix + * hardware should not invoke {@code .refresh()} on their status + * signals in their implementations. This method is required for the + * odometry thread to register signals to be refreshed automatically. + * Because signals are not thread safe, all returned signals should + * also be cloned in their initialization as to not interfere with + * telemetry, which is invoked on the main thread. The exception to + * this rule is IMU pitch and roll values, as they are only measured + * synchronously when calling {@link SwerveAPI#refresh()}. + */ + public default List getSignals() { + return List.of(); + } + + /** + * If the device has encountered an error while reading inputs. + */ + public default boolean readError() { + return false; + } +} diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java new file mode 100644 index 0000000..4698aed --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveEncoders.java @@ -0,0 +1,319 @@ +package org.team340.lib.swerve.hardware; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.wpilibj.RobotBase; +import java.util.List; +import java.util.function.BiFunction; +import org.team340.lib.logging.CANcoderLogger; +import org.team340.lib.logging.SparkAbsoluteEncoderLogger; +import org.team340.lib.swerve.SwerveAPI; +import org.team340.lib.swerve.config.SwerveConfig; +import org.team340.lib.swerve.hardware.SwerveMotors.SwerveMotor; +import org.team340.lib.util.Mutable; +import org.team340.lib.util.ctre.PhoenixUtil; +import org.team340.lib.util.rev.SparkAbsoluteEncoderConfig; +import org.team340.lib.util.rev.SparkFlexConfig; +import org.team340.lib.util.rev.SparkMaxConfig; + +/** + * Contains implementations for absolute encoders to be used with the {@link SwerveAPI}. + */ +public final class SwerveEncoders { + + private SwerveEncoders() { + throw new AssertionError("This is a utility class!"); + } + + /** + * A swerve module's absolute encoder. + * All units are in rotations. + */ + public abstract static class SwerveEncoder implements SwerveBaseHardware { + + /** + * {@code (config, turnMotor) -> SwerveEncoder} + */ + @FunctionalInterface + public static interface Ctor extends BiFunction {} + + /** + * Constructs a swerve encoder. Wraps to support simulation if applicable. + * @param ctor The encoder's constructor. + * @param config The general swerve API configuration. + * @param turnMotor The turn motor associated with the encoder's module. + */ + public static SwerveEncoder construct(Ctor ctor, SwerveConfig config, SwerveMotor turnMotor) { + SwerveEncoder encoder = ctor.apply(config, turnMotor); + if (RobotBase.isSimulation()) encoder = simulate(encoder, config, turnMotor); + return encoder; + } + + /** + * Gets the encoder's position in rotations. + */ + public abstract double getPosition(); + + /** + * Some motor controllers can be configured to use external encoders + * as a feedback device for closed-loop control. This method returns + * {@code true} if the encoder's corresponding turn motor has been + * configured to accept an absolute position when setting its closed + * loop position target, as well as return an absolute position via + * its {@code getPosition()} method. + */ + public boolean hookStatus() { + return false; + } + } + + /** + * Configures a Spark Max attached duty cycle absolute encoder. + * @param offset Offset of the magnet in rotations. + * @param inverted If the encoder is inverted. + */ + public static SwerveEncoder.Ctor sparkMaxEncoder(double offset, boolean inverted) { + return (config, turnMotor) -> { + if (!(turnMotor.getAPI() instanceof CANSparkMax)) { + throw new UnsupportedOperationException("Turn motor is not a Spark Max"); + } + + var deviceLogger = new SparkAbsoluteEncoderLogger(); + CANSparkMax sparkMax = (CANSparkMax) turnMotor.getAPI(); + SparkAbsoluteEncoder encoder = sparkMax.getAbsoluteEncoder(); + + new SparkMaxConfig() + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S5, (int) (config.odometryPeriod * 1000.0)) + .setPeriodicFramePeriod( + SparkMaxConfig.Frame.S6, + (int) (SwerveBaseHardware.TELEMETRY_CAN_PERIOD * 1000.0) + ) + .apply(sparkMax); + + new SparkAbsoluteEncoderConfig() + .setPositionConversionFactor(1.0) + .setVelocityConversionFactor(1.0) + .setInverted(inverted) + .setZeroOffset(offset) + .apply(sparkMax, encoder); + + return new SwerveEncoder() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + + @Override + public Object getAPI() { + return encoder; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, encoder, errorHandler); + } + + @Override + public void close() {} + }; + }; + } + + /** + * Configures a Spark Flex attached duty cycle absolute encoder. + * @param offset Offset of the magnet in rotations. + * @param inverted If the encoder is inverted. + */ + public static SwerveEncoder.Ctor sparkFlexEncoder(double offset, boolean inverted) { + return (config, turnMotor) -> { + if (!(turnMotor.getAPI() instanceof CANSparkFlex)) { + throw new UnsupportedOperationException("Turn motor is not a Spark Flex"); + } + + var deviceLogger = new SparkAbsoluteEncoderLogger(); + CANSparkFlex sparkFlex = (CANSparkFlex) turnMotor.getAPI(); + SparkAbsoluteEncoder encoder = sparkFlex.getAbsoluteEncoder(); + + new SparkFlexConfig() + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S5, (int) (config.odometryPeriod * 1000.0)) + .setPeriodicFramePeriod( + SparkFlexConfig.Frame.S6, + (int) (SwerveBaseHardware.TELEMETRY_CAN_PERIOD * 1000.0) + ) + .apply(sparkFlex); + + new SparkAbsoluteEncoderConfig() + .setPositionConversionFactor(1.0) + .setVelocityConversionFactor(1.0) + .setInverted(inverted) + .setZeroOffset(offset) + .apply(sparkFlex, encoder); + + return new SwerveEncoder() { + @Override + public double getPosition() { + return encoder.getPosition(); + } + + @Override + public Object getAPI() { + return encoder; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, encoder, errorHandler); + } + + @Override + public void close() {} + }; + }; + } + + /** + * Configures a {@link CANcoder}. + * @param id CAN ID of the device, as configured in Phoenix Tuner. + * @param offset Offset of the magnet in rotations. + * @param inverted If the encoder is inverted. + */ + public static SwerveEncoder.Ctor canCoder(int id, double offset, boolean inverted) { + return (config, turnMotor) -> { + var deviceLogger = new CANcoderLogger(); + CANcoder canCoder = new CANcoder(id, config.phoenixCanBus); + Mutable hookStatus = new Mutable<>(false); + + StatusSignal position = canCoder.getPosition().clone(); + StatusSignal velocity = canCoder.getVelocity().clone(); + + var canCoderConfig = new CANcoderConfiguration(); + canCoderConfig.MagnetSensor.MagnetOffset = offset; + canCoderConfig.MagnetSensor.SensorDirection = inverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + + PhoenixUtil.run(canCoder, "Apply CANcoderConfiguration", () -> + canCoder.getConfigurator().apply(canCoderConfig) + ); + + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity); + canCoder.optimizeBusUtilization(1.0 / SwerveBaseHardware.TELEMETRY_CAN_PERIOD, 0.05); + + if (turnMotor.getAPI() instanceof TalonFX) { + TalonFX talonFX = (TalonFX) turnMotor.getAPI(); + + var feedbackConfig = new FeedbackConfigs(); + feedbackConfig.FeedbackRemoteSensorID = id; + feedbackConfig.RotorToSensorRatio = config.turnGearRatio; + feedbackConfig.FeedbackSensorSource = config.phoenixPro + ? FeedbackSensorSourceValue.FusedCANcoder + : FeedbackSensorSourceValue.RemoteCANcoder; + + var closedLoopConfig = new ClosedLoopGeneralConfigs(); + closedLoopConfig.ContinuousWrap = true; + + PhoenixUtil.run(talonFX, "Apply FeedbackConfigs", () -> talonFX.getConfigurator().apply(feedbackConfig) + ); + PhoenixUtil.run(talonFX, "Apply ClosedLoopGeneralConfigs", () -> + talonFX.getConfigurator().apply(closedLoopConfig) + ); + + hookStatus.value = true; + } + + return new SwerveEncoder() { + @Override + public double getPosition() { + return BaseStatusSignal.getLatencyCompensatedValue(position, velocity); + } + + @Override + public boolean hookStatus() { + return hookStatus.get(); + } + + @Override + public Object getAPI() { + return canCoder; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, canCoder, errorHandler); + } + + @Override + public List getSignals() { + return List.of(position, velocity); + } + + @Override + public void close() { + canCoder.close(); + } + }; + }; + } + + /** + * Rudimentary encoder simulation wrapper. Follows the position of the turn motor. + * @param encoder The encoder to wrap. + * @param config The general swerve API configuration. + * @param turnMotor The turn motor associated with the encoder's module. + */ + private static SwerveEncoder simulate(SwerveEncoder encoder, SwerveConfig config, SwerveMotor turnMotor) { + return new SwerveEncoder() { + @Override + public double getPosition() { + return turnMotor.getPosition() / (hookStatus() ? 1.0 : config.turnGearRatio); + } + + @Override + public boolean hookStatus() { + return encoder.hookStatus(); + } + + @Override + public Object getAPI() { + return encoder; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + encoder.log(logger, errorHandler); + var simLogger = logger.getSubLogger(".sim"); + simLogger.log("position", getPosition()); + simLogger.log("hookStatus", hookStatus()); + } + + @Override + public List getSignals() { + return encoder.getSignals(); + } + + @Override + public boolean readError() { + return encoder.readError(); + } + + @Override + public void close() { + try { + encoder.close(); + } catch (Exception e) {} + } + }; + } +} diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java new file mode 100644 index 0000000..7d4d504 --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveIMUs.java @@ -0,0 +1,257 @@ +package org.team340.lib.swerve.hardware; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.SPI; +import java.util.List; +import java.util.function.Consumer; +import java.util.function.Function; +import org.team340.lib.logging.ADIS16470Logger; +import org.team340.lib.logging.Pigeon2Logger; +import org.team340.lib.swerve.SwerveAPI; +import org.team340.lib.swerve.config.SwerveConfig; +import org.team340.lib.swerve.hardware.SwerveIMUs.SwerveIMU.IMUSimHook; +import org.team340.lib.util.Math2; +import org.team340.lib.util.Mutable; + +/** + * Contains implementations for IMUs to be used with the {@link SwerveAPI}. + */ +public final class SwerveIMUs { + + private SwerveIMUs() { + throw new AssertionError("This is a utility class!"); + } + + /** + * A swerve IMU. + */ + public abstract static class SwerveIMU implements SwerveBaseHardware { + + /** + * {@code (config) -> SwerveIMU} + */ + @FunctionalInterface + public static interface Ctor extends Function {} + + /** + * Provides consumer to be saved and invoked periodically with the + * robot's current chassis speeds to update the simulated IMU. + */ + @FunctionalInterface + public static interface IMUSimHook extends Consumer> {} + + /** + * Constructs a swerve IMU. Wraps to support simulation if applicable. + * @param ctor The IMU's constructor. + * @param config The general swerve API configuration. + * @param simHook Hook to update the IMU if simulation is active. + */ + public static SwerveIMU construct(Ctor ctor, SwerveConfig config, IMUSimHook simHook) { + SwerveIMU imu = ctor.apply(config); + if (RobotBase.isSimulation()) imu = simulate(imu, config, simHook); + return imu; + } + + /** + * Gets the IMU's absolute yaw. + */ + public abstract Rotation2d getYaw(); + + /** + * Gets the IMU's absolute pitch. + */ + public abstract Rotation2d getPitch(); + + /** + * Gets the IMU's absolute roll. + */ + public abstract Rotation2d getRoll(); + } + + /** + * Configures an {@link ADIS16470_IMU ADIS16470 IMU}. + * @param yawAxis The axis to use for yaw. + * @param pitchAxis The axis to use for pitch. + * @param rollAxis The axis to use for roll. + * @param port The SPI port used. + * @param calibrationTime The time frame to calibrate for. + */ + public static SwerveIMU.Ctor adis16470( + IMUAxis yawAxis, + IMUAxis pitchAxis, + IMUAxis rollAxis, + SPI.Port port, + CalibrationTime calibrationTime + ) { + return config -> { + var deviceLogger = new ADIS16470Logger(); + ADIS16470_IMU adis16470 = new ADIS16470_IMU(yawAxis, pitchAxis, rollAxis, port, calibrationTime); + + return new SwerveIMU() { + @Override + public Rotation2d getYaw() { + return Rotation2d.fromDegrees(adis16470.getAngle(adis16470.getYawAxis())); + } + + @Override + public Rotation2d getPitch() { + return Rotation2d.fromDegrees(adis16470.getAngle(adis16470.getPitchAxis())); + } + + @Override + public Rotation2d getRoll() { + return Rotation2d.fromDegrees(adis16470.getAngle(adis16470.getRollAxis())); + } + + @Override + public Object getAPI() { + return adis16470; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, adis16470, errorHandler); + } + + @Override + public void close() { + adis16470.close(); + } + }; + }; + } + + /** + * Configures a {@link Pigeon2}. + * @param id CAN ID of the device, as configured in Phoenix Tuner. + */ + public static SwerveIMU.Ctor pigeon2(int id) { + return config -> { + var deviceLogger = new Pigeon2Logger(); + Pigeon2 pigeon2 = new Pigeon2(id, config.phoenixCanBus); + + StatusSignal yaw = pigeon2.getYaw().clone(); + StatusSignal pitch = pigeon2.getPitch(); + StatusSignal roll = pigeon2.getRoll(); + StatusSignal yawVelocity = pigeon2.getAngularVelocityZWorld().clone(); + StatusSignal pitchVelocity = pigeon2.getAngularVelocityXWorld(); + StatusSignal rollVelocity = pigeon2.getAngularVelocityYWorld(); + + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, yaw, yawVelocity); + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.period, pitch, roll, pitchVelocity, rollVelocity); + pigeon2.optimizeBusUtilization(1.0 / SwerveBaseHardware.TELEMETRY_CAN_PERIOD, 0.05); + + return new SwerveIMU() { + @Override + public Rotation2d getYaw() { + return Rotation2d.fromDegrees(BaseStatusSignal.getLatencyCompensatedValue(yaw, yawVelocity)); + } + + @Override + public Rotation2d getPitch() { + return Rotation2d.fromDegrees( + BaseStatusSignal.getLatencyCompensatedValue(pitch.refresh(), pitchVelocity.refresh()) + ); + } + + @Override + public Rotation2d getRoll() { + return Rotation2d.fromDegrees( + BaseStatusSignal.getLatencyCompensatedValue(roll.refresh(), rollVelocity.refresh()) + ); + } + + @Override + public Object getAPI() { + return pigeon2; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, pigeon2, errorHandler); + } + + @Override + public List getSignals() { + return List.of(yaw, yawVelocity); + } + + @Override + public void close() { + pigeon2.close(); + } + }; + }; + } + + /** + * Rudimentary IMU simulation wrapper. Calculates yaw based on the robot's angular velocity. + * @param imu The IMU to wrap. + * @param config The general swerve API configuration. + * @param Hook to update the IMU if simulation is active. + */ + private static SwerveIMU simulate(SwerveIMU imu, SwerveConfig config, IMUSimHook simHook) { + Mutable yaw = new Mutable<>(Math2.kZeroRotation2d); + simHook.accept(speeds -> + yaw.accept(yaw.get().plus(Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * config.period))) + ); + + return new SwerveIMU() { + @Override + public Rotation2d getYaw() { + return yaw.get(); + } + + @Override + public Rotation2d getPitch() { + return Math2.kZeroRotation2d; + } + + @Override + public Rotation2d getRoll() { + return Math2.kZeroRotation2d; + } + + @Override + public Object getAPI() { + return imu; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + imu.log(logger, errorHandler); + var simLogger = logger.getSubLogger(".sim"); + simLogger.log("yaw", getYaw(), Rotation2d.struct); + simLogger.log("pitch", getPitch(), Rotation2d.struct); + simLogger.log("roll", getRoll(), Rotation2d.struct); + } + + @Override + public List getSignals() { + return imu.getSignals(); + } + + @Override + public boolean readError() { + return imu.readError(); + } + + @Override + public void close() { + try { + imu.close(); + } catch (Exception e) {} + } + }; + } +} diff --git a/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java new file mode 100644 index 0000000..81cdc6e --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/hardware/SwerveMotors.java @@ -0,0 +1,548 @@ +package org.team340.lib.swerve.hardware; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; +import edu.wpi.first.epilogue.logging.DataLogger; +import edu.wpi.first.epilogue.logging.errors.ErrorHandler; +import edu.wpi.first.wpilibj.RobotBase; +import java.util.List; +import java.util.function.BiFunction; +import org.team340.lib.logging.SparkFlexLogger; +import org.team340.lib.logging.SparkMaxLogger; +import org.team340.lib.logging.TalonFXLogger; +import org.team340.lib.swerve.SwerveAPI; +import org.team340.lib.swerve.config.SwerveConfig; +import org.team340.lib.util.ctre.PhoenixUtil; +import org.team340.lib.util.rev.RelativeEncoderConfig; +import org.team340.lib.util.rev.SparkFlexConfig; +import org.team340.lib.util.rev.SparkMaxConfig; +import org.team340.lib.util.rev.SparkPIDControllerConfig; + +/** + * Contains implementations for motors to be used with the {@link SwerveAPI}. + */ +public final class SwerveMotors { + + private SwerveMotors() { + throw new AssertionError("This is a utility class!"); + } + + /** + * A swerve motor. + * All units are in rotations. + */ + public abstract static class SwerveMotor implements SwerveBaseHardware { + + /** + * {@code (config, isMoveMotor) -> SwerveMotor} + */ + @FunctionalInterface + public static interface Ctor extends BiFunction {} + + /** + * Constructs a swerve motor. Wraps to support simulation if applicable. + * @param ctor The motor's constructor. + * @param config The general swerve API configuration. + * @param isMoveMotor {@code true} if the motor is a move motor. + */ + public static SwerveMotor construct(Ctor ctor, SwerveConfig config, boolean isMoveMotor) { + SwerveMotor motor = ctor.apply(config, isMoveMotor); + if (RobotBase.isSimulation()) motor = simulate(motor, config); + return motor; + } + + /** + * Gets the motor's position in rotations. + */ + public abstract double getPosition(); + + /** + * Sets the motor's closed-loop position target. + * @param position The target position in rotations. + */ + public abstract void setPosition(double position); + + /** + * Gets the motor's velocity in rotations/second. + */ + public abstract double getVelocity(); + + /** + * Sets the motor's closed-loop velocity target. + * @param velocity The target velocity in rotations/second. + */ + public abstract void setVelocity(double velocity); + + /** + * Sets the motor's output voltage. + * @param voltage The voltage to apply. + */ + public abstract void setVoltage(double voltage); + + /** + * Re-applies PID and FF gains from the swerve config. Used + * for setting new gains after the config has been mutated. + */ + public abstract void reapplyGains(); + } + + /** + * Configures a {@link CANSparkMax Spark Max}. + * @param id CAN ID of the device, as configured in the REV Hardware Client. + * @param type The motor type connected to the controller. + * @param inverted If the motor is inverted. + */ + public static SwerveMotor.Ctor sparkMax(int id, MotorType type, boolean inverted) { + return (config, isMoveMotor) -> { + var deviceLogger = new SparkMaxLogger(); + CANSparkMax sparkMax = new CANSparkMax(id, type); + RelativeEncoder relativeEncoder = sparkMax.getEncoder(); + SparkPIDController pidController = sparkMax.getPIDController(); + int PID_SLOT = 0; + + double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; + double[] ffGains = isMoveMotor ? config.moveFF : new double[] { 0.0, 0.0 }; + + new SparkMaxConfig() + .clearFaults() + .enableVoltageCompensation(config.voltage) + .setSmartCurrentLimit((int) (isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit)) + .setIdleMode( + (isMoveMotor ? config.moveBrakeMode : config.turnBrakeMode) ? IdleMode.kBrake : IdleMode.kCoast + ) + .setInverted(inverted) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S0, (int) (config.period * 1000.0)) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S1, (int) (config.odometryPeriod * 1000.0)) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S2, (int) (config.odometryPeriod * 1000.0)) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S3, 10000) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S4, 10000) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S5, 10000) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S6, 10000) + .setPeriodicFramePeriod(SparkMaxConfig.Frame.S7, 10000) + .apply(sparkMax); + + new SparkPIDControllerConfig() + .setFeedbackDevice(relativeEncoder) + .setPID(pidGains[0], pidGains[1], pidGains[2], PID_SLOT) + .setIZone(pidGains[3], PID_SLOT) + .apply(sparkMax, pidController); + + new RelativeEncoderConfig() + .setPositionConversionFactor(1.0) + .setVelocityConversionFactor(1.0 / 60.0) + .setMeasurementPeriod(isMoveMotor ? 16 : 24) + .setAverageDepth(isMoveMotor ? 2 : 8) + .apply(sparkMax, relativeEncoder); + + return new SwerveMotor() { + @Override + public double getPosition() { + return relativeEncoder.getPosition(); + } + + @Override + public void setPosition(double position) { + pidController.setReference( + position, + CANSparkBase.ControlType.kPosition, + PID_SLOT, + 0.0, + ArbFFUnits.kVoltage + ); + } + + @Override + public double getVelocity() { + return relativeEncoder.getVelocity(); + } + + @Override + public void setVelocity(double velocity) { + pidController.setReference( + velocity, + CANSparkBase.ControlType.kVelocity, + PID_SLOT, + ffGains[0] * Math.signum(velocity) + ffGains[1] * velocity, + ArbFFUnits.kVoltage + ); + } + + @Override + public void setVoltage(double voltage) { + sparkMax.setVoltage(voltage); + } + + @Override + public void reapplyGains() { + if (isMoveMotor) { + ffGains[0] = config.moveFF[0]; + ffGains[1] = config.moveFF[1]; + } + + double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; + pidController.setP(pidGains[0], PID_SLOT); + pidController.setI(pidGains[1], PID_SLOT); + pidController.setD(pidGains[2], PID_SLOT); + pidController.setIZone(pidGains[3], PID_SLOT); + } + + @Override + public Object getAPI() { + return sparkMax; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, sparkMax, errorHandler); + } + + @Override + public boolean readError() { + return !sparkMax.getLastError().equals(REVLibError.kOk); + } + + @Override + public void close() { + sparkMax.close(); + } + }; + }; + } + + /** + * Configures a {@link CANSparkFlex Spark Flex}. + * @param id CAN ID of the device, as configured in the REV Hardware Client. + * @param type The motor type connected to the controller. + * @param inverted If the motor is inverted. + */ + public static SwerveMotor.Ctor sparkFlex(int id, MotorType type, boolean inverted) { + return (config, isMoveMotor) -> { + var deviceLogger = new SparkFlexLogger(); + CANSparkFlex sparkFlex = new CANSparkFlex(id, type); + RelativeEncoder relativeEncoder = sparkFlex.getEncoder(); + SparkPIDController pidController = sparkFlex.getPIDController(); + int PID_SLOT = 0; + + double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; + double[] ffGains = isMoveMotor ? config.moveFF : new double[] { 0.0, 0.0 }; + + new SparkFlexConfig() + .clearFaults() + .enableVoltageCompensation(config.voltage) + .setSmartCurrentLimit((int) (isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit)) + .setIdleMode( + (isMoveMotor ? config.moveBrakeMode : config.turnBrakeMode) ? IdleMode.kBrake : IdleMode.kCoast + ) + .setInverted(inverted) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S0, (int) (config.period * 1000.0)) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S1, (int) (config.odometryPeriod * 1000.0)) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S2, (int) (config.odometryPeriod * 1000.0)) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S3, 10000) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S4, 10000) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S5, 10000) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S6, 10000) + .setPeriodicFramePeriod(SparkFlexConfig.Frame.S7, 10000) + .apply(sparkFlex); + + new SparkPIDControllerConfig() + .setFeedbackDevice(relativeEncoder) + .setPID(pidGains[0], pidGains[1], pidGains[2], PID_SLOT) + .setIZone(pidGains[3], PID_SLOT) + .apply(sparkFlex, pidController); + + new RelativeEncoderConfig() + .setPositionConversionFactor(1.0) + .setVelocityConversionFactor(1.0 / 60.0) + .setMeasurementPeriod(isMoveMotor ? 32 : 80) + .setAverageDepth(isMoveMotor ? 8 : 16) + .apply(sparkFlex, relativeEncoder); + + return new SwerveMotor() { + @Override + public double getPosition() { + return relativeEncoder.getPosition(); + } + + @Override + public void setPosition(double position) { + pidController.setReference( + position, + CANSparkBase.ControlType.kPosition, + PID_SLOT, + 0.0, + ArbFFUnits.kVoltage + ); + } + + @Override + public double getVelocity() { + return relativeEncoder.getVelocity(); + } + + @Override + public void setVelocity(double velocity) { + pidController.setReference( + velocity, + CANSparkBase.ControlType.kVelocity, + PID_SLOT, + ffGains[0] * Math.signum(velocity) + ffGains[1] * velocity, + ArbFFUnits.kVoltage + ); + } + + @Override + public void setVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } + + @Override + public void reapplyGains() { + if (isMoveMotor) { + ffGains[0] = config.moveFF[0]; + ffGains[1] = config.moveFF[1]; + } + + double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; + pidController.setP(pidGains[0], PID_SLOT); + pidController.setI(pidGains[1], PID_SLOT); + pidController.setD(pidGains[2], PID_SLOT); + pidController.setIZone(pidGains[3], PID_SLOT); + } + + @Override + public Object getAPI() { + return sparkFlex; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, sparkFlex, errorHandler); + } + + @Override + public boolean readError() { + return !sparkFlex.getLastError().equals(REVLibError.kOk); + } + + @Override + public void close() { + sparkFlex.close(); + } + }; + }; + } + + /** + * Configures a {@link TalonFX}. + * @param id CAN ID of the device, as configured in Phoenix Tuner. + * @param inverted If the motor is inverted. + */ + public static SwerveMotor.Ctor talonFX(int id, boolean inverted) { + return (config, isMoveMotor) -> { + var deviceLogger = new TalonFXLogger(); + TalonFX talonFX = new TalonFX(id, config.phoenixCanBus); + int PID_SLOT = 0; + + StatusSignal position = talonFX.getPosition().clone(); + StatusSignal velocity = talonFX.getVelocity().clone(); + + boolean enableFOC = isMoveMotor ? config.phoenixMoveFOC : config.phoenixTurnFOC; + PositionVoltage positionControl = new PositionVoltage(0.0) + .withSlot(PID_SLOT) + .withEnableFOC(enableFOC) + .withUpdateFreqHz(0.0); + VelocityVoltage velocityControl = new VelocityVoltage(0.0) + .withSlot(PID_SLOT) + .withEnableFOC(enableFOC) + .withUpdateFreqHz(0.0); + VoltageOut voltageControl = new VoltageOut(0); + + double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; + double[] ffGains = isMoveMotor ? config.moveFF : new double[] { 0.0, 0.0 }; + + var talonConfig = new TalonFXConfiguration(); + + double currentLimit = isMoveMotor ? config.moveCurrentLimit : config.turnCurrentLimit; + talonConfig.CurrentLimits.StatorCurrentLimit = currentLimit; + talonConfig.CurrentLimits.StatorCurrentLimitEnable = true; + talonConfig.CurrentLimits.SupplyCurrentLimit = currentLimit; + talonConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + talonConfig.MotorOutput.Inverted = inverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + talonConfig.MotorOutput.NeutralMode = (isMoveMotor ? config.moveBrakeMode : config.turnBrakeMode) + ? NeutralModeValue.Brake + : NeutralModeValue.Coast; + + talonConfig.Slot0.kP = pidGains[0]; + talonConfig.Slot0.kI = pidGains[1]; + talonConfig.Slot0.kD = pidGains[2]; + talonConfig.Slot0.kS = ffGains[0]; + talonConfig.Slot0.kV = ffGains[1]; + + PhoenixUtil.run(talonFX, "Apply TalonFXConfiguration", () -> talonFX.getConfigurator().apply(talonConfig)); + + BaseStatusSignal.setUpdateFrequencyForAll(1.0 / config.odometryPeriod, position, velocity); + talonFX.optimizeBusUtilization(1.0 / SwerveBaseHardware.TELEMETRY_CAN_PERIOD, 0.05); + + return new SwerveMotor() { + @Override + public double getPosition() { + return BaseStatusSignal.getLatencyCompensatedValue(position, velocity); + } + + @Override + public void setPosition(double position) { + talonFX.setControl(positionControl.withPosition(position)); + } + + @Override + public double getVelocity() { + return velocity.getValue(); + } + + @Override + public void setVelocity(double velocity) { + talonFX.setControl(velocityControl.withVelocity(velocity)); + } + + @Override + public void setVoltage(double voltage) { + talonFX.setControl(voltageControl.withOutput(voltage)); + } + + @Override + public void reapplyGains() { + double[] pidGains = isMoveMotor ? config.movePID : config.turnPID; + double[] ffGains = isMoveMotor ? config.moveFF : new double[] { 0.0, 0.0 }; + + var slot0Config = new Slot0Configs(); + slot0Config.kP = pidGains[0]; + slot0Config.kI = pidGains[1]; + slot0Config.kD = pidGains[2]; + slot0Config.kS = ffGains[0]; + slot0Config.kV = ffGains[1]; + talonFX.getConfigurator().apply(slot0Config); + } + + @Override + public Object getAPI() { + return talonFX; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + deviceLogger.tryUpdate(logger, talonFX, errorHandler); + } + + @Override + public List getSignals() { + return List.of(position, velocity); + } + + @Override + public void close() { + talonFX.close(); + } + }; + }; + } + + /** + * Rudimentary motor simulation wrapper. Motor physics are not simulated, + * as the simulation assumes the motor perfectly tracks its setpoint. + * @param motor The motor to wrap. + * @param config The general swerve API configuration. + */ + private static SwerveMotor simulate(SwerveMotor motor, SwerveConfig config) { + return new SwerveMotor() { + private double position = 0.0; + private double velocity = 0.0; + + @Override + public double getPosition() { + return position; + } + + @Override + public void setPosition(double position) { + velocity = (position - this.position) / config.period; + this.position = position; + motor.setPosition(position); + } + + @Override + public double getVelocity() { + return velocity; + } + + @Override + public void setVelocity(double velocity) { + position = position + (velocity * config.period); + this.velocity = velocity; + motor.setVelocity(velocity); + } + + @Override + public void setVoltage(double voltage) { + // No-op in sim + // Intended for characterization with a real robot + velocity = 0.0; + motor.setVoltage(voltage); + } + + @Override + public void reapplyGains() { + motor.reapplyGains(); + } + + @Override + public Object getAPI() { + return motor; + } + + @Override + public void log(DataLogger logger, ErrorHandler errorHandler) { + motor.log(logger, errorHandler); + var simLogger = logger.getSubLogger(".sim"); + simLogger.log("position", getPosition()); + simLogger.log("velocity", getVelocity()); + } + + @Override + public List getSignals() { + return motor.getSignals(); + } + + @Override + public boolean readError() { + return motor.readError(); + } + + @Override + public void close() { + try { + motor.close(); + } catch (Exception e) {} + } + }; + } +} diff --git a/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java b/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java deleted file mode 100644 index 718c2fd..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.team340.lib.swerve.hardware.encoders; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; - -/** - * An absolute encoder wrapper for swerve. - * Bound to {@link SwerveModule}s. - */ -public interface SwerveEncoder { - /** - * Supported encoders. - */ - public static enum Type { - CANCODER, - SPARK_ENCODER, - } - - /** - * Gets the encoder's position in radians. - * Clamped from {@code -PI} to {@code PI}. - */ - public abstract double getPosition(); - - /** - * If the device has encountered an error while reading inputs. - */ - public default boolean readError() { - return false; - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java b/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java deleted file mode 100644 index 28b0562..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java +++ /dev/null @@ -1,47 +0,0 @@ -package org.team340.lib.swerve.hardware.encoders.vendors; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; -import org.team340.lib.util.Math2; - -/** - * CTRE CANcoder swerve wrapper. - */ -public class SwerveCANcoder implements SwerveEncoder { - - private final StatusSignal absolutePositionSignal; - - /** - * Create the CANcoder wrapper. - * @param canCoder The CANcoder to wrap. - * @param config The general swerve config. - * @param moduleConfig The encoder's module's config. - */ - public SwerveCANcoder(CANcoder canCoder, SwerveConfig config, SwerveModuleConfig moduleConfig) { - absolutePositionSignal = canCoder.getAbsolutePosition(); - - double hz = 1.0 / config.getPeriod(); - absolutePositionSignal.setUpdateFrequency(hz); - canCoder.optimizeBusUtilization(); - - CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); - - canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - canCoderConfig.MagnetSensor.MagnetOffset = moduleConfig.getEncoderOffset() / Math2.TWO_PI; - canCoderConfig.MagnetSensor.SensorDirection = - moduleConfig.getEncoderInverted() ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive; - - canCoder.getConfigurator().apply(canCoderConfig); - } - - @Override - public double getPosition() { - return absolutePositionSignal.refresh().getValue() * Math2.TWO_PI; - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveSparkEncoder.java b/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveSparkEncoder.java deleted file mode 100644 index 581693c..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveSparkEncoder.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.team340.lib.swerve.hardware.encoders.vendors; - -import com.revrobotics.CANSparkBase; -import com.revrobotics.REVLibError; -import com.revrobotics.SparkAbsoluteEncoder; -import edu.wpi.first.math.MathUtil; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; - -/** - * Wrapper for an absolute encoder attached directly to a REV Spark for swerve (Through bor, MagEncoder with adapter board, CANandcoder, etc). - */ -public class SwerveSparkEncoder implements SwerveEncoder { - - private final CANSparkBase spark; - private final SparkAbsoluteEncoder sparkEncoder; - - /** - * Create the Spark Attached Encoder wrapper. - * @param spark The Spark the encoder is attached to. - * @param sparkEncoder The encoder to wrap. - */ - public SwerveSparkEncoder(CANSparkBase spark, SparkAbsoluteEncoder sparkEncoder) { - this.spark = spark; - this.sparkEncoder = sparkEncoder; - // Config options are applied in SwerveSparkMax / SwerveSparkFlex. - } - - @Override - public double getPosition() { - return MathUtil.angleModulus(sparkEncoder.getPosition()); - } - - @Override - public boolean readError() { - return !spark.getLastError().equals(REVLibError.kOk); - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java b/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java deleted file mode 100644 index 2b7f756..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.team340.lib.swerve.hardware.imu; - -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * An IMU wrapper for swerve. - */ -public interface SwerveIMU { - /** - * Supported IMUs. - */ - public static enum Type { - ADIS16470, - PIGEON2, - } - - /** - * Gets the IMU's absolute yaw. - */ - public Rotation2d getYaw(); - - /** - * Gets the IMU's absolute pitch. - */ - public Rotation2d getPitch(); - - /** - * Gets the IMU's absolute roll. - */ - public Rotation2d getRoll(); - - /** - * Zero the pitch and roll of the IMU and set the yaw to a specified angle. - * @param yaw The yaw to zero. - */ - public void setZero(Rotation2d yaw); - - /** - * If the device has encountered an error while reading inputs. - */ - public default boolean readError() { - return false; - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMUSim.java b/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMUSim.java deleted file mode 100644 index 3c6bf5f..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMUSim.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.team340.lib.swerve.hardware.imu; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import org.team340.lib.util.Math2; - -public class SwerveIMUSim implements SwerveIMU { - - private final Timer timer = new Timer(); - - private Rotation2d yaw = Math2.ROTATION2D_0; - private double yawRadiansPerSecond = 0.0; - - /** - * Updates the simulated IMU using the robot's current chassis speeds. - * @param newSpeeds The new robot speeds. - */ - public void updateSim(ChassisSpeeds newSpeeds) { - yaw = yaw.plus(Rotation2d.fromRadians(yawRadiansPerSecond * timer.get())); - yawRadiansPerSecond = newSpeeds.omegaRadiansPerSecond; - timer.restart(); - } - - @Override - public Rotation2d getYaw() { - return yaw; - } - - @Override - public Rotation2d getPitch() { - return Math2.ROTATION2D_0; - } - - @Override - public Rotation2d getRoll() { - return Math2.ROTATION2D_0; - } - - @Override - public void setZero(Rotation2d yaw) { - this.yaw = yaw; - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwerveADIS16470.java b/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwerveADIS16470.java deleted file mode 100644 index 7289722..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwerveADIS16470.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.team340.lib.swerve.hardware.imu.vendors; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.ADIS16470_IMU; -import org.team340.lib.swerve.hardware.imu.SwerveIMU; - -/** - * ADIS 16470 swerve wrapper. - */ -public class SwerveADIS16470 implements SwerveIMU { - - private final ADIS16470_IMU adis16470; - - /** - * Create the ADIS 16470 wrapper. - * @param adis16470 The ADIS 16470 to wrap. - */ - public SwerveADIS16470(ADIS16470_IMU adis16470) { - this.adis16470 = adis16470; - } - - @Override - public Rotation2d getYaw() { - return Rotation2d.fromDegrees(adis16470.getAngle(adis16470.getYawAxis())); - } - - @Override - public Rotation2d getPitch() { - return Rotation2d.fromDegrees(adis16470.getAngle(adis16470.getPitchAxis())); - } - - @Override - public Rotation2d getRoll() { - return Rotation2d.fromDegrees(adis16470.getAngle(adis16470.getRollAxis())); - } - - @Override - public void setZero(Rotation2d yaw) { - adis16470.setGyroAngle(adis16470.getYawAxis(), yaw.getDegrees()); - adis16470.setGyroAngle(adis16470.getPitchAxis(), 0.0); - adis16470.setGyroAngle(adis16470.getRollAxis(), 0.0); - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java b/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java deleted file mode 100644 index 26b7669..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java +++ /dev/null @@ -1,57 +0,0 @@ -package org.team340.lib.swerve.hardware.imu.vendors; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Rotation2d; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.hardware.imu.SwerveIMU; - -/** - * CTRE Pigeon 2 swerve wrapper. - */ -public class SwervePigeon2 implements SwerveIMU { - - private final Pigeon2 pigeon2; - private final StatusSignal yawSignal; - private final StatusSignal pitchSignal; - private final StatusSignal rollSignal; - - /** - * Create the Pigeon 2 wrapper. - * @param pigeon2 The Pigeon 2 to wrap. - * @param config General config. - */ - public SwervePigeon2(Pigeon2 pigeon2, SwerveConfig config) { - this.pigeon2 = pigeon2; - - yawSignal = pigeon2.getYaw(); - pitchSignal = pigeon2.getPitch(); - rollSignal = pigeon2.getRoll(); - - double hz = 1.0 / config.getPeriod(); - BaseStatusSignal.setUpdateFrequencyForAll(hz, yawSignal, pitchSignal, rollSignal); - pigeon2.optimizeBusUtilization(); - } - - @Override - public Rotation2d getYaw() { - return Rotation2d.fromRadians(yawSignal.refresh().getValue()); - } - - @Override - public Rotation2d getPitch() { - return Rotation2d.fromRadians(pitchSignal.refresh().getValue()); - } - - @Override - public Rotation2d getRoll() { - return Rotation2d.fromRadians(rollSignal.refresh().getValue()); - } - - @Override - public void setZero(Rotation2d yaw) { - pigeon2.reset(); - pigeon2.setYaw(yaw.getDegrees()); - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java b/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java deleted file mode 100644 index 27d7957..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java +++ /dev/null @@ -1,63 +0,0 @@ -package org.team340.lib.swerve.hardware.motors; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; - -/** - * A motor for swerve, can be a move or turn motor. - * Bound to {@link SwerveModule}s. - */ -public interface SwerveMotor { - /** - * Supported motors. - */ - public static enum Type { - SPARK_MAX_BRUSHED, - SPARK_MAX_BRUSHLESS, - SPARK_FLEX_BRUSHED, - SPARK_FLEX_BRUSHLESS, - TALONFX, - } - - /** - * Configures the current limit of the motor. - * @param newLimit The new current limit. - */ - public abstract void configCurrentLimit(double newLimit); - - /** - * Gets the motor's velocity. - */ - public abstract double getVelocity(); - - /** - * Gets the motor's relative position. - */ - public abstract double getPosition(); - - /** - * Gets the motor's applied duty cycle. - */ - public abstract double getDutyCycle(); - - /** - * Sets the motor's closed loop target. - * If the motor is a move motor, the target is in meters/second. - * If the motor is a turn motor, the target is an unclamped position in radians. - * @param target The target. - * @param ff Arbitrary feed forward. Only applied to move motors. - */ - public abstract void setReference(double target, double ff); - - /** - * Sets the motor's output voltage. - * @param voltage The voltage to apply. - */ - public abstract void setVoltage(double voltage); - - /** - * If the device has encountered an error while reading inputs. - */ - public default boolean readError() { - return false; - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java deleted file mode 100644 index b6add93..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java +++ /dev/null @@ -1,145 +0,0 @@ -package org.team340.lib.swerve.hardware.motors.vendors; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; -import org.team340.lib.swerve.util.SwerveConversions; -import org.team340.lib.util.Math2; -import org.team340.lib.util.config.PIDConfig; -import org.team340.lib.util.config.rev.RelativeEncoderConfig; -import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig; -import org.team340.lib.util.config.rev.SparkFlexConfig; -import org.team340.lib.util.config.rev.SparkFlexConfig.Frame; -import org.team340.lib.util.config.rev.SparkPIDControllerConfig; - -/** - * Wrapper for a REV Spark Flex for swerve. - */ -public class SwerveSparkFlex implements SwerveMotor { - - private static final int PID_SLOT = 0; - - private final CANSparkFlex sparkFlex; - private final boolean isMoveMotor; - private final RelativeEncoder relativeEncoder; - private final SparkPIDController pidController; - - /** - * Create the Spark Flex wrapper. - * @param isMoveMotor If the motor is a move motor. - * @param sparkFlex The Spark Flex to wrap. - * @param encoder The absolute encoder being used. {@code null} if the motor is a move motor. - * @param config The general swerve config. - * @param moduleConfig The motor's module's config. - */ - public SwerveSparkFlex( - boolean isMoveMotor, - CANSparkFlex sparkFlex, - SwerveEncoder encoder, - SwerveConfig config, - SwerveModuleConfig moduleConfig - ) { - this.sparkFlex = sparkFlex; - this.isMoveMotor = isMoveMotor; - - relativeEncoder = sparkFlex.getEncoder(); - pidController = sparkFlex.getPIDController(); - - SwerveConversions conversions = new SwerveConversions(config); - - int periodMs = (int) (config.getPeriod() * 1000.0); - boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()) && !isMoveMotor; - double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian()); - PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); - - new SparkFlexConfig() - .clearFaults() - .enableVoltageCompensation(config.getOptimalVoltage()) - .setSmartCurrentLimit((int) (isMoveMotor ? config.getMoveCurrentLimit() : config.getTurnCurrentLimit())) - .setIdleMode( - (isMoveMotor ? moduleConfig.getMoveMotorBrake() : moduleConfig.getTurnMotorBrake()) ? IdleMode.kBrake : IdleMode.kCoast - ) - .setInverted(isMoveMotor ? moduleConfig.getMoveMotorInverted() : moduleConfig.getTurnMotorInverted()) - .setOpenLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) - .setClosedLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) - .setPeriodicFramePeriod(Frame.S0, periodMs) - .setPeriodicFramePeriod(Frame.S1, periodMs) - .setPeriodicFramePeriod(Frame.S2, periodMs) - .setPeriodicFramePeriod(Frame.S3, 10000) - .setPeriodicFramePeriod(Frame.S4, usingAttachedEncoder ? periodMs : 10000) - .setPeriodicFramePeriod(Frame.S5, usingAttachedEncoder ? periodMs : 10000) - .apply(sparkFlex); - - new SparkPIDControllerConfig() - .setFeedbackDevice(relativeEncoder) - .setPID(pidConfig.p(), pidConfig.i(), pidConfig.d(), PID_SLOT) - .setIZone(pidConfig.iZone(), PID_SLOT) - .apply(sparkFlex, pidController); - - new RelativeEncoderConfig() - .setPositionConversionFactor(conversionFactor) - .setVelocityConversionFactor(conversionFactor / 60.0) - .apply(sparkFlex, relativeEncoder); - - if (usingAttachedEncoder) { - new SparkAbsoluteEncoderConfig() - .setPositionConversionFactor(Math2.TWO_PI) - .setVelocityConversionFactor(Math2.TWO_PI / 60.0) - .setInverted(moduleConfig.getEncoderInverted()) - .setZeroOffset(moduleConfig.getEncoderOffset()) - .apply(sparkFlex, sparkFlex.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)); - } - - sparkFlex.set(0.0); - relativeEncoder.setPosition(0.0); - } - - @Override - public void configCurrentLimit(double newLimit) { - sparkFlex.setSmartCurrentLimit((int) newLimit); - } - - @Override - public double getVelocity() { - return relativeEncoder.getVelocity(); - } - - @Override - public double getPosition() { - return relativeEncoder.getPosition(); - } - - @Override - public double getDutyCycle() { - return sparkFlex.getAppliedOutput(); - } - - @Override - public void setReference(double target, double ff) { - pidController.setReference( - target, - isMoveMotor ? CANSparkFlex.ControlType.kVelocity : CANSparkFlex.ControlType.kPosition, - PID_SLOT, - ff, - ArbFFUnits.kVoltage - ); - } - - @Override - public void setVoltage(double voltage) { - sparkFlex.setVoltage(voltage); - } - - @Override - public boolean readError() { - return !sparkFlex.getLastError().equals(REVLibError.kOk); - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java deleted file mode 100644 index 2c4dc8a..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java +++ /dev/null @@ -1,145 +0,0 @@ -package org.team340.lib.swerve.hardware.motors.vendors; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkPIDController.ArbFFUnits; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; -import org.team340.lib.swerve.util.SwerveConversions; -import org.team340.lib.util.Math2; -import org.team340.lib.util.config.PIDConfig; -import org.team340.lib.util.config.rev.RelativeEncoderConfig; -import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig; -import org.team340.lib.util.config.rev.SparkMaxConfig; -import org.team340.lib.util.config.rev.SparkMaxConfig.Frame; -import org.team340.lib.util.config.rev.SparkPIDControllerConfig; - -/** - * Wrapper for a REV Spark Max for swerve. - */ -public class SwerveSparkMax implements SwerveMotor { - - private static final int PID_SLOT = 0; - - private final CANSparkMax sparkMax; - private final boolean isMoveMotor; - private final RelativeEncoder relativeEncoder; - private final SparkPIDController pidController; - - /** - * Create the Spark Max wrapper. - * @param isMoveMotor If the motor is a move motor. - * @param sparkMax The Spark Max to wrap. - * @param encoder The absolute encoder being used. {@code null} if the motor is a move motor. - * @param config The general swerve config. - * @param moduleConfig The motor's module's config. - */ - public SwerveSparkMax( - boolean isMoveMotor, - CANSparkMax sparkMax, - SwerveEncoder encoder, - SwerveConfig config, - SwerveModuleConfig moduleConfig - ) { - this.sparkMax = sparkMax; - this.isMoveMotor = isMoveMotor; - - relativeEncoder = sparkMax.getEncoder(); - pidController = sparkMax.getPIDController(); - - SwerveConversions conversions = new SwerveConversions(config); - - int periodMs = (int) (config.getPeriod() * 1000.0); - boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()) && !isMoveMotor; - double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian()); - PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); - - new SparkMaxConfig() - .clearFaults() - .enableVoltageCompensation(config.getOptimalVoltage()) - .setSmartCurrentLimit((int) (isMoveMotor ? config.getMoveCurrentLimit() : config.getTurnCurrentLimit())) - .setIdleMode( - (isMoveMotor ? moduleConfig.getMoveMotorBrake() : moduleConfig.getTurnMotorBrake()) ? IdleMode.kBrake : IdleMode.kCoast - ) - .setInverted(isMoveMotor ? moduleConfig.getMoveMotorInverted() : moduleConfig.getTurnMotorInverted()) - .setOpenLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) - .setClosedLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) - .setPeriodicFramePeriod(Frame.S0, periodMs) - .setPeriodicFramePeriod(Frame.S1, periodMs) - .setPeriodicFramePeriod(Frame.S2, periodMs) - .setPeriodicFramePeriod(Frame.S3, 10000) - .setPeriodicFramePeriod(Frame.S4, usingAttachedEncoder ? periodMs : 10000) - .setPeriodicFramePeriod(Frame.S5, usingAttachedEncoder ? periodMs : 10000) - .apply(sparkMax); - - new SparkPIDControllerConfig() - .setFeedbackDevice(relativeEncoder) - .setPID(pidConfig.p(), pidConfig.i(), pidConfig.d(), PID_SLOT) - .setIZone(pidConfig.iZone(), PID_SLOT) - .apply(sparkMax, pidController); - - new RelativeEncoderConfig() - .setPositionConversionFactor(conversionFactor) - .setVelocityConversionFactor(conversionFactor / 60.0) - .apply(sparkMax, relativeEncoder); - - if (usingAttachedEncoder) { - new SparkAbsoluteEncoderConfig() - .setPositionConversionFactor(Math2.TWO_PI) - .setVelocityConversionFactor(Math2.TWO_PI / 60.0) - .setInverted(moduleConfig.getEncoderInverted()) - .setZeroOffset(moduleConfig.getEncoderOffset()) - .apply(sparkMax, sparkMax.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)); - } - - sparkMax.set(0.0); - relativeEncoder.setPosition(0.0); - } - - @Override - public void configCurrentLimit(double newLimit) { - sparkMax.setSmartCurrentLimit((int) newLimit); - } - - @Override - public double getVelocity() { - return relativeEncoder.getVelocity(); - } - - @Override - public double getPosition() { - return relativeEncoder.getPosition(); - } - - @Override - public double getDutyCycle() { - return sparkMax.getAppliedOutput(); - } - - @Override - public void setReference(double target, double ff) { - pidController.setReference( - target, - isMoveMotor ? CANSparkMax.ControlType.kVelocity : CANSparkMax.ControlType.kPosition, - PID_SLOT, - ff, - ArbFFUnits.kVoltage - ); - } - - @Override - public void setVoltage(double voltage) { - sparkMax.setVoltage(voltage); - } - - @Override - public boolean readError() { - return !sparkMax.getLastError().equals(REVLibError.kOk); - } -} diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java deleted file mode 100644 index 440a4d0..0000000 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java +++ /dev/null @@ -1,125 +0,0 @@ -package org.team340.lib.swerve.hardware.motors.vendors; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; -import org.team340.lib.swerve.util.SwerveConversions; -import org.team340.lib.util.Math2; -import org.team340.lib.util.config.PIDConfig; - -// TODO iZone - -/** - * Wrapper for a Talon FX for swerve. - */ -public class SwerveTalonFX implements SwerveMotor { - - private static final int PID_SLOT = 0; - - private final TalonFX talonFX; - private final boolean isMoveMotor; - private final StatusSignal velocitySignal; - private final StatusSignal positionSignal; - private final StatusSignal dutyCycleSignal; - private final double conversionFactor; - - /** - * Create the Talon FX wrapper. - * @param isMoveMotor If the motor is a move motor. - * @param talonFX The Talon FX to wrap. - * @param config The general swerve config. - * @param moduleConfig The motor's module's config. - */ - public SwerveTalonFX(boolean isMoveMotor, TalonFX talonFX, SwerveConfig config, SwerveModuleConfig moduleConfig) { - this.talonFX = talonFX; - this.isMoveMotor = isMoveMotor; - - SwerveConversions conversions = new SwerveConversions(config); - conversionFactor = isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian(); - - TalonFXConfiguration fxConfig = new TalonFXConfiguration(); - - fxConfig.CurrentLimits.SupplyCurrentLimit = isMoveMotor ? config.getMoveCurrentLimit() : config.getTurnCurrentLimit(); - fxConfig.CurrentLimits.SupplyCurrentLimitEnable = false; - fxConfig.CurrentLimits.StatorCurrentLimit = isMoveMotor ? config.getMoveCurrentLimit() : config.getTurnCurrentLimit(); - fxConfig.CurrentLimits.StatorCurrentLimitEnable = true; - - fxConfig.MotorOutput.NeutralMode = - (isMoveMotor ? moduleConfig.getMoveMotorBrake() : moduleConfig.getTurnMotorBrake()) - ? NeutralModeValue.Brake - : NeutralModeValue.Coast; - fxConfig.MotorOutput.Inverted = - (isMoveMotor ? moduleConfig.getMoveMotorInverted() : moduleConfig.getTurnMotorInverted()) - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - - fxConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate(); - fxConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate(); - - PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); - fxConfig.Slot0.kP = pidConfig.p(); - fxConfig.Slot0.kI = pidConfig.i(); - fxConfig.Slot0.kD = pidConfig.d(); - fxConfig.Slot0.kS = 0.0; - fxConfig.Slot0.kV = 0.0; - - velocitySignal = talonFX.getVelocity(); - positionSignal = talonFX.getPosition(); - dutyCycleSignal = talonFX.getDutyCycle(); - - double hz = 1.0 / config.getPeriod(); - BaseStatusSignal.setUpdateFrequencyForAll(hz, velocitySignal, positionSignal, dutyCycleSignal); - talonFX.optimizeBusUtilization(); - - talonFX.clearStickyFaults(); - talonFX.getConfigurator().apply(fxConfig); - - talonFX.set(0.0); - talonFX.setPosition(0.0); - } - - @Override - public void configCurrentLimit(double newLimit) { - talonFX.getConfigurator().apply(new CurrentLimitsConfigs().withSupplyCurrentLimit(newLimit).withStatorCurrentLimit(newLimit)); - } - - @Override - public double getVelocity() { - return velocitySignal.refresh().getValue() / conversionFactor; - } - - @Override - public double getPosition() { - return positionSignal.refresh().getValue() / conversionFactor; - } - - @Override - public double getDutyCycle() { - return dutyCycleSignal.refresh().getValue(); - } - - @Override - public void setReference(double target, double ff) { - if (isMoveMotor) { - VelocityVoltage request = new VelocityVoltage(target * conversionFactor).withSlot(PID_SLOT).withFeedForward(ff); - talonFX.setControl(request); - } else { - PositionVoltage request = new PositionVoltage(target * 1.0 / Math2.TWO_PI).withSlot(PID_SLOT).withFeedForward(ff); - talonFX.setControl(request); - } - } - - @Override - public void setVoltage(double voltage) { - talonFX.setVoltage(voltage); - } -} diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveConversions.java b/src/main/java/org/team340/lib/swerve/util/SwerveConversions.java deleted file mode 100644 index c03c4dd..0000000 --- a/src/main/java/org/team340/lib/swerve/util/SwerveConversions.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.team340.lib.swerve.util; - -import edu.wpi.first.math.util.Units; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.util.Math2; - -/** - * Conversions for config defined measurements. - */ -public record SwerveConversions(double moveRotationsPerMeter, double turnRotationsPerRadian) { - /** - * Generate conversions from a config. - * @param config The config to generate from. - */ - public SwerveConversions(SwerveConfig config) { - this( - config.getMoveGearRatio() / (Units.inchesToMeters(config.getWheelDiameterInches()) * Math.PI), - config.getTurnGearRatio() / Math2.TWO_PI - ); - } -} diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java b/src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java deleted file mode 100644 index 3e66d46..0000000 --- a/src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java +++ /dev/null @@ -1,124 +0,0 @@ -package org.team340.lib.swerve.util; - -import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.wpilibj.Notifier; -import java.util.ArrayDeque; -import java.util.Deque; -import java.util.Iterator; -import java.util.concurrent.locks.ReentrantLock; -import org.team340.lib.swerve.SwerveModule; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.swerve.hardware.imu.SwerveIMU; - -/** - * Samples swerve hardware on a separate thread running at a higher frequency than the main robot loop to provide more accurate odometry. - * Call {@link SwerveOdometryThread#update(SwerveDrivePoseEstimator)} to update the pose estimator with sampled measurements. - */ -public class SwerveOdometryThread { - - private class Sample { - - public double timestamp; - public Rotation2d yaw; - public SwerveModulePosition[] modulePositions; - } - - private final SwerveModule[] modules; - private final SwerveIMU imu; - - private final Deque samples = new ArrayDeque<>(); - - private final Notifier thread; - private final double period; - private final int moduleCount; - private final ReentrantLock mutex = new ReentrantLock(); - - private int readErrors = 0; - - /** - * Creates the odometry thread. - * @param modules The robot's swerve modules. - * @param imu The IMU. - * @param config The general swerve config. - */ - public SwerveOdometryThread(SwerveModule[] modules, SwerveIMU imu, SwerveConfig config) { - this.modules = modules; - this.imu = imu; - - period = config.getOdometryPeriod(); - moduleCount = modules.length; - if (period != config.getPeriod()) { - thread = new Notifier(this::recordSample); - thread.setName("Swerve Odometry"); - } else { - thread = null; - } - } - - /** - * Starts the thread. - */ - public void start() { - if (thread != null) { - thread.startPeriodic(period); - } - } - - /** - * Updates the pose estimator with recorded odometry samples. - * @param poseEstimator The pose estimator to update. - */ - public void update(SwerveDrivePoseEstimator poseEstimator) { - if (thread == null) recordSample(); - - try { - mutex.lock(); - Iterator sampleIterator = samples.iterator(); - while (sampleIterator.hasNext()) { - Sample sample = sampleIterator.next(); - poseEstimator.updateWithTime(sample.timestamp, sample.yaw, sample.modulePositions); - } - } finally { - samples.clear(); - mutex.unlock(); - } - } - - /** - * Returns the number of hardware read errors encountered by the thread. - */ - public int readErrorCount() { - return readErrors; - } - - /** - * Records a sample. - */ - private void recordSample() { - try { - mutex.lock(); - Sample sample = new Sample(); - int sampleErrors = 0; - - sample.yaw = imu.getYaw(); - if (imu.readError()) sampleErrors++; - - sample.modulePositions = new SwerveModulePosition[moduleCount]; - for (int i = 0; i < moduleCount; i++) { - sample.modulePositions[i] = modules[i].getModulePosition(); - sampleErrors += modules[i].readErrorCount(); - } - - readErrors += sampleErrors; - if (sampleErrors == 0) { - sample.timestamp = MathSharedStore.getTimestamp(); - samples.add(sample); - } - } finally { - mutex.unlock(); - } - } -} diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java b/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java deleted file mode 100644 index ac4c3d4..0000000 --- a/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java +++ /dev/null @@ -1,457 +0,0 @@ -package org.team340.lib.swerve.util; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import org.team340.lib.swerve.config.SwerveConfig; -import org.team340.lib.util.Math2; - -/** - * A controller that applies constraints onto the speeds applied to swerve modules to account for physical limitations of the robot. - * Adapted from {@link https://github.com/Team254/FRC-2022-Public/blob/main/src/main/java/com/team254/lib/swerve/SwerveSetpointGenerator.java}. - */ -public class SwerveRatelimiter { - - /** - * A state. - */ - public static record SwerveState(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {} - - private final SwerveConfig config; - private final SwerveDriveKinematics kinematics; - private final int moduleCount; - private SwerveState lastState; - - /** - * Create the ratelimiter. - * @param config The general swerve config. - * @param kinematics The kinematics instance used by the swerve subsystem. - * @param initialModuleStates The initial states of the swerve modules. - */ - public SwerveRatelimiter(SwerveConfig config, SwerveDriveKinematics kinematics, SwerveModuleState[] initialModuleStates) { - this.config = config; - this.kinematics = kinematics; - this.lastState = new SwerveState(kinematics.toChassisSpeeds(initialModuleStates), initialModuleStates); - moduleCount = initialModuleStates.length; - } - - /** - * Gets the ratelimiter's last state. - */ - public SwerveState getLastState() { - return lastState; - } - - /** - * Sets the ratelimiter's last state. - * @param chassisSpeeds The speeds for the state. - * @param moduleStates The module states to use. - */ - public void setLastState(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) { - setLastState(new SwerveState(chassisSpeeds, moduleStates)); - } - - /** - * Sets the ratelimiter's last state. - * @param state The state to use. - */ - public void setLastState(SwerveState state) { - lastState = state; - } - - /** - * Calculates a new state. - * @param desiredSpeeds The new desired chassis speeds. - * @return The new state. - */ - public SwerveState calculate(ChassisSpeeds desiredSpeeds) { - // - Determine the desired swerve module states. - SwerveModuleState[] desiredModuleStates = kinematics.toSwerveModuleStates(desiredSpeeds); - - // - Preliminary wheel speed desaturation based on the configured max robot velocity. - SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, config.getVelocity()); - - // - Determine desired speeds based on desaturated module states. - desiredSpeeds = kinematics.toChassisSpeeds(desiredModuleStates); - - // - Declare a list for modules that require special behavior for their - // heading outside of the kinematics calculated heading. - List> overrideHeading = new ArrayList<>(moduleCount); - - // - If the desired speed is a stop. - boolean desireStop = Math2.twist2dEpsilonEquals( - new Twist2d(desiredSpeeds.vxMetersPerSecond, desiredSpeeds.vyMetersPerSecond, desiredSpeeds.omegaRadiansPerSecond), - Math2.TWIST2D_0 - ); - // (Special behavior for module rotation when the desired chassis speeds is 0) - // If the desired state has no movement: - if (desireStop) { - // - Find the last state's total velocity across the X and Y axis. - Translation2d lastV = new Translation2d(lastState.chassisSpeeds.vxMetersPerSecond, lastState.chassisSpeeds.vyMetersPerSecond); - - // - Determine if the last state had a velocity of 0. - boolean wasMoving = !Math2.epsilonEquals(lastV.getNorm(), 0.0); - - // For every module: - for (int i = 0; i < moduleCount; ++i) { - // - If the robot was moving, set the desired angle to the direction of the - // robot's current lateral motion. This is to prevent the robot from veering - // if it is stopping from a state of rotating while translating. - desiredModuleStates[i].angle = wasMoving ? lastV.getAngle() : lastState.moduleStates[i].angle; - - // - Translational speed is still 0. - desiredModuleStates[i].speedMetersPerSecond = 0.0; - - // - Add the heading to overrides. - overrideHeading.add(Optional.of(desiredModuleStates[i].angle)); - } - } - - // - Declare arrays to be hydrated with calculated module velocities and headings. - double[] modulesLastVx = new double[moduleCount]; - double[] modulesLastVy = new double[moduleCount]; - Rotation2d[] modulesLastHeading = new Rotation2d[moduleCount]; - double[] modulesDesiredVx = new double[moduleCount]; - double[] modulesDesiredVy = new double[moduleCount]; - Rotation2d[] modulesDesiredHeading = new Rotation2d[moduleCount]; - - // - Declare a tripwire to determine if all modules are flipping with the - // desired speeds (indicative of moving in the opposite direction within - // 90 degrees). If this remains true, it is probably faster to start - // over with a velocity of 0. - boolean allFlip = true; - - // (Special behavior for translation direction changes in the - // opposite direction of current movement within 90 degrees) - // For every module: - for (int i = 0; i < moduleCount; ++i) { - // - Find the last module velocity and heading using the last module states. - modulesLastVx[i] = lastState.moduleStates[i].angle.getCos() * lastState.moduleStates[i].speedMetersPerSecond; - modulesLastVy[i] = lastState.moduleStates[i].angle.getSin() * lastState.moduleStates[i].speedMetersPerSecond; - modulesLastHeading[i] = lastState.moduleStates[i].angle; - - // If the last module velocity was in reverse: - // - Flip the heading. - if (lastState.moduleStates[i].speedMetersPerSecond < 0.0) { - modulesLastHeading[i] = Rotation2d.fromRadians(MathUtil.angleModulus(modulesLastHeading[i].getRadians() + Math.PI)); - } - - // - Find the desired module velocity and heading using the desired module states. - modulesDesiredVx[i] = desiredModuleStates[i].angle.getCos() * desiredModuleStates[i].speedMetersPerSecond; - modulesDesiredVy[i] = desiredModuleStates[i].angle.getSin() * desiredModuleStates[i].speedMetersPerSecond; - modulesDesiredHeading[i] = desiredModuleStates[i].angle; - - // If the desired module velocity is in reverse: - // - Flip the heading. - if (desiredModuleStates[i].speedMetersPerSecond < 0.0) { - modulesDesiredHeading[i] = Rotation2d.fromRadians(MathUtil.angleModulus(modulesDesiredHeading[i].getRadians() + Math.PI)); - } - - // If the desired state doesn't require a flip and the allFlip tripwire has not been hit: - // - Set the allFlip tripwire to false. - if (allFlip && Math.abs(modulesLastHeading[i].times(-1.0).rotateBy(modulesDesiredHeading[i]).getRadians()) < Math2.HALF_PI) { - allFlip = false; - } - } - - // If the allFlip tripwire has been hit, and the last and desired state contains movement: - // - Start over with a desired speed of 0 (this should be faster). - if ( - allFlip && - !desireStop && - !Math2.twist2dEpsilonEquals( - new Twist2d( - lastState.chassisSpeeds.vxMetersPerSecond, - lastState.chassisSpeeds.vyMetersPerSecond, - lastState.chassisSpeeds.omegaRadiansPerSecond - ), - Math2.TWIST2D_0 - ) - ) { - return calculate(Math2.CHASSIS_SPEEDS_0); - } - - // - Declare the velocity delta scalar. This is an interpolation between the desired speeds and the last state's speeds. It - // will be lowered based on kinematic constraints. Applied to the final chassis speeds and module states before - // they are overridden. - double dvScalar = 1.0; - - // - Find the maximum feasible delta in the a module's heading. - // Acceleration is assumed to be a non-factor. - double maxHeadingDelta = config.getPeriod() * config.getModuleRotationalVelocity(); - - // (Limit the velocity delta based on module heading constraints) - // If we aren't stopping: - if (!desireStop) { - // For every module: - for (int i = 0; i < moduleCount; ++i) { - // - Add an empty Optional to the override heading array (this is to ensure null values are not retrieved). - overrideHeading.add(Optional.empty()); - - // If the last state's velocity is 0: - if (Math2.epsilonEquals(lastState.moduleStates[i].speedMetersPerSecond, 0.0)) { - // If the desired state's velocity is 0: - // - Add the module to the overrideHeading list with the last state's heading and skip the rest of the loop. - if (Math2.epsilonEquals(desiredModuleStates[i].speedMetersPerSecond, 0.0)) { - overrideHeading.set(i, Optional.of(lastState.moduleStates[i].angle)); - continue; - } - - // - Find the rotational delta between the last and desired module heading. - Rotation2d rotDelta = lastState.moduleStates[i].angle.times(-1.0).rotateBy(desiredModuleStates[i].angle); - - // If the rotational delta is greater than PI / 2 (90 degrees) and should be optimized: - // - Rotate the delta to an optimized position. - if (flipHeading(rotDelta)) rotDelta = rotDelta.rotateBy(Math2.ROTATION2D_PI); - - // If the desired state will take a single periodic loop to achieve: - // - Add the module to the overrideHeading list with the desired state's - // heading and skip the rest of the loop. Calculating a reduction for the - // velocity delta shouldn't result in a meaningful change. - // Else: - // - Add the module to the overrideHeading list with a - // heading achievable in the next periodic loop. - // - Velocity change is now 0, as we wait for all modules to move before - // accelerating from a stop. This prevents the "wiggle" typically seen in - // swerve drive after accelerating from a dead stop. - if (Math.abs(rotDelta.getRadians()) / maxHeadingDelta <= 1.0) { - overrideHeading.set(i, Optional.of(desiredModuleStates[i].angle)); - continue; - } else { - overrideHeading.set( - i, - Optional.of( - lastState.moduleStates[i].angle.rotateBy( - Rotation2d.fromRadians(Math.signum(rotDelta.getRadians()) * maxHeadingDelta) - ) - ) - ); - dvScalar = 0.0; - continue; - } - } - - // - If the velocity delta scalar is 0, skip the rest of the loop. - if (dvScalar == 0.0) continue; - - // - Find the module's velocity delta scalar from its heading delta (refer to the method below on how this is derived). - double moduleDvScalar = getModuleHeadingVDS( - modulesLastVx[i], - modulesLastVy[i], - modulesLastHeading[i].getRadians(), - modulesDesiredVx[i], - modulesDesiredVy[i], - modulesDesiredHeading[i].getRadians(), - maxHeadingDelta - ); - - // - Set the scalar to the minimum of its current value and the - // calculated maximum feasible scalar from the module. - dvScalar = Math.min(dvScalar, moduleDvScalar); - } - } - - // (Limit the velocity delta scalar based on module velocity constraints) - // For every module: - for (int i = 0; i < moduleCount; ++i) { - // If the scalar is already 0: - // - Exit. - if (dvScalar == 0.0) break; - - // - Calculate the desaturated x and y velocity based on the - // current velocity delta scalar. - double desiredModuleVx = dvScalar == 1.0 - ? modulesDesiredVx[i] - : ((modulesDesiredVx[i] - modulesLastVx[i]) * dvScalar) + modulesLastVx[i]; - double desiredModuleVy = dvScalar == 1.0 - ? modulesDesiredVy[i] - : ((modulesDesiredVy[i] - modulesLastVy[i]) * dvScalar) + modulesLastVy[i]; - - // - Find the module's velocity delta scalar from its velocity delta (refer to the method below on how this is derived). - double moduleDvScalar = - dvScalar * - getModuleVelocityVDS( - modulesLastVx[i], - modulesLastVy[i], - desiredModuleVx, - desiredModuleVy, - config.getPeriod() * config.getAcceleration() - ); - - // - Set the scalar to the minimum of its current value and the - // calculated maximum feasible scalar from the module. - dvScalar = Math.min(dvScalar, moduleDvScalar); - } - - // - Declare the constrained speeds using the velocity delta scalar. - ChassisSpeeds constrainedSpeeds = new ChassisSpeeds( - lastState.chassisSpeeds.vxMetersPerSecond + - (dvScalar * (desiredSpeeds.vxMetersPerSecond - lastState.chassisSpeeds.vxMetersPerSecond)), - lastState.chassisSpeeds.vyMetersPerSecond + - (dvScalar * (desiredSpeeds.vyMetersPerSecond - lastState.chassisSpeeds.vyMetersPerSecond)), - lastState.chassisSpeeds.omegaRadiansPerSecond + - (dvScalar * (desiredSpeeds.omegaRadiansPerSecond - lastState.chassisSpeeds.omegaRadiansPerSecond)) - ); - - // - Compute module states from constrained speeds.. - SwerveModuleState[] constrainedStates = kinematics.toSwerveModuleStates(constrainedSpeeds); - - // (Module overrides / "global" heading optimization) - // For every module: - for (int i = 0; i < moduleCount; ++i) { - // - Get the module's override. - Optional couldOverride = overrideHeading.get(i); - - // If the module is overridden: - if (couldOverride.isPresent()) { - // - Get the override. - Rotation2d override = couldOverride.get(); - - // - If the override is a flipped heading, reverse the module's speed. - if (flipHeading(constrainedStates[i].angle.times(-1.0).rotateBy(override))) { - constrainedStates[i].speedMetersPerSecond *= -1.0; - } - - // - Set the module to the overridden angle. - constrainedStates[i].angle = override; - } - - // - Find the module's change in heading. - Rotation2d deltaRotation = lastState.moduleStates[i].angle.times(-1.0).rotateBy(constrainedStates[i].angle); - - // If the change in heading is greater than 90 degrees: - if (flipHeading(deltaRotation)) { - // - Optimize the heading and reverse the module's speed. - constrainedStates[i].angle = - Rotation2d.fromRadians(MathUtil.angleModulus(constrainedStates[i].angle.getRadians() + Math.PI)); - constrainedStates[i].speedMetersPerSecond *= -1.0; - } - } - - // - Create the new state. - SwerveState newState = new SwerveState(constrainedSpeeds, constrainedStates); - // - Set the last state. - setLastState(newState); - // - Return the new state. - return newState; - } - - /** - * If a heading should be optimized. - * @param heading The heading to check. - */ - private boolean flipHeading(Rotation2d heading) { - return Math.abs(heading.getRadians()) > Math2.HALF_PI; - } - - /** - * Calculates the velocity delta scalar (percent of difference between last and desired state) from a module's heading. - * @param lastVx Last state {@code x} velocity. - * @param lastVy Last state {@code y} velocity. - * @param lastHeading Last state heading. - * @param desiredVx Desired {@code x} velocity. - * @param desiredVy Desired {@code y} velocity. - * @param desiredHeading Desired heading. - * @param maxRotationalVelocityStep The maximum allowed difference in rotational velocity in a periodic loop iteration. - */ - private double getModuleHeadingVDS( - double lastVx, - double lastVy, - double lastHeading, - double desiredVx, - double desiredVy, - double desiredHeading, - double maxRotationalVelocityStep - ) { - // - Make sure the headings are optimized. - desiredHeading = Math2.wrapAbout(lastHeading, desiredHeading); - - // - Find the difference between the last and desired headings. - double diff = desiredHeading - lastHeading; - - // - If the last and desired heading is achievable in one - // periodic loop, return 1 as no interpolation is needed. - if (Math.abs(diff) <= maxRotationalVelocityStep) return 1.0; - - // - Find an achievable heading. - double achievableHeading = lastHeading + Math.signum(diff) * maxRotationalVelocityStep; - - // - Describes the following function: - // - x is the module's X velocity relative to the robot. - // - y is the module's Y velocity relative to the robot. - // Returns: - // - Get the module's velocity as a heading. - // - Localize the robot's velocity heading around the last module heading. - // - Subtract the achievable heading. - Math2.Parametric func = (x, y) -> Math2.wrapAbout(lastHeading, Math.atan2(y, x)) - achievableHeading; - - // - Solves the above function for its root within the bounds of the last - // and desired chassis speeds. Colloquially, this solves for a percentage to - // reduce the robot's velocity by to ensure that undesirable behavior does not - // arise from modules pulling the robot in unpredictable directions during - // the heading transitional period. - return Math2.findRoot( - func, - lastVx, - lastVy, - lastHeading - achievableHeading, - desiredVx, - desiredVy, - desiredHeading - achievableHeading, - 6 - ); - } - - /** - * Calculates the velocity delta scalar (percent of difference between last and desired state) from a module's velocity. - * @param lastVx Last state {@code x} velocity. - * @param lastVy Last state {@code y} velocity. - * @param desiredVx Desired {@code x} velocity. - * @param desiredVy Desired {@code y} velocity. - * @param maxVelocityStep The maximum allowed difference in velocity in a periodic loop iteration. - */ - private double getModuleVelocityVDS(double lastVx, double lastVy, double desiredVx, double desiredVy, double maxVelocityStep) { - // - Compute the last and desired translational velocities. - double lastNorm = Math.hypot(lastVx, lastVy); - double desiredNorm = Math.hypot(desiredVx, desiredVy); - - // - Find the difference between the last and desired headings. - double diff = desiredNorm - lastNorm; - - // - If the last and desired velocity is achievable in one - // periodic loop, return 1 as no interpolation is needed. - if (Math.abs(diff) <= maxVelocityStep) return 1.0; - - // - Find an achievable velocity. - double achievableVelocity = lastNorm + Math.signum(diff) * maxVelocityStep; - - // - Describes the following function: - // - x is the module's X velocity relative to the robot. - // - y is the module's Y velocity relative to the robot. - // Returns: - // - Find the total velocity. - // - Take the difference of the achievable velocity. - Math2.Parametric func = (x, y) -> Math.hypot(x, y) - achievableVelocity; - - // - Solves the above function for its root within the bounds of the - // last and desired velocity. This is used to prevent infeasible - // acceleration while translating on a per module basis. - return Math2.findRoot( - func, - lastVx, - lastVy, - lastNorm - achievableVelocity, - desiredVx, - desiredVy, - desiredNorm - achievableVelocity, - 10 - ); - } -} diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveVisualizer.java b/src/main/java/org/team340/lib/swerve/util/SwerveVisualizer.java deleted file mode 100644 index b937008..0000000 --- a/src/main/java/org/team340/lib/swerve/util/SwerveVisualizer.java +++ /dev/null @@ -1,194 +0,0 @@ -package org.team340.lib.swerve.util; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import java.util.function.BiConsumer; -import java.util.function.Supplier; - -/** - * Helpers for sending swerve visualizations to NT. - */ -public class SwerveVisualizer implements Sendable { - - private static final double[] EMPTY_DOUBLE = new double[0]; - - private final Supplier robot; - private final Supplier moduleStates; - private final Supplier desiredModuleStates; - - private Supplier target = () -> EMPTY_DOUBLE; - private double[] trajectory = EMPTY_DOUBLE; - private double[] trajectoryTarget = EMPTY_DOUBLE; - private double[] visionMeasurements = EMPTY_DOUBLE; - private double[] visionTargets = EMPTY_DOUBLE; - - /** - * Create the visualizer. - * @param robot A supplier that returns the robot's position. - * @param moduleStates A supplier that returns module states. - * @param desiredModuleStates A supplier that returns desired module states. - */ - public SwerveVisualizer( - Supplier robot, - Supplier moduleStates, - Supplier desiredModuleStates - ) { - this.robot = robot; - this.moduleStates = moduleStates; - this.desiredModuleStates = desiredModuleStates; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.addDoubleArrayProperty("robot", () -> pose2d(robot.get()), null); - builder.addDoubleArrayProperty("moduleStates", () -> moduleStates(moduleStates.get()), null); - builder.addDoubleArrayProperty("desiredModuleStates", () -> moduleStates(desiredModuleStates.get()), null); - builder.addDoubleArrayProperty("target", () -> target.get(), null); - builder.addDoubleArrayProperty("trajectory", () -> trajectory, null); - builder.addDoubleArrayProperty("trajectoryTarget", () -> trajectoryTarget, null); - builder.addDoubleArrayProperty("visionMeasurements", () -> visionMeasurements, null); - builder.addDoubleArrayProperty("visionTargets", () -> visionTargets, null); - } - - /** - * Adds a trajectory. - * @param trajectory The trajectory to be added. - * @return A consumer that accepts the current state of the trajectory's path following. - * The first argument is a boolean that should be {@code true} when the trajectory is being followed. - * The second argument is a pose that should be the trajectory's current target. Ignored if the first argument is {@code false}. - */ - public BiConsumer addTrajectory(Pose2d[] trajectory) { - double[] serialized = pose2d(trajectory); - BiConsumer consumer = (following, target) -> { - if (following) { - this.trajectory = serialized; - trajectoryTarget = pose2d(target); - } else if (this.trajectory == serialized) { - this.trajectory = EMPTY_DOUBLE; - trajectoryTarget = EMPTY_DOUBLE; - } - }; - return consumer; - } - - /** - * Updates visualizations for vision measurements. - * @param measurements Estimated poses returned from vision measurements. - * @param targets Vision targets. - */ - public void updateVision(Pose2d[] measurements, Pose3d[] targets) { - visionMeasurements = pose2d(measurements); - visionTargets = pose3d(targets); - } - - /** - * Disables the target visualization. - */ - public void removeTarget() { - target = () -> EMPTY_DOUBLE; - } - - /** - * Updates the target visualization. - * @param pose A target {@link Pose2d}. - */ - public void updateTarget(Pose2d pose) { - double[] array = pose2d(pose); - target = () -> array; - } - - /** - * Updates the target visualization. - * @param rotation A target {@link Rotation2d} for the robot to face. - */ - public void updateTarget(Rotation2d rotation) { - target = - () -> { - Pose2d pose = robot.get(); - return new double[] { pose.getX(), pose.getY(), rotation.getRadians() }; - }; - } - - /** - * Updates the target visualization. - * @param rotation A target rotation in radians (CCW positive) for the robot to face. - */ - public void updateTarget(double rotation) { - target = - () -> { - Pose2d pose = robot.get(); - return new double[] { pose.getX(), pose.getY(), rotation }; - }; - } - - /** - * Converts module states into a double array. - * Returns in the following format (heading is in radians): - * 
    -     * [
-     *   heading0, velocity0,
-     *   heading1, velocity1,
-     *   ...
-     * ]
-     *
    - * @param states The states to convert. - */ - public static double[] moduleStates(SwerveModuleState[] states) { - double[] array = new double[states.length * 2]; - for (int i = 0; i < states.length; i++) { - array[i * 2] = states[i].angle.getRadians(); - array[(i * 2) + 1] = states[i].speedMetersPerSecond; - } - return array; - } - - /** - * Converts {@link Pose2d}s into a double array. - * Returns in the following format (heading is in radians): - * 
    -     * [
-     *   x, y, rot,
-     *   x, y, rot,
-     *   ...
-     * ]
-     *
    - */ - public static double[] pose2d(Pose2d... poses) { - double[] array = new double[poses.length * 3]; - for (int i = 0; i < poses.length; i++) { - array[i * 3] = poses[i].getX(); - array[(i * 3) + 1] = poses[i].getY(); - array[(i * 3) + 2] = poses[i].getRotation().getRadians(); - } - return array; - } - - /** - * Converts {@link Pose3d}s into a double array. - * Returns in the following format (rotation is in radians): - * 
    -     * [
-     *   x, y, z, wRot, xRot, yRot, zRot,
-     *   x, y, z, wRot, xRot, yRot, zRot,
-     *   ...
-     * ]
-     *
    - */ - public static double[] pose3d(Pose3d... poses) { - double[] array = new double[poses.length * 7]; - for (int i = 0; i < poses.length; i++) { - array[i * 7] = poses[i].getX(); - array[(i * 7) + 1] = poses[i].getY(); - array[(i * 7) + 2] = poses[i].getZ(); - array[(i * 7) + 3] = poses[i].getRotation().getQuaternion().getW(); - array[(i * 7) + 4] = poses[i].getRotation().getQuaternion().getX(); - array[(i * 7) + 5] = poses[i].getRotation().getQuaternion().getY(); - array[(i * 7) + 6] = poses[i].getRotation().getQuaternion().getZ(); - } - return array; - } -} diff --git a/src/main/java/org/team340/lib/util/Alliance.java b/src/main/java/org/team340/lib/util/Alliance.java index 59f04d0..870e06f 100644 --- a/src/main/java/org/team340/lib/util/Alliance.java +++ b/src/main/java/org/team340/lib/util/Alliance.java @@ -4,22 +4,47 @@ /** * Utility class for getting the robot's alliance. + * Can optionally be overridden using {@link Alliance#enableOverride(boolean)}. */ public final class Alliance { private Alliance() { - throw new UnsupportedOperationException("This is a utility class!"); + throw new AssertionError("This is a utility class!"); + } + + private static boolean overrideActive = false; + private static boolean overrideIsBlue = false; + + /** + * Overrides FMS alliance data in favor of a user-set value when + * using {@link Alliance#isBlue()} and {@link Alliance#isRed()}. + * @param isBlue The value to override with. + */ + public static void enableOverride(boolean isBlue) { + overrideActive = true; + overrideIsBlue = isBlue; + } + + /** + * Disables the override if active. + */ + public static void disableOverride() { + overrideActive = false; } /** * Returns {@code true} if the robot is on the blue alliance. + * If the robot's alliance is unknown, defaults to {@code true} (blue). */ public static boolean isBlue() { - return DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue).equals(DriverStation.Alliance.Blue); + return overrideActive + ? overrideIsBlue + : DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue).equals(DriverStation.Alliance.Blue); } /** * Returns {@code true} if the robot is on the red alliance. + * If the robot's alliance is unknown, defaults to {@code false} (blue). */ public static boolean isRed() { return !isBlue(); diff --git a/src/main/java/org/team340/lib/commands/CommandBuilder.java b/src/main/java/org/team340/lib/util/CommandBuilder.java similarity index 78% rename from src/main/java/org/team340/lib/commands/CommandBuilder.java rename to src/main/java/org/team340/lib/util/CommandBuilder.java index e78a6d7..41ee455 100644 --- a/src/main/java/org/team340/lib/commands/CommandBuilder.java +++ b/src/main/java/org/team340/lib/util/CommandBuilder.java @@ -1,12 +1,16 @@ -package org.team340.lib.commands; +package org.team340.lib.util; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.ConcurrentModificationException; import java.util.function.Consumer; import java.util.function.Supplier; /** * A command builder. Very similar to {@link FunctionalCommand}. + * Stylistic alternative to using decorators. */ public class CommandBuilder extends Command { @@ -37,7 +41,9 @@ public CommandBuilder(Subsystem... requirements) { * The initial subroutine of a command. Called once when the command is initially scheduled. */ public CommandBuilder onInitialize(Runnable onInitialize) { - if (this.isScheduled()) throw new IllegalStateException("Cannot change methods of a command while it is scheduled"); + if (this.isScheduled()) throw new ConcurrentModificationException( + "Cannot change methods of a command while it is scheduled" + ); this.onInitialize = onInitialize; return this; } @@ -46,7 +52,9 @@ public CommandBuilder onInitialize(Runnable onInitialize) { * The main body of a command. Called repeatedly while the command is scheduled. */ public CommandBuilder onExecute(Runnable onExecute) { - if (this.isScheduled()) throw new IllegalStateException("Cannot change methods of a command while it is scheduled"); + if (this.isScheduled()) throw new ConcurrentModificationException( + "Cannot change methods of a command while it is scheduled" + ); this.onExecute = onExecute; return this; } @@ -64,7 +72,9 @@ public CommandBuilder onEnd(Runnable onEnd) { * or when it interrupted/canceled. Supplied boolean is if the command was interrupted. */ public CommandBuilder onEnd(Consumer onEnd) { - if (this.isScheduled()) throw new IllegalStateException("Cannot change methods of a command while it is scheduled"); + if (this.isScheduled()) throw new ConcurrentModificationException( + "Cannot change methods of a command while it is scheduled" + ); this.onEnd = onEnd; return this; } @@ -84,7 +94,9 @@ public CommandBuilder isFinished(boolean isFinished) { * method and un-schedule it. By default, this returns {@code false}. */ public CommandBuilder isFinished(Supplier isFinished) { - if (this.isScheduled()) throw new IllegalStateException("Cannot change methods of a command while it is scheduled"); + if (this.isScheduled()) throw new ConcurrentModificationException( + "Cannot change methods of a command while it is scheduled" + ); this.isFinished = isFinished; return this; } diff --git a/src/main/java/org/team340/lib/util/GRRSubsystem.java b/src/main/java/org/team340/lib/util/GRRSubsystem.java new file mode 100644 index 0000000..346a30c --- /dev/null +++ b/src/main/java/org/team340/lib/util/GRRSubsystem.java @@ -0,0 +1,25 @@ +package org.team340.lib.util; + +import edu.wpi.first.wpilibj2.command.Subsystem; + +public abstract class GRRSubsystem implements Subsystem { + + public GRRSubsystem() { + register(); + } + + /** + * Creates a command builder that requires this subsystem. + */ + protected CommandBuilder commandBuilder() { + return new CommandBuilder(this); + } + + /** + * Creates a command builder that requires this subsystem. + * @param name The name of the command. + */ + protected CommandBuilder commandBuilder(String name) { + return new CommandBuilder(name, this); + } +} diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/util/Math2.java index 059c351..0b4aa15 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -1,16 +1,14 @@ package org.team340.lib.util; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; /** * Math utilities. @@ -19,108 +17,48 @@ public final class Math2 { private Math2() { - throw new UnsupportedOperationException("This is a utility class!"); + throw new AssertionError("This is a utility class!"); } - /** - * Shared maximum accuracy floating point. - */ - public static final double DEFAULT_EPSILON = 1e-9; - /** - * Default precision used in {@link Math2#toFixed(double)}. - */ - public static final double DEFAULT_TO_FIXED_PRECISION = 1e-3; - /** - * {@code PI * 2} - */ - public static final double TWO_PI = Math.PI * 2.0; - /** - * {@code PI / 2} - */ - public static final double HALF_PI = Math.PI / 2.0; - /** - * {@code PI / 3} - */ - public static final double THIRD_PI = Math.PI / 3.0; - /** - * {@code PI / 4} - */ + /** Shared maximum accuracy floating point. */ + public static final double EPSILON = 1e-8; + /** {@code PI/6} (30deg) */ + public static final double SIXTH_PI = Math.PI / 6.0; + /** {@code PI/4} (45deg) */ public static final double QUARTER_PI = Math.PI / 4.0; - /** - * {@code 2PI / 3} - */ - public static final double TWO_THIRD_PI = 2.0 * Math.PI / 3.0; + /** {@code PI/3} (60deg) */ + public static final double THIRD_PI = Math.PI / 3.0; + /** {@code PI/2} (90deg) */ + public static final double HALF_PI = Math.PI / 2.0; + /** {@code 2PI/3} (120deg) */ + public static final double TWO_THIRDS_PI = (2.0 * Math.PI) / 3.0; + /** {@code 3PI/4} (135deg) */ + public static final double THREE_QUARTERS_PI = (3.0 * Math.PI) / 4.0; + /** {@code 5PI/6} (150deg) */ + public static final double FIVE_SIXTHS_PI = (5.0 * Math.PI) / 6.0; + /** {@code PI*2} (360deg) */ + public static final double TWO_PI = Math.PI * 2.0; + + public static final Pose2d kZeroPose2d = new Pose2d(); + public static final Rotation2d kZeroRotation2d = new Rotation2d(); + public static final Rotation3d kZeroRotation3d = new Rotation3d(); + public static final Rotation2d kPiRotation2d = new Rotation2d(Math.PI); /** - * Identity {@link Translation2d}. - */ - public static final Translation2d TRANSLATION2D_0 = new Translation2d(); - /** - * Identity {@link Translation3d}. - */ - public static final Translation3d TRANSLATION3D_0 = new Translation3d(); - /** - * Identity {@link Rotation2d}. - */ - public static final Rotation2d ROTATION2D_0 = new Rotation2d(); - /** - * A {@link Rotation2d} with a value of {@code 1/2 PI}. - */ - public static final Rotation2d ROTATION2D_HALF_PI = new Rotation2d(HALF_PI); - /** - * A {@link Rotation2d} with a value of {@code PI}. - */ - public static final Rotation2d ROTATION2D_PI = new Rotation2d(Math.PI); - /** - * A {@link Rotation2d} with a value of {@code -1/2 PI}. - */ - public static final Rotation2d ROTATION2D_NEG_HALF_PI = new Rotation2d(-HALF_PI); - /** - * A {@link Rotation2d} with a value of {@code -PI}. - */ - public static final Rotation2d ROTATION2D_NEG_PI = new Rotation2d(-Math.PI); - /** - * Identity {@link Rotation3d}. - */ - public static final Rotation3d ROTATION3D_0 = new Rotation3d(); - /** - * Identity {@link Transform2d}. - */ - public static final Transform2d TRANSFORM2D_0 = new Transform2d(); - /** - * Identity {@link Transform3d}. - */ - public static final Transform3d TRANSFORM3D_0 = new Transform3d(); - /** - * Identity {@link Twist2d}. - */ - public static final Twist2d TWIST2D_0 = new Twist2d(); - /** - * Identity {@link Twist3d}. - */ - public static final Twist3d TWIST3D_0 = new Twist3d(); - /** - * Identity {@link Pose2d}. - */ - public static final Pose2d POSE2D_0 = new Pose2d(); - /** - * Identity {@link Pose3d}. - */ - public static final Pose3d POSE3D_0 = new Pose3d(); - /** - * Identity {@link ChassisSpeeds}. + * Returns a random double from {@code 0.0} to {@code max}. + * @param max The maximum value to return. */ - public static final ChassisSpeeds CHASSIS_SPEEDS_0 = new ChassisSpeeds(); + public static double random(double max) { + return Math.random() * max; + } /** - * Wraps an angle within {@code +-PI} of a reference. - * @param ref The reference angle in radians. - * @param angle The angle to wrap in radians. - * @return The wrapped angle in radians. + * Returns a random double from {@code min} to {@code max}. + * @param min The minimum value to return. + * @param max The maximum value to return. */ - public static double wrapAbout(double ref, double angle) { - double diff = angle - ref; - if (diff > Math.PI) return angle - (Math2.TWO_PI); else if (diff < -Math.PI) return angle + (Math2.TWO_PI); else return angle; + public static double random(double min, double max) { + return (Math.random() * (max - min)) + min; } /** @@ -130,7 +68,7 @@ public static double wrapAbout(double ref, double angle) { * @return {@code true} if the values are equal. */ public static boolean epsilonEquals(double a, double b) { - return epsilonEquals(a, b, DEFAULT_EPSILON); + return epsilonEquals(a, b, EPSILON); } /** @@ -145,103 +83,135 @@ public static boolean epsilonEquals(double a, double b, double epsilon) { } /** - * Checks if two {@link Twist2d}s are equal within the accuracy of the default epsilon. + * Checks if two {@link Translation2d}s are equal within the accuracy of the default epsilon. * @param a The first value to compare. * @param b The second value to compare. * @return {@code true} if the values are equal. */ - public static boolean twist2dEpsilonEquals(Twist2d a, Twist2d b) { - return twist2dEpsilonEquals(a, b, DEFAULT_EPSILON); + public static boolean translationEpsilonEquals(Translation2d a, Translation2d b) { + return translationEpsilonEquals(a, b, EPSILON); } /** - * Checks if two {@link Twist2d}s are equal within the accuracy of a provided epsilon. + * Checks if two {@link Translation2d}s are equal within the accuracy of a provided epsilon. * @param a The first value to compare. * @param b The second value to compare. * @param epsilon Epsilon value to compare with. * @return {@code true} if the values are equal. */ - public static boolean twist2dEpsilonEquals(Twist2d a, Twist2d b, double epsilon) { - return epsilonEquals(a.dx, b.dx, epsilon) && epsilonEquals(a.dy, b.dy, epsilon) && epsilonEquals(a.dtheta, b.dtheta, epsilon); + public static boolean translationEpsilonEquals(Translation2d a, Translation2d b, double epsilon) { + return (epsilonEquals(a.getX(), b.getX(), epsilon) && epsilonEquals(a.getY(), b.getY(), epsilon)); } /** - * Rounds a value to a fixed point of 3 decimal places. - * @param value The value to round. - * @return The rounded value. + * Checks if two {@link Transform2d}s are equal within the accuracy of the default epsilon. + * @param a The first value to compare. + * @param b The second value to compare. + * @return {@code true} if the values are equal. */ - public static double toFixed(double value) { - return toFixed(value, DEFAULT_TO_FIXED_PRECISION); + public static boolean transformEpsilonEquals(Transform2d a, Transform2d b) { + return transformEpsilonEquals(a, b, EPSILON); } /** - * Rounds a value to a fixed point. - * @param value The value to round. - * @param precision The fixed point precision to round to as a decimal. For example, {@code 1e-3} rounds to 3 decimal places. This must be a power of {@code 10}. - * @return The rounded value. + * Checks if two {@link Transform2d}s are equal within the accuracy of a provided epsilon. + * @param a The first value to compare. + * @param b The second value to compare. + * @param epsilon Epsilon value to compare with. + * @return {@code true} if the values are equal. */ - public static double toFixed(double value, double precision) { - return Math.round(value / precision) * precision; + public static boolean transformEpsilonEquals(Transform2d a, Transform2d b, double epsilon) { + return ( + epsilonEquals(a.getX(), b.getX(), epsilon) && + epsilonEquals(a.getY(), b.getY(), epsilon) && + epsilonEquals(a.getRotation().getRadians(), b.getRotation().getRadians(), epsilon) + ); } /** - * Converts radians to degrees and rounds to 3 decimal places. - * @param radians The radians to format. + * Checks if two {@link Pose2d}s are equal within the accuracy of the default epsilon. + * @param a The first value to compare. + * @param b The second value to compare. + * @return {@code true} if the values are equal. */ - public static double formatRadians(double radians) { - return toFixed(Math.toDegrees(radians)); + public static boolean poseEpsilonEquals(Pose2d a, Pose2d b) { + return poseEpsilonEquals(a, b, EPSILON); } /** - * Returns a random double from {@code 0.0} to {@code max}. - * @param max The maximum value to return. + * Checks if two {@link Pose2d}s are equal within the accuracy of a provided epsilon. + * @param a The first value to compare. + * @param b The second value to compare. + * @param epsilon Epsilon value to compare with. + * @return {@code true} if the values are equal. */ - public static double random(double max) { - return Math.random() * max; + public static boolean poseEpsilonEquals(Pose2d a, Pose2d b, double epsilon) { + return ( + epsilonEquals(a.getX(), b.getX(), epsilon) && + epsilonEquals(a.getY(), b.getY(), epsilon) && + epsilonEquals(a.getRotation().getRadians(), b.getRotation().getRadians(), epsilon) + ); } /** - * Returns a random double from {@code min} to {@code max}. - * @param min The minimum value to return. - * @param max The maximum value to return. + * Checks if two {@link Twist2d}s are equal within the accuracy of the default epsilon. + * @param a The first value to compare. + * @param b The second value to compare. + * @return {@code true} if the values are equal. */ - public static double random(double min, double max) { - return (Math.random() * (max - min)) + min; + public static boolean twistEpsilonEquals(Twist2d a, Twist2d b) { + return twistEpsilonEquals(a, b, EPSILON); + } + + /** + * Checks if two {@link Twist2d}s are equal within the accuracy of a provided epsilon. + * @param a The first value to compare. + * @param b The second value to compare. + * @param epsilon Epsilon value to compare with. + * @return {@code true} if the values are equal. + */ + public static boolean twistEpsilonEquals(Twist2d a, Twist2d b, double epsilon) { + return ( + epsilonEquals(a.dx, b.dx, epsilon) && + epsilonEquals(a.dy, b.dy, epsilon) && + epsilonEquals(a.dtheta, b.dtheta, epsilon) + ); + } + + /** + * Copies values from a source {@link ChassisSpeeds} object to another. + * @param source The speeds to copy from. + * @param output The speeds to copy into. + * @return The output speeds. + */ + public static ChassisSpeeds copyInto(ChassisSpeeds source, ChassisSpeeds output) { + output.vxMetersPerSecond = source.vxMetersPerSecond; + output.vyMetersPerSecond = source.vyMetersPerSecond; + output.omegaRadiansPerSecond = source.omegaRadiansPerSecond; + return output; } /** - * Definition of a 2D parametric function. + * Copies values from a source {@link SwerveModulePosition} object to another. + * @param source The swerve module position to copy from. + * @param output The swerve module position to copy into. + * @return The output position. */ - @FunctionalInterface - public static interface Parametric { - public double f(double x, double y); + public static SwerveModulePosition copyInto(SwerveModulePosition source, SwerveModulePosition output) { + output.distanceMeters = source.distanceMeters; + output.angle = source.angle; + return output; } /** - * Finds the root of a 2D parametric function with the false position method (regula falsi). - * @param func The function to take the root of. - * @param x0 {@code x} value of the lower bracket. - * @param y0 {@code y} value of the lower bracket. - * @param f0 value of {@code func} at {@code x0}, {@code y0}. - * @param x1 {@code x} value of the upper bracket. - * @param y1 {@code y} value of the upper bracket. - * @param f1 value of {@code func} at {@code x1}, {@code y1}. - * @param iterationsLeft Number of iterations of root finding remaining. - * @return The parameter value {@code s} that interpolating between {@code 0.0} and {@code 1.0} that corresponds with the approximate root. + * Copies values from a source {@link SwerveModuleState} object to another. + * @param source The swerve module state to copy from. + * @param output The swerve module state to copy into. + * @return The output state. */ - public static double findRoot(Parametric func, double x0, double y0, double f0, double x1, double y1, double f1, int iterationsLeft) { - if (iterationsLeft < 0 || epsilonEquals(f0, f1)) return 1.0; - iterationsLeft--; - - double sGuess = Math.max(0.0, Math.min(1.0, -f0 / (f1 - f0))); - double xGuess = (x1 - x0) * sGuess + x0; - double yGuess = (y1 - y0) * sGuess + y0; - double fGuess = func.f(xGuess, yGuess); - - if (Math.signum(f0) == Math.signum(fGuess)) { - return sGuess + (1.0 - sGuess) * findRoot(func, xGuess, yGuess, fGuess, x1, y1, f1, iterationsLeft); - } else { - return sGuess * findRoot(func, x0, y0, f0, xGuess, yGuess, fGuess, iterationsLeft); - } + public static SwerveModuleState copyInto(SwerveModuleState source, SwerveModuleState output) { + output.speedMetersPerSecond = source.speedMetersPerSecond; + output.angle = source.angle; + return output; } } diff --git a/src/main/java/org/team340/lib/util/Mutable.java b/src/main/java/org/team340/lib/util/Mutable.java index bb8d720..cca1a18 100644 --- a/src/main/java/org/team340/lib/util/Mutable.java +++ b/src/main/java/org/team340/lib/util/Mutable.java @@ -1,21 +1,24 @@ package org.team340.lib.util; +import java.util.function.Consumer; +import java.util.function.Supplier; + /** * A simple mutable object that stores a value. * - *

    Useful for declaring primitives in an enclosing scope to be accessed + *

    Useful for declaring primitives in an enclosing scope to be mutated * inside a lambda, as lambdas prohibit capturing non final variables, * with the exception of instance variables. This use case however should - * be performed with caution, as a race condition may occur if the lambda + * be executed with caution, as a race condition may occur if the lambda * escapes its capturing thread. * *

    Fortunately for our purposes, using this class in a command factory * to provide stateful behavior does not suffer from aforementioned race * conditions as commands are invoked synchronously. */ -public class Mutable { +public class Mutable implements Supplier, Consumer { - private T value; + public T value; /** * Create the mutable object. @@ -27,7 +30,9 @@ public Mutable(T value) { /** * Gets the current value. + * @return The current value. */ + @Override public T get() { return value; } @@ -36,7 +41,8 @@ public T get() { * Sets a new value. * @param value The new value. */ - public void set(T value) { + @Override + public void accept(T value) { this.value = value; } } diff --git a/src/main/java/org/team340/lib/util/Polar2d.java b/src/main/java/org/team340/lib/util/Polar2d.java deleted file mode 100644 index be73e39..0000000 --- a/src/main/java/org/team340/lib/util/Polar2d.java +++ /dev/null @@ -1,27 +0,0 @@ -package org.team340.lib.util; - -import edu.wpi.first.math.interpolation.Interpolatable; - -public class Polar2d implements Interpolatable { - - private final double theta; - private final double r; - - public Polar2d(double theta, double r) { - this.theta = theta; - this.r = r; - } - - public double getTheta() { - return theta; - } - - public double getR() { - return r; - } - - @Override - public Polar2d interpolate(Polar2d endValue, double t) { - return new Polar2d(theta * t + endValue.theta * (1 - t), r * t + endValue.r * (1 - t)); - } -} diff --git a/src/main/java/org/team340/lib/util/Profiler.java b/src/main/java/org/team340/lib/util/Profiler.java new file mode 100644 index 0000000..c24a6a9 --- /dev/null +++ b/src/main/java/org/team340/lib/util/Profiler.java @@ -0,0 +1,206 @@ +package org.team340.lib.util; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Supplier; + +/** + * A simple pseudo call-graph profiler that publishes timings to NetworkTables. + * + *

    Data is published to {@code /Profiling}, with CPU time utilized by the garbage + * collector also published at {@code /Profiling/GC}. Time spent publishing to NT + * is also published at {@code /Profiling/Overhead}. All timings are recorded in + * milliseconds, as the duration of execution. The profiler records the time to + * execute blocks of code, with support for nesting execution times which are + * reflected in an expanding NT tree. + * + *

    All invocations of {@link Profiler#start(String)} are expected to be + * followed by a closing {@link Profiler#end()}. Additionally, it is expected + * that a single {@link Profiler#start(String)}/{@link Profiler#end()} pair is + * found at the highest level of the robot's code (i.e. {@code robotPeriodic}), + * with no other "root" pairs. If either of these limitations are left unsatisfied, + * an error will be printed to the Driver Station. Profiling across threads is also + * not supported. + */ +public final class Profiler { + + private Profiler() { + throw new AssertionError("This is a utility class!"); + } + + private static final NetworkTable nt = NetworkTableInstance.getDefault().getTable("Profiling"); + + private static final Map callGraph = new HashMap<>(); + private static final List stack = new ArrayList<>(); + + private static final List gcList = ManagementFactory.getGarbageCollectorMXBeans(); + private static final DoublePublisher overheadPub = nt.getDoubleTopic("Overhead").publish(); + private static final DoublePublisher gcPub = nt.getDoubleTopic("GC").publish(); + + private static String root = ""; + private static double lastGCTime = 0.0; + + /** + * Determines if the profiler is currently running. + * @return {@link true} if the profiler is running. + */ + public static boolean isRunning() { + return stack.size() > 0; + } + + /** + * Profiles a section of user code. Serves as a shorthand for calling + * {@link Profiler#start(String)}, {user code}, {@link Profiler#end()}. + * @param name The name of the call. Must be unique. + * @param runnable The user code to be ran. + */ + public static void run(String name, Runnable runnable) { + start(name); + runnable.run(); + end(); + } + + /** + * Profiles a section of user code. Serves as a shorthand for calling + * {@link Profiler#start(String)}, {user code}, {@link Profiler#end()}. + * @param name The name of the call. Must be unique. + * @param runnable The user code to be ran. + * @return The value returned from the supplier. + */ + public static T run(String name, Supplier supplier) { + start(name); + T v = supplier.get(); + end(); + return v; + } + + /** + * Starts to profile a section of user code. This method should be invoked + * before the user code to be profiled, with {@link Profiler#end()} expected + * to be after. Additional {@link Profiler#start(String)}/{@link Profiler#end()} + * pairs can be nested within the section of user code, and will appear as a + * sub-topic in NetworkTables. Timings of nested pairs are still included in + * the overall time recorded by the parent pair. + * @param name The name of the call. Must be unique. + */ + public static void start(String name) { + if (stack.isEmpty()) { + if (root.isEmpty()) { + root = name; + } else if (name != root) { + DriverStation.reportError( + "[Profiler] Unexpected secondary root with name \"" + + name + + "\", expected primary root \"" + + root + + "\"", + true + ); + } + } + + stack.add(name); + String fullName = String.join("/", stack); + CallData call = callGraph.get(fullName); + if (call == null) { + call = new CallData(fullName); + callGraph.put(fullName, call); + } + + call.onStart(); + } + + /** + * Ends profiling a section of code. This method should be invoked + * after the user code to be profiled. Trailing calls to this method + * will result in an error printed to the Driver Station. + */ + public static void end() { + CallData call = callGraph.get(String.join("/", stack)); + if (call != null) { + call.onEnd(); + stack.remove(stack.size() - 1); + + if (stack.isEmpty()) { + long start = RobotController.getFPGATime(); + var it = callGraph.entrySet().iterator(); + while (it.hasNext()) { + CallData entryCall = it.next().getValue(); + if (!entryCall.done) { + entryCall.close(); + it.remove(); + continue; + } + + entryCall.pubAndReset(); + } + + double gcSum = 0.0; + for (var gc : gcList) { + double gcTime = gc.getCollectionTime(); + if (gcTime != -1.0) gcSum += gcTime; + } + gcPub.set(gcSum - lastGCTime); + lastGCTime = gcSum; + + overheadPub.set((RobotController.getFPGATime() - start) / 1000.0); + } + } else { + DriverStation.reportError("[Profiler] Unexpected end() call", true); + } + } + + /** + * Manages the data of a call. + */ + private static final class CallData implements AutoCloseable { + + public final String fullName; + public double time = -1.0; + public boolean done = false; + public DoublePublisher pub; + + public CallData(String fullName) { + this.fullName = fullName; + } + + /** + * Should be invoked when a call starts. + */ + public void onStart() { + time = RobotController.getFPGATime() / 1000.0; + } + + /** + * Should be invoked when a call ends. + */ + public void onEnd() { + time = (RobotController.getFPGATime() / 1000.0) - time; + done = true; + } + + /** + * Publishes the last timing to NT and resets state. + */ + public void pubAndReset() { + if (pub == null) pub = nt.getDoubleTopic(fullName).publish(); + pub.set(time); + time = -1.0; + done = false; + } + + @Override + public void close() { + pub.close(); + } + } +} diff --git a/src/main/java/org/team340/lib/util/SendableFactory.java b/src/main/java/org/team340/lib/util/SendableFactory.java deleted file mode 100644 index 432654f..0000000 --- a/src/main/java/org/team340/lib/util/SendableFactory.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.team340.lib.util; - -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import java.util.function.Consumer; - -/** - * Factory for creating {@link Sendable}s inline. - */ -public class SendableFactory { - - private SendableFactory() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - /** - * Creates a {@link Sendable} object. - * @param initSendable A {@link SendableBuilder} consumer, called when the return object's {@code initSendable()} method is invoked. - * @return The created sendable. - */ - public static Sendable create(Consumer initSendable) { - return new SendableImpl(initSendable); - } - - private static class SendableImpl implements Sendable { - - private final Consumer initSendableConsumer; - - public SendableImpl(Consumer initSendable) { - initSendableConsumer = initSendable; - } - - @Override - public void initSendable(SendableBuilder builder) { - initSendableConsumer.accept(builder); - } - } -} diff --git a/src/main/java/org/team340/lib/util/Sleep.java b/src/main/java/org/team340/lib/util/Sleep.java index 8c36cf7..bbc93ec 100644 --- a/src/main/java/org/team340/lib/util/Sleep.java +++ b/src/main/java/org/team340/lib/util/Sleep.java @@ -1,30 +1,73 @@ package org.team340.lib.util; +import static edu.wpi.first.units.Units.Milliseconds; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Time; import edu.wpi.first.wpilibj.RobotBase; /** - * Utility class for sleeping the current thread, unless in simulation. + * Utility class for sleeping the current thread, + * with the option to skip if in simulation. */ public final class Sleep { private Sleep() { - throw new UnsupportedOperationException("This is a utility class!"); + throw new AssertionError("This is a utility class!"); + } + + /** + * Sleeps the thread for the specified duration. + * Will always run, even if in simulation. + * @param time The time to sleep for. + */ + public static void of(Measure

    Periodic Status 0

    + *

    Periodic Status 0

    * * Default rate: {@code 10ms} * @@ -82,7 +89,7 @@ public static enum Frame { */ S0(PeriodicFrame.kStatus0), /** - *

    Periodic Status 1

    + *

    Periodic Status 1

    * * Default rate: {@code 20ms} * @@ -113,7 +120,7 @@ public static enum Frame { */ S1(PeriodicFrame.kStatus1), /** - *

    Periodic Status 2

    + *

    Periodic Status 2

    * * Default rate: {@code 20ms} * @@ -132,7 +139,7 @@ public static enum Frame { */ S2(PeriodicFrame.kStatus2), /** - *

    Periodic Status 3

    + *

    Periodic Status 3

    * * Default rate: {@code 50ms} * @@ -159,7 +166,7 @@ public static enum Frame { */ S3(PeriodicFrame.kStatus3), /** - *

    Periodic Status 4

    + *

    Periodic Status 4

    * * Default rate: {@code 20ms} * @@ -182,7 +189,7 @@ public static enum Frame { */ S4(PeriodicFrame.kStatus4), /** - *

    Periodic Status 5

    + *

    Periodic Status 5

    * * Default rate: {@code 200ms} * @@ -205,7 +212,7 @@ public static enum Frame { */ S5(PeriodicFrame.kStatus5), /** - *

    Periodic Status 6

    + *

    Periodic Status 6

    * * Default rate: {@code 200ms} * @@ -226,7 +233,26 @@ public static enum Frame { * * */ - S6(PeriodicFrame.kStatus6); + S6(PeriodicFrame.kStatus6), + /** + *

    Periodic Status 7

    + * + * Default rate: {@code 250ms} + * + *

    Frame content:

    + * + * + * + * + * + * + * + * + * + * + *
    Available Data Description
    IAccum I accumulator of the PID controller.
    + */ + S7(PeriodicFrame.kStatus7); private final PeriodicFrame frame; @@ -253,7 +279,8 @@ public SparkFlexConfig clearFaults() { public SparkFlexConfig disableVoltageCompensation() { addStep( sparkFlex -> sparkFlex.disableVoltageCompensation(), - sparkFlex -> Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON), + sparkFlex -> + Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON), "Disable Voltage Compensation" ); return this; @@ -280,7 +307,12 @@ public SparkFlexConfig enableSoftLimit(CANSparkFlex.SoftLimitDirection direction public SparkFlexConfig enableVoltageCompensation(double nominalVoltage) { addStep( sparkFlex -> sparkFlex.enableVoltageCompensation(nominalVoltage), - sparkFlex -> Math2.epsilonEquals(sparkFlex.getVoltageCompensationNominalVoltage(), nominalVoltage, RevConfigRegistry.EPSILON), + sparkFlex -> + Math2.epsilonEquals( + sparkFlex.getVoltageCompensationNominalVoltage(), + nominalVoltage, + RevConfigRegistry.EPSILON + ), "Enable Voltage Compensation" ); return this; @@ -328,7 +360,12 @@ public SparkFlexConfig follow(CANSparkFlex.ExternalFollower leader, int deviceId * @param invert Set the follower to output opposite of the leader. */ public SparkFlexConfig follow(CANSparkFlex.ExternalFollower leader, int deviceId, boolean invert) { - addStep(sparkFlex -> sparkFlex.follow(leader, deviceId, invert), sparkFlex -> sparkFlex.isFollower(), false, "Follow"); + addStep( + sparkFlex -> sparkFlex.follow(leader, deviceId, invert), + sparkFlex -> sparkFlex.isFollower(), + false, + "Follow" + ); return this; } @@ -364,7 +401,11 @@ public SparkFlexConfig setClosedLoopRampRate(double rate) { * @param mode Idle mode (coast or brake). */ public SparkFlexConfig setIdleMode(CANSparkFlex.IdleMode mode) { - addStep(sparkFlex -> sparkFlex.setIdleMode(mode), sparkFlex -> sparkFlex.getIdleMode().equals(mode), "Idle Mode"); + addStep( + sparkFlex -> sparkFlex.setIdleMode(mode), + sparkFlex -> sparkFlex.getIdleMode().equals(mode), + "Idle Mode" + ); return this; } @@ -407,13 +448,11 @@ public SparkFlexConfig setOpenLoopRampRate(double rate) { * @param periodMs The rate the controller sends the frame in milliseconds. */ public SparkFlexConfig setPeriodicFramePeriod(Frame frame, int periodMs) { - List applied = new ArrayList<>(); addStep( sparkFlex -> { - if (!applied.contains(sparkFlex)) { - RevConfigRegistry.addPeriodic(() -> sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs)); - applied.add(sparkFlex); - } + RevConfigRegistry.addFrameRefresher(sparkFlex.hashCode() + "." + frame.name(), () -> + sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs) + ); return sparkFlex.setPeriodicFramePeriod(frame.frame, periodMs); }, "Periodic Frame Status " + frame.ordinal() @@ -535,7 +574,7 @@ public SparkFlexConfig restoreFactoryDefaults() { addStep( sparkFlex -> { REVLibError res = sparkFlex.restoreFactoryDefaults(); - Sleep.ms(FACTORY_DEFAULTS_SLEEP); + Sleep.seconds(kFactoryDefaultsSleep, true); return res; }, "Restore Factory Defaults" @@ -552,7 +591,7 @@ public SparkFlexConfig restoreFactoryDefaults(boolean persist) { addStep( sparkFlex -> { REVLibError res = sparkFlex.restoreFactoryDefaults(persist); - Sleep.ms(FACTORY_DEFAULTS_SLEEP); + Sleep.seconds(kFactoryDefaultsSleep, true); return res; }, "Restore Factory Defaults" diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java b/src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java similarity index 52% rename from src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java rename to src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java index 2ee8e6f..f5f999a 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java +++ b/src/main/java/org/team340/lib/util/rev/SparkLimitSwitchConfig.java @@ -1,4 +1,4 @@ -package org.team340.lib.util.config.rev; +package org.team340.lib.util.rev; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; @@ -7,49 +7,40 @@ /** * Config builder for {@link SparkLimitSwitch}. */ -public final class SparkLimitSwitchConfig extends RevConfigBase { - - /** - * Creates an empty config. - */ - public SparkLimitSwitchConfig() {} - - /** - * Creates a config that copies the config steps from the base provided. - * @param base The config to copy the steps from. - */ - private SparkLimitSwitchConfig(RevConfigBase base) { - super(base); - } +public class SparkLimitSwitchConfig extends RevConfigBase { /** * Clones this config. */ public SparkLimitSwitchConfig clone() { - return new SparkLimitSwitchConfig(this); + var config = new SparkLimitSwitchConfig(); + config.configSteps.addAll(configSteps); + return config; } /** - * Applies the config to a Spark Max attached limit switch. + * Applies the config to a Spark Max attached limit switch. Note that this is a blocking + * operation. Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. * @param sparkMax The Spark Max the limit switch is attached to. * @param limitSwitch The limit switch. */ public void apply(CANSparkMax sparkMax, SparkLimitSwitch limitSwitch) { - super.applySteps(limitSwitch, "Spark Max (ID " + sparkMax.getDeviceId() + ") Limit Switch"); + applySteps(limitSwitch, "Spark Max (ID " + sparkMax.getDeviceId() + ") Limit Switch"); } /** - * Applies the config to a Spark Flex limit switch. + * Applies the config to a Spark Flex limit switch. Note that this is a blocking operation. + * Errors are printed when calling {@link RevConfigRegistry#burnFlashAll()}. * @param sparkFlex The Spark Flex the limit switch is attached to. * @param limitSwitch The limit switch. */ public void apply(CANSparkFlex sparkFlex, SparkLimitSwitch limitSwitch) { - super.applySteps(limitSwitch, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Limit Switch"); + applySteps(limitSwitch, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Limit Switch"); } /** * Enables or disables controller shutdown based on the limit switch. - * @param enable Enable / disable motor shutdown based on the limit switch state. This does not affect the result of the get() command. + * @param enable Enable/disable motor shutdown based on the limit switch state. This does not affect the result of the get() command. */ public SparkLimitSwitchConfig enableLimitSwitch(boolean enable) { addStep( diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java b/src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java similarity index 85% rename from src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java rename to src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java index 407fac8..6dd0e86 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java +++ b/src/main/java/org/team340/lib/util/rev/SparkMaxConfig.java @@ -1,57 +1,64 @@ -package org.team340.lib.util.config.rev; +package org.team340.lib.util.rev; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import java.util.ArrayList; -import java.util.List; import org.team340.lib.util.Math2; import org.team340.lib.util.Sleep; /** * Config builder for {@link CANSparkMax}. */ -public final class SparkMaxConfig extends RevConfigBase { +public class SparkMaxConfig extends RevConfigBase { + + private static final double kFactoryDefaultsSleep = 0.050; + private static final SparkMaxConfig kDefaults = new SparkMaxConfig() + .clearFaults() + .restoreFactoryDefaults() + .enableVoltageCompensation(12.0) + .setPeriodicFramePeriod(Frame.S0, 20) + .setPeriodicFramePeriod(Frame.S1, 20) + .setPeriodicFramePeriod(Frame.S2, 20) + .setPeriodicFramePeriod(Frame.S3, 10000) + .setPeriodicFramePeriod(Frame.S4, 10000) + .setPeriodicFramePeriod(Frame.S5, 10000) + .setPeriodicFramePeriod(Frame.S6, 10000) + .setPeriodicFramePeriod(Frame.S7, 10000); /** - * Creates an empty config. + * Returns a default Spark MAX config. */ - public SparkMaxConfig() {} - - /** - * Creates a config that copies the config steps from the base provided. - * @param base The config to copy the steps from. - */ - private SparkMaxConfig(RevConfigBase base) { - super(base); + public static SparkMaxConfig defaults() { + return kDefaults.clone(); } /** * Clones this config. */ public SparkMaxConfig clone() { - return new SparkMaxConfig(this); + var config = new SparkMaxConfig(); + config.configSteps.addAll(configSteps); + return config; } - private static final double FACTORY_DEFAULTS_SLEEP = 50.0; - /** - * Applies the config. + * Applies the config. Note that this is a blocking operation. Errors + * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. * @param sparkMax The Spark Max to apply the config to. */ public void apply(CANSparkMax sparkMax) { String identifier = "Spark Max (ID " + sparkMax.getDeviceId() + ")"; - super.applySteps(sparkMax, identifier); + applySteps(sparkMax, identifier); RevConfigRegistry.addBurnFlash(identifier, () -> sparkMax.burnFlash()); } /** - * Spark Max CAN status frames. - * @see https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces#periodic-status-frames + * Spark Flex CAN status frames. + * @see https://docs.revrobotics.com/brushless/spark-max/control-interfaces#periodic-status-frames */ public static enum Frame { /** - *

    Periodic Status 0

    + *

    Periodic Status 0

    * * Default rate: {@code 10ms} * @@ -82,7 +89,7 @@ public static enum Frame { */ S0(PeriodicFrame.kStatus0), /** - *

    Periodic Status 1

    + *

    Periodic Status 1

    * * Default rate: {@code 20ms} * @@ -113,7 +120,7 @@ public static enum Frame { */ S1(PeriodicFrame.kStatus1), /** - *

    Periodic Status 2

    + *

    Periodic Status 2

    * * Default rate: {@code 20ms} * @@ -132,7 +139,7 @@ public static enum Frame { */ S2(PeriodicFrame.kStatus2), /** - *

    Periodic Status 3

    + *

    Periodic Status 3

    * * Default rate: {@code 50ms} * @@ -159,7 +166,7 @@ public static enum Frame { */ S3(PeriodicFrame.kStatus3), /** - *

    Periodic Status 4

    + *

    Periodic Status 4

    * * Default rate: {@code 20ms} * @@ -182,7 +189,7 @@ public static enum Frame { */ S4(PeriodicFrame.kStatus4), /** - *

    Periodic Status 5

    + *

    Periodic Status 5

    * * Default rate: {@code 200ms} * @@ -205,7 +212,7 @@ public static enum Frame { */ S5(PeriodicFrame.kStatus5), /** - *

    Periodic Status 6

    + *

    Periodic Status 6

    * * Default rate: {@code 200ms} * @@ -226,7 +233,26 @@ public static enum Frame { * * */ - S6(PeriodicFrame.kStatus6); + S6(PeriodicFrame.kStatus6), + /** + *

    Periodic Status 7

    + * + * Default rate: {@code 250ms} + * + *

    Frame content:

    + * + * + * + * + * + * + * + * + * + * + *
    Available Data Description
    IAccum I accumulator of the PID controller.
    + */ + S7(PeriodicFrame.kStatus7); private final PeriodicFrame frame; @@ -253,7 +279,8 @@ public SparkMaxConfig clearFaults() { public SparkMaxConfig disableVoltageCompensation() { addStep( sparkMax -> sparkMax.disableVoltageCompensation(), - sparkMax -> Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON), + sparkMax -> + Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), 0.0, RevConfigRegistry.EPSILON), "Disable Voltage Compensation" ); return this; @@ -280,7 +307,12 @@ public SparkMaxConfig enableSoftLimit(CANSparkMax.SoftLimitDirection direction, public SparkMaxConfig enableVoltageCompensation(double nominalVoltage) { addStep( sparkMax -> sparkMax.enableVoltageCompensation(nominalVoltage), - sparkMax -> Math2.epsilonEquals(sparkMax.getVoltageCompensationNominalVoltage(), nominalVoltage, RevConfigRegistry.EPSILON), + sparkMax -> + Math2.epsilonEquals( + sparkMax.getVoltageCompensationNominalVoltage(), + nominalVoltage, + RevConfigRegistry.EPSILON + ), "Enable Voltage Compensation" ); return this; @@ -328,7 +360,12 @@ public SparkMaxConfig follow(CANSparkMax.ExternalFollower leader, int deviceId) * @param invert Set the follower to output opposite of the leader. */ public SparkMaxConfig follow(CANSparkMax.ExternalFollower leader, int deviceId, boolean invert) { - addStep(sparkMax -> sparkMax.follow(leader, deviceId, invert), sparkMax -> sparkMax.isFollower(), false, "Follow"); + addStep( + sparkMax -> sparkMax.follow(leader, deviceId, invert), + sparkMax -> sparkMax.isFollower(), + false, + "Follow" + ); return this; } @@ -407,13 +444,11 @@ public SparkMaxConfig setOpenLoopRampRate(double rate) { * @param periodMs The rate the controller sends the frame in milliseconds. */ public SparkMaxConfig setPeriodicFramePeriod(Frame frame, int periodMs) { - List applied = new ArrayList<>(); addStep( sparkMax -> { - if (!applied.contains(sparkMax)) { - RevConfigRegistry.addPeriodic(() -> sparkMax.setPeriodicFramePeriod(frame.frame, periodMs)); - applied.add(sparkMax); - } + RevConfigRegistry.addFrameRefresher(sparkMax.hashCode() + "." + frame.name(), () -> + sparkMax.setPeriodicFramePeriod(frame.frame, periodMs) + ); return sparkMax.setPeriodicFramePeriod(frame.frame, periodMs); }, "Periodic Frame Status " + frame.ordinal() @@ -535,7 +570,7 @@ public SparkMaxConfig restoreFactoryDefaults() { addStep( sparkMax -> { REVLibError res = sparkMax.restoreFactoryDefaults(); - Sleep.ms(FACTORY_DEFAULTS_SLEEP); + Sleep.seconds(kFactoryDefaultsSleep, true); return res; }, "Restore Factory Defaults" @@ -552,7 +587,7 @@ public SparkMaxConfig restoreFactoryDefaults(boolean persist) { addStep( sparkMax -> { REVLibError res = sparkMax.restoreFactoryDefaults(persist); - Sleep.ms(FACTORY_DEFAULTS_SLEEP); + Sleep.seconds(kFactoryDefaultsSleep, true); return res; }, "Restore Factory Defaults" diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java b/src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java similarity index 88% rename from src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java rename to src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java index 066dc00..9601039 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java +++ b/src/main/java/org/team340/lib/util/rev/SparkPIDControllerConfig.java @@ -1,4 +1,4 @@ -package org.team340.lib.util.config.rev; +package org.team340.lib.util.rev; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; @@ -6,49 +6,39 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.AccelStrategy; import org.team340.lib.util.Math2; -import org.team340.lib.util.config.PIDConfig; /** * Config builder for {@link SparkPIDController}. */ -public final class SparkPIDControllerConfig extends RevConfigBase { - - /** - * Creates an empty config. - */ - public SparkPIDControllerConfig() {} - - /** - * Creates a config that copies the config steps from the base provided. - * @param base The config to copy the steps from. - */ - private SparkPIDControllerConfig(RevConfigBase base) { - super(base); - } +public class SparkPIDControllerConfig extends RevConfigBase { /** * Clones this config. */ public SparkPIDControllerConfig clone() { - return new SparkPIDControllerConfig(this); + var config = new SparkPIDControllerConfig(); + config.configSteps.addAll(configSteps); + return config; } /** - * Applies the config. + * Applies the config. Note that this is a blocking operation. Errors + * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. * @param sparkMax The Spark Max to apply the config to. * @param pidController The PID controller. */ public void apply(CANSparkMax sparkMax, SparkPIDController pidController) { - super.applySteps(pidController, "Spark Max (ID " + sparkMax.getDeviceId() + ") PID Controller"); + applySteps(pidController, "Spark Max (ID " + sparkMax.getDeviceId() + ") PID Controller"); } /** - * Applies the config. + * Applies the config. Note that this is a blocking operation. Errors + * are printed when calling {@link RevConfigRegistry#burnFlashAll()}. * @param sparkFlex The Spark Flex to apply the config to. * @param pidController The PID controller. */ public void apply(CANSparkFlex sparkFlex, SparkPIDController pidController) { - super.applySteps(pidController, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") PID Controller"); + applySteps(pidController, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") PID Controller"); } /** @@ -182,7 +172,8 @@ public SparkPIDControllerConfig setI(double gain, int slotId) { public SparkPIDControllerConfig setIMaxAccum(double iMaxAccum, int slotId) { addStep( pidController -> pidController.setIMaxAccum(iMaxAccum, slotId), - pidController -> Math2.epsilonEquals(pidController.getIMaxAccum(slotId), iMaxAccum, RevConfigRegistry.EPSILON), + pidController -> + Math2.epsilonEquals(pidController.getIMaxAccum(slotId), iMaxAccum, RevConfigRegistry.EPSILON), "I Max Accumulator (Slot " + slotId + ")" ); return this; @@ -297,7 +288,8 @@ public SparkPIDControllerConfig setPositionPIDWrappingEnabled(boolean enable) { public SparkPIDControllerConfig setPositionPIDWrappingMaxInput(double max) { addStep( pidController -> pidController.setPositionPIDWrappingMaxInput(max), - pidController -> Math2.epsilonEquals(pidController.getPositionPIDWrappingMaxInput(), max, RevConfigRegistry.EPSILON), + pidController -> + Math2.epsilonEquals(pidController.getPositionPIDWrappingMaxInput(), max, RevConfigRegistry.EPSILON), "Position PID Wrapping Max Input" ); return this; @@ -310,7 +302,8 @@ public SparkPIDControllerConfig setPositionPIDWrappingMaxInput(double max) { public SparkPIDControllerConfig setPositionPIDWrappingMinInput(double min) { addStep( pidController -> pidController.setPositionPIDWrappingMinInput(min), - pidController -> Math2.epsilonEquals(pidController.getPositionPIDWrappingMinInput(), min, RevConfigRegistry.EPSILON), + pidController -> + Math2.epsilonEquals(pidController.getPositionPIDWrappingMinInput(), min, RevConfigRegistry.EPSILON), "Position PID Wrapping Min Input" ); return this; @@ -354,27 +347,6 @@ public SparkPIDControllerConfig setPID(double pGain, double iGain, double dGain, return this; } - /** - * Sets PIDF gains on the Spark Max. - * @param pidConfig The PID config object to apply. - */ - public SparkPIDControllerConfig setPID(PIDConfig pidfConfig) { - setPID(pidfConfig.p(), pidfConfig.i(), pidfConfig.d()); - setIZone(pidfConfig.iZone()); - return this; - } - - /** - * Sets PIDF gains on the Spark Max. - * @param pidConfig The PID config object to apply. - * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. - */ - public SparkPIDControllerConfig setPID(PIDConfig pidfConfig, int slotId) { - setPID(pidfConfig.p(), pidfConfig.i(), pidfConfig.d(), slotId); - setIZone(pidfConfig.iZone(), slotId); - return this; - } - /** * Sets PIDF gains on the Spark Max. * @param pGain The proportional gain value, must be positive. @@ -430,7 +402,11 @@ public SparkPIDControllerConfig setSmartMotionAllowedClosedLoopError(double allo addStep( pidController -> pidController.setSmartMotionAllowedClosedLoopError(allowedErr, slotId), pidController -> - Math2.epsilonEquals(pidController.getSmartMotionAllowedClosedLoopError(slotId), allowedErr, RevConfigRegistry.EPSILON), + Math2.epsilonEquals( + pidController.getSmartMotionAllowedClosedLoopError(slotId), + allowedErr, + RevConfigRegistry.EPSILON + ), "Smart Motion Allowed Closed Loop Error (Slot " + slotId + ")" ); return this; @@ -445,7 +421,8 @@ public SparkPIDControllerConfig setSmartMotionAllowedClosedLoopError(double allo public SparkPIDControllerConfig setSmartMotionMaxAccel(double maxAccel, int slotId) { addStep( pidController -> pidController.setSmartMotionMaxAccel(maxAccel, slotId), - pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxAccel(slotId), maxAccel, RevConfigRegistry.EPSILON), + pidController -> + Math2.epsilonEquals(pidController.getSmartMotionMaxAccel(slotId), maxAccel, RevConfigRegistry.EPSILON), "Smart Motion Max Acceleration (Slot " + slotId + ")" ); return this; @@ -460,7 +437,8 @@ public SparkPIDControllerConfig setSmartMotionMaxAccel(double maxAccel, int slot public SparkPIDControllerConfig setSmartMotionMaxVelocity(double maxVel, int slotId) { addStep( pidController -> pidController.setSmartMotionMaxVelocity(maxVel, slotId), - pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxVelocity(slotId), maxVel, RevConfigRegistry.EPSILON), + pidController -> + Math2.epsilonEquals(pidController.getSmartMotionMaxVelocity(slotId), maxVel, RevConfigRegistry.EPSILON), "Smart Motion Max Velocity (Slot " + slotId + ")" ); return this; @@ -474,7 +452,12 @@ public SparkPIDControllerConfig setSmartMotionMaxVelocity(double maxVel, int slo public SparkPIDControllerConfig setSmartMotionMinOutputVelocity(double minVel, int slotId) { addStep( pidController -> pidController.setSmartMotionMinOutputVelocity(minVel, slotId), - pidController -> Math2.epsilonEquals(pidController.getSmartMotionMinOutputVelocity(slotId), minVel, RevConfigRegistry.EPSILON), + pidController -> + Math2.epsilonEquals( + pidController.getSmartMotionMinOutputVelocity(slotId), + minVel, + RevConfigRegistry.EPSILON + ), "Smart Motion Min Velocity (Slot " + slotId + ")" ); return this; diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index a497ab0..6a24846 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -3,33 +3,23 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; -import com.revrobotics.CANSparkBase.ExternalFollower; -import com.revrobotics.CANSparkBase.IdleMode; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; +import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.SPI.Port; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import org.team340.lib.controller.Controller2Config; +import org.team340.lib.controller.ControllerConfig; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; -import org.team340.lib.swerve.hardware.motors.SwerveMotor; -import org.team340.lib.util.Math2; -import org.team340.lib.util.config.FeedForwardConfig; -import org.team340.lib.util.config.PIDConfig; -import org.team340.lib.util.config.rev.RelativeEncoderConfig; -import org.team340.lib.util.config.rev.SparkAbsoluteEncoderConfig; -import org.team340.lib.util.config.rev.SparkFlexConfig; -import org.team340.lib.util.config.rev.SparkLimitSwitchConfig; -import org.team340.lib.util.config.rev.SparkMaxConfig; -import org.team340.lib.util.config.rev.SparkPIDControllerConfig; +import org.team340.lib.swerve.hardware.SwerveEncoders; +import org.team340.lib.swerve.hardware.SwerveIMUs; +import org.team340.lib.swerve.hardware.SwerveMotors; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -38,452 +28,91 @@ */ public final class Constants { - public static final double PERIOD = 0.020; - public static final double TELEMETRY_PERIOD = 0.020; - public static final double POWER_USAGE_PERIOD = 0.020; - public static final double VOLTAGE = 12.0; - public static final double FIELD_LENGTH = 16.541; - public static final double FIELD_WIDTH = 8.211; + public static final double kPeriod = 0.020; + public static final double kVoltage = 12.0; + public static final double kFieldLength = 16.541; + public static final double kFieldWidth = 8.211; - /** - * Driver and co-driver controller constants. - */ - public static final class ControllerConstants { - - public static final double DRIVE_EXP = 1.0; - public static final double DRIVE_MULTIPLIER = 0.85; - public static final double DRIVE_MULTIPLIER_MODIFIED = 0.975; + public static final ControllerConfig kDriver = new ControllerConfig() + .setPort(0) + .setDeadbands(0.15, 0.05) + .setThresholds(0.5, 0.05); - public static final double DRIVE_ROT_EXP = 2.0; - public static final double DRIVE_ROT_MULTIPLIER = 0.4; - - public static final Controller2Config DRIVER = new Controller2Config() - .setLabel("Driver") - .setPort(0) - .setJoystickDeadband(0.15) - .setJoystickThreshold(0.7) - .setTriggerDeadband(0.1) - .setTriggerThreshold(0.1); - - public static final Controller2Config CO_DRIVER = new Controller2Config() - .setLabel("CoDriver") - .setPort(1) - .setJoystickDeadband(0.1) - .setJoystickThreshold(0.7) - .setTriggerDeadband(0.1) - .setTriggerThreshold(0.1); - } + public static final ControllerConfig kCoDriver = new ControllerConfig() + .setPort(1) + .setDeadbands(0.15, 0.05) + .setThresholds(0.5, 0.05); /** * Map of hardware device IDs. */ public static final class RobotMap { - public static final int FRONT_LEFT_MOVE = 2; - public static final int FRONT_LEFT_TURN = 3; - public static final int BACK_LEFT_MOVE = 4; - public static final int BACK_LEFT_TURN = 5; - public static final int BACK_RIGHT_MOVE = 6; - public static final int BACK_RIGHT_TURN = 7; - public static final int FRONT_RIGHT_MOVE = 8; - public static final int FRONT_RIGHT_TURN = 9; - - public static final int INTAKE_ARM_LEFT_MOTOR = 20; - public static final int INTAKE_ARM_RIGHT_MOTOR = 21; - public static final int INTAKE_ROLLER_UPPER_MOTOR = 22; - public static final int INTAKE_ROLLER_LOWER_MOTOR = 23; - - public static final int SHOOTER_PIVOT_MOTOR = 30; - public static final int SHOOTER_FEEDER_MOTOR = 31; - public static final int SHOOTER_SHOOT_LEFT_MOTOR = 32; - public static final int SHOOTER_SHOOT_RIGHT_MOTOR = 33; - - public static final int CLIMBER_LEFT_MOTOR = 40; - public static final int CLIMBER_RIGHT_MOTOR = 41; - - public static final int PIVOT_LOWER_LIMIT = 7; - public static final int SHOOTER_NOTE_DETECTOR = 8; - public static final int INTAKE_NOTE_DETECTOR = 9; - - public static final int LIGHTS = 9; - } - - public static final class ClimberConstants { - - // Limits - public static final double MIN_POS = 0.0; - public static final double MAX_POS = 120.0; - - // Speeds - public static final double CLIMBING_SPEED = 1.0; - - // Misc - public static final double CLOSED_LOOP_ERR = Math.toRadians(5.0); - public static final double BALANCE_COMPENSATION = 0.1; - - // Hardware Configs - public static final class Configs { - - // Relative Encoder Conversion Factor - private static final double REL_ENC_FACTOR = 1.0; - - public static final SparkFlexConfig MOTOR = new SparkFlexConfig() - .clearFaults() - .enableVoltageCompensation(VOLTAGE) - .setSmartCurrentLimit(40) - .setIdleMode(IdleMode.kBrake) - .setInverted(true) - .setClosedLoopRampRate(0.2) - .setOpenLoopRampRate(0.2); - - public static final RelativeEncoderConfig ENCODER = new RelativeEncoderConfig() - .setPositionConversionFactor(REL_ENC_FACTOR) - .setVelocityConversionFactor(REL_ENC_FACTOR / 60); - - public static final SparkLimitSwitchConfig LIMIT = new SparkLimitSwitchConfig().enableLimitSwitch(false); - } - } - - public static final class FeederConstants { - - // Speeds - public static final double RECEIVE_SPEED = 0.5; - public static final double INTAKE_HUMAN_SPEED = -0.1; - public static final double SEAT_SPEED = 0.05; - public static final double REVERSE_SEAT_SPEED = -0.04; - public static final double SHOOT_SPEED = 1.0; - public static final double BARF_FORWARD_SPEED = -0.5; - public static final double POOP_SPEED = -1.0; - public static final double BARF_BACKWARD_SPEED = 0.5; - - // Positions - public static final double SEAT_POSITION = 2.357; - - // Misc - public static final double CLOSED_LOOP_ERR = 0.125; - public static final double SHOOT_DELAY = 0.3; - - // Hardware Configs - public static final class Configs { - - // Relative Encoder Conversion Factor - private static final double REL_ENC_FACTOR = 1.0; - - public static final SparkMaxConfig MOTOR = new SparkMaxConfig() - .clearFaults() - .enableVoltageCompensation(VOLTAGE) - .setSmartCurrentLimit(30) - .setIdleMode(IdleMode.kBrake) - .setInverted(true) - .setClosedLoopRampRate(0.0) - .setOpenLoopRampRate(0.0); - - public static final RelativeEncoderConfig ENCODER = new RelativeEncoderConfig() - .setPositionConversionFactor(REL_ENC_FACTOR) - .setVelocityConversionFactor(REL_ENC_FACTOR / 60); - - public static final SparkPIDControllerConfig PID = new SparkPIDControllerConfig().setPID(0.15, 0.0, 0.0); - } - } - - public static final class IntakeConstants { - - // Limits - public static final double MIN_POS = 0.0; - public static final double MAX_POS = Math.toRadians(135.0); - - // Speeds - public static final double INTAKE_SPEED = 0.9; - public static final double HANDOFF_SPEED = -0.25; - public static final double AMP_UPPER_SPEED = -0.6; - public static final double AMP_LOWER_SPEED = -0.075; - public static final double BARF_SPEED = -0.5; - public static final double POOP_SPEED = -1.0; - public static final double OVERRIDE_INTAKE_SPEED = 0.25; - - // Positions - public static final double DOWN_POSITION = Math.toRadians(0.0); - public static final double HANDOFF_POSITION = Math.toRadians(0.0); - public static final double SAFE_POSITION = Math.toRadians(30.0); - public static final double POOP_POSITION = Math.toRadians(25.0); - public static final double RETRACT_POSITION = Math.toRadians(65.0); - public static final double UPRIGHT_POSITION = Math.toRadians(90.0); - public static final double BARF_POSITION = Math.toRadians(10.0); - public static final double AMP_POSITION = Math.toRadians(56.0); - public static final double PID_INACTIVE_POSITION = Math.toRadians(2.0); - - // Misc - public static final double CLOSED_LOOP_ERR = Math.toRadians(5.0); - public static final double ALLOWABLE_DIFFERENCE = Math.toRadians(15.0); - - // Arm Hardware Configs - public static final class ArmConfigs { - - // Relative Encoder Conversion Factor - private static final double REL_ENC_FACTOR = Math2.TWO_PI; - - private static final SparkFlexConfig MOTOR_BASE = new SparkFlexConfig() - .clearFaults() - .enableVoltageCompensation(VOLTAGE) - .setSmartCurrentLimit(60, 30) - .setIdleMode(IdleMode.kBrake) - .setInverted(true) - .setClosedLoopRampRate(0.35) - .setOpenLoopRampRate(0.35); - - public static final SparkFlexConfig LEFT_MOTOR = MOTOR_BASE.clone(); - public static final SparkFlexConfig RIGHT_MOTOR = MOTOR_BASE - .clone() - .follow(ExternalFollower.kFollowerSpark, RobotMap.INTAKE_ARM_LEFT_MOTOR, false); - - public static final SparkAbsoluteEncoderConfig ENCODER = new SparkAbsoluteEncoderConfig() - .setPositionConversionFactor(REL_ENC_FACTOR) - .setVelocityConversionFactor(REL_ENC_FACTOR / 60.0) - .setInverted(true) - .setZeroOffset(4.843); - - public static final SparkPIDControllerConfig PID = new SparkPIDControllerConfig() - .setPID(1.0, 0.0015, 0.02) - .setIZone(Math.toRadians(5.25)) - .setPositionPIDWrappingEnabled(true) - .setPositionPIDWrappingInputLimits(0.0, REL_ENC_FACTOR); - } - - // Roller Hardware Configs - public static final class RollerConfigs { - - public static final SparkMaxConfig MOTOR = new SparkMaxConfig() - .clearFaults() - .enableVoltageCompensation(VOLTAGE) - .setSmartCurrentLimit(30) - .setIdleMode(IdleMode.kCoast) - .setInverted(true) - .setClosedLoopRampRate(0.0) - .setOpenLoopRampRate(0.0); - } - } - - public static final class LightsConstants { - - public static final int LENGTH = 126; - } - - public static final class PivotConstants { - - // Limits - public static final double MIN_POS = 0.0; - public static final double MAX_POS = Math.toRadians(89.0); - - // Speeds - public static final double HOMING_SPEED = -0.2; - - // Positions - public static final double BARF_FORWARD_POSITION = Math.toRadians(0.0); - public static final double DOWN_POSITION = Math.toRadians(2.0); - public static final double ROCK_SKIP_POSITION = Math.toRadians(0.25); - public static final double MARY_POPPINS_POSITION = Math.toRadians(45.0); - public static final double AMP_HANDOFF_POSITION = Math.toRadians(0.0); - public static final double FIX_DEADZONE_POSITION = Math.toRadians(57.5); - public static final double INTAKE_SAFE_POSITION = Math.toRadians(60.0); - - // Misc - public static final double CLOSED_LOOP_ERR = Math.toRadians(0.015); - public static final double AT_LIMIT_SPEED_ALLOWANCE = -0.075; - - // Hardware Configs - public static final class Configs { - - // Relative Encoder Conversion Factor - private static final double REL_ENC_FACTOR = Math.toRadians(1.02432); - - public static final SparkFlexConfig MOTOR = new SparkFlexConfig() - .clearFaults() - .enableVoltageCompensation(VOLTAGE) - .setSmartCurrentLimit(60, 30) - .setIdleMode(IdleMode.kCoast) - .setInverted(true) - .setClosedLoopRampRate(0.25) - .setOpenLoopRampRate(0.25); - - public static final SparkPIDControllerConfig PID = new SparkPIDControllerConfig() - .setPID(2.3, 0.004, 2.7) - .setIZone(Math.toRadians(2.0)); - - public static final RelativeEncoderConfig ENCODER = new RelativeEncoderConfig() - .setPositionConversionFactor(REL_ENC_FACTOR) - .setVelocityConversionFactor(REL_ENC_FACTOR / 60); - } - - // Shooting position lookup table - public static final InterpolatingDoubleTreeMap DISTANCE_MAP = new InterpolatingDoubleTreeMap(); - - static { - DISTANCE_MAP.put(1.31, Math.toRadians(59.01)); - DISTANCE_MAP.put(1.53, Math.toRadians(52.90)); - DISTANCE_MAP.put(1.81, Math.toRadians(46.82)); - DISTANCE_MAP.put(2.09, Math.toRadians(42.10)); - DISTANCE_MAP.put(2.09, Math.toRadians(42.10)); - DISTANCE_MAP.put(2.71, Math.toRadians(37.76)); - DISTANCE_MAP.put(2.83, Math.toRadians(35.65)); - DISTANCE_MAP.put(3.05, Math.toRadians(33.54)); - DISTANCE_MAP.put(3.16, Math.toRadians(33.38)); - DISTANCE_MAP.put(3.45, Math.toRadians(31.25)); - DISTANCE_MAP.put(4.09, Math.toRadians(27.74)); - DISTANCE_MAP.put(4.40, Math.toRadians(26.46)); - DISTANCE_MAP.put(4.44, Math.toRadians(26.39)); - DISTANCE_MAP.put(4.60, Math.toRadians(25.72)); - DISTANCE_MAP.put(4.84, Math.toRadians(23.27)); - DISTANCE_MAP.put(5.07, Math.toRadians(23.15)); - DISTANCE_MAP.put(5.38, Math.toRadians(21.75)); - DISTANCE_MAP.put(5.63, Math.toRadians(21.12)); - DISTANCE_MAP.put(5.94, Math.toRadians(20.39)); - DISTANCE_MAP.put(6.38, Math.toRadians(19.61)); - DISTANCE_MAP.put(6.55, Math.toRadians(20.7)); - DISTANCE_MAP.put(6.87, Math.toRadians(19.76)); - DISTANCE_MAP.put(7.55, Math.toRadians(18.0)); - DISTANCE_MAP.put(8.88, Math.toRadians(16.9)); - DISTANCE_MAP.put(9.71, Math.toRadians(16.88)); - // DISTANCE_MAP.put(1.44, Math.toRadians(55.0)); - // DISTANCE_MAP.put(1.61, Math.toRadians(52.0)); - // DISTANCE_MAP.put(1.99, Math.toRadians(48.0)); - // DISTANCE_MAP.put(2.34, Math.toRadians(42.0)); - // DISTANCE_MAP.put(2.87, Math.toRadians(35.0)); - // DISTANCE_MAP.put(3.34, Math.toRadians(32.0)); - // DISTANCE_MAP.put(3.92, Math.toRadians(29.0)); - // DISTANCE_MAP.put(4.28, Math.toRadians(27.0)); - // DISTANCE_MAP.put(5.32, Math.toRadians(23.0)); - // DISTANCE_MAP.put(5.82, Math.toRadians(21.75)); - // DISTANCE_MAP.put(6.55, Math.toRadians(20.7)); - // DISTANCE_MAP.put(6.87, Math.toRadians(19.76)); - // DISTANCE_MAP.put(7.55, Math.toRadians(18.0)); - // DISTANCE_MAP.put(8.88, Math.toRadians(16.9)); - // DISTANCE_MAP.put(9.71, Math.toRadians(16.88)); - } - } - - public static final class ShooterConstants { - - // Speeds - public static final double RAMP_UP_SPEED = 0.95; - public static final double RAMP_DOWN_SPEED = 0.0; - public static final double INTAKE_HUMAN_SPEED = -0.175; - public static final double FIX_DEADZONE_SPEED = -0.5; - public static final double ROCK_SKIP_SPEED = 0.6; - public static final double MARY_POPPINS_SPEED = 0.24; - public static final double FORWARD_BARF_SPEED = -0.5; - public static final double BACKWARD_BARF_SPEED = 0.5; - - // Misc - public static final double CLOSED_LOOP_ERR = 40.0; - public static final double PID_ACTIVE_RANGE = 750.0; - public static final double RIGHT_PERCENT_OF_LEFT = 0.55; - - // Hardware Configs - public static final class Configs { - - // Relative Encoder Conversion Factor - private static final double REL_ENC_CONVERSION = 2.0; - - private static final SparkFlexConfig MOTOR_BASE = new SparkFlexConfig() - .clearFaults() - .enableVoltageCompensation(VOLTAGE) - .setSmartCurrentLimit(50, 35) - .setIdleMode(IdleMode.kCoast) - .setClosedLoopRampRate(0.0) - .setOpenLoopRampRate(0.0); - - public static final SparkFlexConfig LEFT_MOTOR = MOTOR_BASE.clone().setInverted(true); - public static final SparkFlexConfig RIGHT_MOTOR = MOTOR_BASE.clone().setInverted(false); - - public static final SparkPIDControllerConfig PID = new SparkPIDControllerConfig() - .setPID(0.00047, 0.000002, 0.0) - .setIZone(100.0); - - public static final RelativeEncoderConfig ENCODER = new RelativeEncoderConfig() - .setPositionConversionFactor(REL_ENC_CONVERSION) - .setVelocityConversionFactor(REL_ENC_CONVERSION); - - public static final FeedForwardConfig FEED_FORWARD_LEFT = new FeedForwardConfig( - 0.11331 / 60.0, - 0.065448 / 60.0, - 0.076179 / 60.0 - ); - public static final FeedForwardConfig FEED_FORWARD_RIGHT = new FeedForwardConfig( - 0.11331 / 60.0, - 0.064905 / 60.0, - 0.071392 / 60.0 - ); - public static final SysIdRoutine.Config SYSID = new SysIdRoutine.Config(); - } - - // Shooting speed lookup table - public static final InterpolatingDoubleTreeMap DISTANCE_MAP = new InterpolatingDoubleTreeMap(); - - static { - DISTANCE_MAP.put(0.0, 3000.0); - DISTANCE_MAP.put(6.0, 6500.0); - DISTANCE_MAP.put(10.0, 7750.0); - } + public static final int kMoveFL = 2; + public static final int kTurnFL = 3; + public static final int kMoveBL = 4; + public static final int kTurnBL = 5; + public static final int kMoveBR = 6; + public static final int kTurnBR = 7; + public static final int kMoveFR = 8; + public static final int kTurnFR = 9; + + public static final int kIntakePivotMotor = 20; + public static final int kIntakeRollerMotor = 21; + + public static final int kPivotMotor = 30; + public static final int kFeederMotor = 31; + public static final int kShooterLeftMotor = 32; + public static final int kShooterRightMotor = 33; + + public static final int kPivotLimit = 8; + public static final int kNoteDetector = 9; } public static final class SwerveConstants { private static final SwerveModuleConfig FRONT_LEFT = new SwerveModuleConfig() - .setLabel("Front Left") - .useSparkAttachedEncoder(2.389, true) - .setPosition(0.288925, 0.288925) - .setMoveMotor(RobotMap.FRONT_LEFT_MOVE, true, true) - .setTurnMotor(RobotMap.FRONT_LEFT_TURN, false, true); + .setName("frontLeft") + .setLocation(0.288925, 0.288925) + .setMoveMotor(SwerveMotors.sparkFlex(RobotMap.kMoveFL, MotorType.kBrushless, true)) + .setTurnMotor(SwerveMotors.sparkFlex(RobotMap.kTurnFL, MotorType.kBrushless, true)) + .setEncoder(SwerveEncoders.sparkFlexEncoder(0.3802, true)); + + private static final SwerveModuleConfig FRONT_RIGHT = new SwerveModuleConfig() + .setName("frontRight") + .setLocation(0.288925, -0.288925) + .setMoveMotor(SwerveMotors.sparkFlex(RobotMap.kMoveFR, MotorType.kBrushless, true)) + .setTurnMotor(SwerveMotors.sparkFlex(RobotMap.kTurnFR, MotorType.kBrushless, true)) + .setEncoder(SwerveEncoders.sparkFlexEncoder(0.8859, true)); private static final SwerveModuleConfig BACK_LEFT = new SwerveModuleConfig() - .setLabel("Back Left") - .useSparkAttachedEncoder(1.999, true) - .setPosition(-0.288925, 0.288925) - .setMoveMotor(RobotMap.BACK_LEFT_MOVE, true, true) - .setTurnMotor(RobotMap.BACK_LEFT_TURN, false, true); + .setName("backLeft") + .setLocation(-0.288925, 0.288925) + .setMoveMotor(SwerveMotors.sparkFlex(RobotMap.kMoveBL, MotorType.kBrushless, true)) + .setTurnMotor(SwerveMotors.sparkFlex(RobotMap.kTurnBL, MotorType.kBrushless, true)) + .setEncoder(SwerveEncoders.sparkFlexEncoder(0.3182, true)); private static final SwerveModuleConfig BACK_RIGHT = new SwerveModuleConfig() - .setLabel("Back Right") - .useSparkAttachedEncoder(4.194, true) - .setPosition(-0.288925, -0.288925) - .setMoveMotor(RobotMap.BACK_RIGHT_MOVE, true, true) - .setTurnMotor(RobotMap.BACK_RIGHT_TURN, false, true); - - private static final SwerveModuleConfig FRONT_RIGHT = new SwerveModuleConfig() - .setLabel("Front Right") - .useSparkAttachedEncoder(5.566, true) - .setPosition(0.288925, -0.288925) - .setMoveMotor(RobotMap.FRONT_RIGHT_MOVE, true, true) - .setTurnMotor(RobotMap.FRONT_RIGHT_TURN, false, true); + .setName("backRight") + .setLocation(-0.288925, -0.288925) + .setMoveMotor(SwerveMotors.sparkFlex(RobotMap.kMoveBR, MotorType.kBrushless, true)) + .setTurnMotor(SwerveMotors.sparkFlex(RobotMap.kTurnBR, MotorType.kBrushless, true)) + .setEncoder(SwerveEncoders.sparkFlexEncoder(0.6675, true)); public static final SwerveConfig CONFIG = new SwerveConfig() - .useADIS16470(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, Port.kOnboardCS0, CalibrationTime._4s) - .setPeriod(PERIOD) - .setMovePID(0.302, 0.0024, 0.003, 0.075) - .setMoveFF(0.13312, 2.3443, 0.3159) - .setTurnPID(0.65, 0.001, 3.0, 0.01) - .setRampRate(0.03, 0.03) - .setMotorTypes(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS, SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) - .setMaxSpeeds(4.95, 11.8) - .setRatelimits(8.2, 29.75) - .setTrajectoryConstraints(3.86, 2.4) - .setPowerProperties(VOLTAGE, 60.0, 40.0) - .setMechanicalProperties(6.75, 150.0 / 7.0, 3.82) - .setDiscretizationLookahead(0.040) - .setOdometryPeriod(PERIOD) - .setOdometryStd(0.003, 0.003, 0.0012) - .setVisionStd(0.0, 0.0, 0.0) - .setSysIdConfig(new SysIdRoutine.Config(Volts.of(1.0).per(Seconds.of(0.4)), Volts.of(7.0), Seconds.of(5.5))) - .setFieldSize(FIELD_LENGTH, FIELD_WIDTH) - .addModule(FRONT_LEFT) - .addModule(BACK_LEFT) - .addModule(BACK_RIGHT) - .addModule(FRONT_RIGHT); + .setTimings(Constants.kPeriod, Constants.kPeriod, 0.04) + .setMovePID(0.01, 0.0, 0.0, 0.0) + .setMoveFF(0.0221, 0.1068) + .setTurnPID(0.21, 0.0, 0.1, 0.0) + .setBrakeMode(false, true) + .setLimits(4.9, 13.0, 7.0, 27.5) + .setDriverProfile(4.3, 1.0, 4.2, 2.0) + .setPowerProperties(Constants.kVoltage, 65.0, 40.0) + .setMechanicalProperties(6.75, 150.0 / 7.0, 0.0, Units.inchesToMeters(4.0)) + .setOdometryStd(0.05, 0.05, 0.01) + .setIMU(SwerveIMUs.adis16470(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, Port.kOnboardCS0, CalibrationTime._4s)) + .setModules(FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); - public static final Transform3d FRONT_LEFT_CAMERA = new Transform3d( - new Translation3d(0.28364, 0.28369, 0.24511), - new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(45.0)) - ); public static final Transform3d BACK_LEFT_CAMERA = new Transform3d( new Translation3d(-0.29153, 0.26629, 0.24511), new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(-160.0)) @@ -492,9 +121,11 @@ public static final class SwerveConstants { new Translation3d(-0.29153, -0.26629, 0.24511), new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(160.0)) ); - public static final Transform3d FRONT_RIGHT_CAMERA = new Transform3d( - new Translation3d(0.28364, -0.28369, 0.24511), - new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(-45.0)) + + public static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( + Volts.of(1.0).per(Seconds.of(0.4)), + Volts.of(7.0), + Seconds.of(5.5) ); public static final double VISION_FIELD_MARGIN = 0.5; @@ -502,60 +133,35 @@ public static final class SwerveConstants { public static final double VISION_STD_XY_SCALE = 0.006; public static final double VISION_STD_ROT_SCALE = 0.015; - public static final PIDConfig TRAJ_XY_PID = new PIDConfig(19.6, 0.0, 0.0); - public static final PIDConfig TRAJ_ROT_PID = new PIDConfig(6.9, 0.0, 0.0); - public static final Constraints TRAJ_ROT_CONSTRAINTS = new Constraints(6.5, 7.0); - public static final PIDConfig TRAJ_TARGET_PID = new PIDConfig(7.25, 0.0, 0.15); - - public static final PIDConfig XY_PID = new PIDConfig(6.5, 1.2, 0.05, 0.5); - public static final PIDConfig ROT_PID = new PIDConfig(4.9, 0.5, 0.2, 0.2); - public static final Constraints ROT_CONSTRAINTS = new Constraints(8.0, 37.5); + public static final double ROT_PID_KP = 10.0; + public static final double ROT_PID_KI = 0.5; + public static final double ROT_PID_KD = 0.0; + public static final double ROT_PID_IZONE = 0.8; + public static final double ROT_PID_DEADBAND = 0.15; + public static final Constraints ROT_CONSTRAINTS = new Constraints(8.0, 27.0); public static final double NOTE_VELOCITY = 5.6; public static final double NORM_FUDGE = 0.49; public static final double STRAFE_FUDGE = 0.85; public static final double SPIN_COMPENSATION = Math.toRadians(-2.0); - public static final double DISTANCE_FUDGE_BLUE = 0.16; - public static final double DISTANCE_FUDGE_RED = 0.1; public static final double NORM_FUDGE_MIN = 0.075; - public static final double FACING_SPEAKER_EPSILON = Math.toRadians(5.0); - - public static final double DRIVE_BASE_RADIUS = Math.hypot(FRONT_LEFT.getPosition().getX(), FRONT_LEFT.getPosition().getY()); } public static final class FieldPositions { - public static final Translation2d BLUE_SPEAKER = new Translation2d(-0.04, 5.5479); - public static final Translation2d RED_SPEAKER = new Translation2d(0.03, FIELD_WIDTH - BLUE_SPEAKER.getY()); - - public static final double SPEAKER_HEIGHT = 2.08; - public static final Pose3d BLUE_SPEAKER_3D = new Pose3d( - BLUE_SPEAKER.getX(), - BLUE_SPEAKER.getY(), - SPEAKER_HEIGHT, - Math2.ROTATION3D_0 + public static final Translation2d kBlueSpeaker = new Translation2d(-0.04, 5.5479); + public static final Translation2d kRedSpeaker = new Translation2d( + kFieldLength - kBlueSpeaker.getX(), + kBlueSpeaker.getY() ); - public static final Pose3d RED_SPEAKER_3D = new Pose3d(RED_SPEAKER.getX(), RED_SPEAKER.getY(), SPEAKER_HEIGHT, Math2.ROTATION3D_0); - public static final double AMP_X = 1.79; - public static final Pose2d AMP_APPROACH_BLUE = new Pose2d(AMP_X, 6.94, Math2.ROTATION2D_HALF_PI); - public static final Pose2d AMP_APPROACH_RED = new Pose2d( - AMP_X, - FIELD_WIDTH - AMP_APPROACH_BLUE.getY(), - Math2.ROTATION2D_NEG_HALF_PI + public static final Translation2d kBlueFeed = new Translation2d(0.0, 7.4); + public static final Translation2d kRedFeed = new Translation2d( + kFieldLength - kBlueFeed.getX(), + kBlueFeed.getY() ); - public static final Pose2d AMP_SCORE_BLUE = new Pose2d(AMP_X, 7.7, Math2.ROTATION2D_HALF_PI); - public static final Pose2d AMP_SCORE_RED = new Pose2d(AMP_X, 0.48, Math2.ROTATION2D_NEG_HALF_PI); - - public static final Translation2d STAGE = new Translation2d(4.981067, 4.105783); - - public static final Translation2d FEED_BLUE = new Translation2d(0.0, 7.4); - public static final Translation2d FEED_RED = new Translation2d(FEED_BLUE.getX(), FIELD_WIDTH - FEED_BLUE.getY()); - - public static final double OPPONENT_WING_LINE = 10.66; - public static final double MIDLINE = FIELD_LENGTH / 2.0; - public static final double FENDER_SHOT_DISTANCE = 1.4; + public static final double kFenderShotDistance = 1.4; } } diff --git a/src/main/java/org/team340/robot/Robot.java b/src/main/java/org/team340/robot/Robot.java index 0077e70..26f48b9 100644 --- a/src/main/java/org/team340/robot/Robot.java +++ b/src/main/java/org/team340/robot/Robot.java @@ -1,12 +1,14 @@ package org.team340.robot; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import org.team340.lib.GRRDashboard; -import org.team340.lib.util.config.rev.RevConfigRegistry; +import org.team340.lib.dashboard.GRRDashboard; +import org.team340.lib.util.Profiler; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -14,52 +16,70 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ +@Logged public final class Robot extends TimedRobot { - public Robot() { - super(Constants.PERIOD); - } + private final RobotContainer robotContainer; + private Command autoCommand; - @Override - public void robotInit() { - LiveWindow.setEnabled(false); - enableLiveWindowInTest(false); - LiveWindow.disableAllTelemetry(); + public Robot() { + super(Constants.kPeriod); DriverStation.silenceJoystickConnectionWarning(true); + // Start logging DataLogManager.start(); - DataLogManager.logNetworkTables(true); DriverStation.startDataLog(DataLogManager.getLog()); - GRRDashboard.initSync(this, Constants.TELEMETRY_PERIOD, Constants.POWER_USAGE_PERIOD); - RevConfigRegistry.init(this); - RobotContainer.init(); + Epilogue.configure(config -> { + config.root = "Telemetry"; + }); + + robotContainer = new RobotContainer(); } @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); + Profiler.start("RobotPeriodic"); + Profiler.run("CommandScheduler", () -> CommandScheduler.getInstance().run()); + Profiler.run("Epilogue", () -> Epilogue.update(this)); + Profiler.run("GRRDashboard", GRRDashboard::update); + Profiler.end(); } + @Override + public void simulationPeriodic() {} + @Override public void disabledInit() {} @Override public void disabledPeriodic() {} + @Override + public void disabledExit() {} + @Override public void autonomousInit() { - GRRDashboard.getAutoCommand().schedule(); + autoCommand = GRRDashboard.getSelectedAuto(); + CommandScheduler.getInstance().schedule(autoCommand); } @Override public void autonomousPeriodic() {} + @Override + public void autonomousExit() { + if (autoCommand != null) autoCommand.cancel(); + } + @Override public void teleopInit() {} @Override public void teleopPeriodic() {} + @Override + public void teleopExit() {} + @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); @@ -69,8 +89,5 @@ public void testInit() { public void testPeriodic() {} @Override - public void simulationInit() {} - - @Override - public void simulationPeriodic() {} + public void testExit() {} } diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java index cbf12d9..fee9b10 100644 --- a/src/main/java/org/team340/robot/RobotContainer.java +++ b/src/main/java/org/team340/robot/RobotContainer.java @@ -2,150 +2,116 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import com.choreo.lib.Choreo; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import org.team340.lib.GRRDashboard; -import org.team340.lib.controller.Controller2; -import org.team340.lib.util.Math2; -import org.team340.lib.util.config.rev.RevConfigRegistry; -import org.team340.robot.Constants.ControllerConstants; +import org.team340.lib.controller.Controller; +import org.team340.lib.util.rev.RevConfigRegistry; import org.team340.robot.Constants.FieldPositions; -import org.team340.robot.Constants.PivotConstants; -import org.team340.robot.commands.Autos; import org.team340.robot.commands.Routines; -import org.team340.robot.subsystems.Climber; import org.team340.robot.subsystems.Feeder; +import org.team340.robot.subsystems.Feeder.FeederSpeed; import org.team340.robot.subsystems.Intake; -import org.team340.robot.subsystems.Lights; +import org.team340.robot.subsystems.Intake.IntakeState; import org.team340.robot.subsystems.Pivot; +import org.team340.robot.subsystems.Pivot.PivotPosition; import org.team340.robot.subsystems.Shooter; +import org.team340.robot.subsystems.Shooter.ShooterSpeed; import org.team340.robot.subsystems.Swerve; /** * This class is used to declare subsystems, commands, and trigger mappings. */ +@Logged public final class RobotContainer { - private RobotContainer() { - throw new UnsupportedOperationException("This is a utility class!"); - } + private Controller driver; + private Controller coDriver; - private static Controller2 driver; - private static Controller2 coDriver; + public Feeder feeder; + public Intake intake; + public Pivot pivot; + public Shooter shooter; + public Swerve swerve; - public static Climber climber; - public static Feeder feeder; - public static Intake intake; - public static Lights lights; - public static Pivot pivot; - public static Shooter shooter; - public static Swerve swerve; + @NotLogged + public Routines routines; /** * Entry to initializing subsystems and command execution. */ - public static void init() { + public RobotContainer() { // Initialize controllers. - driver = new Controller2(ControllerConstants.DRIVER); - coDriver = new Controller2(ControllerConstants.CO_DRIVER); - - // Add controllers to the dashboard. - driver.addToDashboard(); - coDriver.addToDashboard(); + driver = new Controller(Constants.kDriver); + coDriver = new Controller(Constants.kCoDriver); // Initialize subsystems. - climber = new Climber(); feeder = new Feeder(); intake = new Intake(); - lights = new Lights(); pivot = new Pivot(); shooter = new Shooter(); swerve = new Swerve(); - // Add subsystems to the dashboard. - climber.addToDashboard(); - feeder.addToDashboard(); - intake.addToDashboard(); - lights.addToDashboard(); - pivot.addToDashboard(); - shooter.addToDashboard(); - swerve.addToDashboard(); - - // Set systems check command. - // GRRDashboard.setSystemsCheck(SystemsCheck.command()); + routines = new Routines(this); // Complete REV hardware initialization. - RevConfigRegistry.burnFlash(); - RevConfigRegistry.printError(); + RevConfigRegistry.burnFlashAll(); // Configure bindings and autos. configBindings(); - configAutos(); } /** * This method should be used to declare triggers (created with an * arbitrary predicate or from controllers) and their bindings. */ - private static void configBindings() { + private void configBindings() { // Set default commands. - intake.setDefaultCommand(intake.maintainPosition()); - lights.setDefaultCommand(lights.defaultCommand(intake::hasNote, feeder::hasNote)); - pivot.setDefaultCommand(pivot.maintainPosition()); - shooter.setDefaultCommand(shooter.targetDistance(swerve::getSpeakerDistance, 2750.0, swerve::inOpponentWing)); - swerve.setDefaultCommand(swerve.drive(RobotContainer::getDriveX, RobotContainer::getDriveY, RobotContainer::getDriveRotate, true)); + intake.setDefaultCommand(intake.apply(IntakeState.kRetract)); + pivot.setDefaultCommand(pivot.apply(PivotPosition.kDown)); + swerve.setDefaultCommand(swerve.drive(driver::getLeftX, driver::getLeftY, driver::getTriggerDifference)); - Routines.onDisable().schedule(); - RobotModeTriggers.disabled().onTrue(Routines.onDisable()); - RobotModeTriggers.teleop().onTrue(swerve.funAndGames()); + routines.onDisable().schedule(); + RobotModeTriggers.disabled().onTrue(routines.onDisable()); /** * Driver bindings. */ // A => Intake (Tap = Down, Hold = Run roller) - driver.a().whileTrue(Routines.intake()).onFalse(Routines.finishIntake()); + driver.a().whileTrue(none()); // B => Intake from Human Player (Hold) - driver.b().onTrue(Routines.intakeHuman(RobotContainer::getDriveX, RobotContainer::getDriveY)).onFalse(Routines.finishIntakeHuman()); + driver.b().whileTrue(routines.humanLoad()).onFalse(pivot.apply(PivotPosition.kDown)); // X => Prep Amp (Hold) - driver.x().whileTrue(Routines.prepAmp(RobotContainer::getDriveX, RobotContainer::getDriveY)); + driver.x().whileTrue(parallel(pivot.apply(PivotPosition.kAmp), shooter.apply(ShooterSpeed.kAmp))); // Y => Shoot (Tap) - driver.y().whileTrue(feeder.shoot()); + driver.y().whileTrue(feeder.apply(FeederSpeed.kShoot)); // Right Joystick Up => Protect intake - driver.rightJoystickUp().onTrue(intake.retractPosition()); + driver.rightJoystickUp().onTrue(none()); // Right Joystick Down => Intake down - driver.rightJoystickDown().onTrue(intake.safePosition()); + driver.rightJoystickDown().onTrue(none()); // Right Bumper => Prep Speaker (Hold) - driver.rightBumper().whileTrue(Routines.prepSpeaker(RobotContainer::getDriveX, RobotContainer::getDriveY)); + driver.rightBumper().whileTrue(routines.prepSpeaker(driver::getLeftX, driver::getLeftY)); // Left Bumper => Prep Feed (Hold) - driver.leftBumper().whileTrue(Routines.prepFeed(RobotContainer::getDriveX, RobotContainer::getDriveY)); + driver.leftBumper().whileTrue(routines.prepFeed(driver::getLeftX, driver::getLeftY)); // POV Up => Barf Forwards (Hold) - driver.povUp().whileTrue(Routines.barfForward()); + driver.povUp().whileTrue(none()); - // POV Down => Pivot Down (Tap) - driver.povDown().onTrue(pivot.goTo(PivotConstants.DOWN_POSITION)); + // POV Down => Pivot home (Tap) + driver.povDown().onTrue(pivot.home(true)); // POV Left => Zero swerve - driver.povLeft().onTrue(swerve.zeroIMU(Math2.ROTATION2D_0)); - - // POV Right => Prep Climb (Hold) - driver.povRight().whileTrue(Routines.prepClimb(RobotContainer::getDriveX, RobotContainer::getDriveY)); - - // Start => Toggle Shooter - driver.start().toggleOnTrue(shooter.setSpeed(0.0)); - - // Back => Dump Odometry - driver.back().onTrue(swerve.dumpOdometry()); + driver.povLeft().onTrue(swerve.tareRotation()); /** * Co-driver bindings. @@ -156,109 +122,41 @@ private static void configBindings() { .and(coDriver.rightBumper()) .whileTrue( parallel( - intake.driveArmManual(() -> -coDriver.getLeftY() * 0.5), - pivot.driveManual(() -> -coDriver.getRightY() * 0.4), - shooter.driveManual(() -> -coDriver.getRightX() * 500.0) - ) - .withName("coDriver.leftBumper().and(coDriver.rightBumper()).whileTrue()") + intake.manual(() -> -coDriver.getLeftY() * 0.5), + pivot.manual(() -> -coDriver.getRightY() * 0.4), + shooter.manual(() -> -coDriver.getRightX() * 500.0) + ).withName("coDriver.leftBumper().and(coDriver.rightBumper()).whileTrue()") ); - // A => Climb (Hold) - coDriver.a().whileTrue(climber.climb(swerve::getRoll)); + // A => Nothing + coDriver.a().whileTrue(none()); // B => Overrides intake (Hold) - coDriver.b().whileTrue(Routines.intakeOverride()); + coDriver.b().whileTrue(none()); // X => Fender Shot (Hold) - coDriver.x().whileTrue(pivot.targetDistance(() -> FieldPositions.FENDER_SHOT_DISTANCE)); - - // Y => Score Amp (Hold) - coDriver.y().whileTrue(intake.scoreAmp()); + coDriver.x().whileTrue(pivot.targetSpeaker(() -> FieldPositions.kFenderShotDistance)); - // Both Triggers => Drives climber manually - coDriver - .leftTrigger() - .and(coDriver.rightTrigger()) - .whileTrue(climber.driveManual(() -> -coDriver.getLeftY() * 0.3, () -> -coDriver.getRightY() * 0.3)); + // Y => Nothing + coDriver.y().whileTrue(none()); // POV Up => Fix Deadzone (Hold) - coDriver.povUp().whileTrue(Routines.fixDeadzone()); + coDriver.povUp().whileTrue(routines.fixDeadzone()); // POV Down => Pivot Home (Hold) coDriver.povDown().whileTrue(pivot.home(true)); - // POV Left => Prep Speaker (Hold) - coDriver.povLeft().whileTrue(Routines.prepSpeaker(RobotContainer::getDriveX, RobotContainer::getDriveY)); - // Back / Start => he he rumble rumble coDriver.back().toggleOnTrue(setCoDriverRumble(RumbleType.kLeftRumble, 0.5).ignoringDisable(true)); coDriver.start().toggleOnTrue(setCoDriverRumble(RumbleType.kRightRumble, 0.5).ignoringDisable(true)); } - /** - * Autonomous commands should be declared here and - * added to {@link GRRDashboard}. - */ - private static void configAutos() { - var fourPieceClose = Choreo.getTrajectoryGroup("FourPieceClose"); - GRRDashboard.addAutoCommand("Four Piece Close", fourPieceClose, Autos.fourPieceClose(fourPieceClose)); - - var fivePieceAmp = Choreo.getTrajectoryGroup("FivePieceAmp"); - GRRDashboard.addAutoCommand("Five Piece Amp", fivePieceAmp, Autos.fivePieceAmp(fivePieceAmp)); - - var fourPieceFar = Choreo.getTrajectoryGroup("FourPieceFar"); - GRRDashboard.addAutoCommand("Four Piece Far", fourPieceFar, Autos.fourPieceFar(fourPieceFar)); - - var fourPieceSource12 = Choreo.getTrajectoryGroup("FourPieceSource12"); - GRRDashboard.addAutoCommand("Four Piece Source: 1, 2", fourPieceSource12, Autos.fourPieceSource(fourPieceSource12)); - - var fourPieceSource13 = Choreo.getTrajectoryGroup("FourPieceSource13"); - GRRDashboard.addAutoCommand("Four Piece Source: 1, 3", fourPieceSource13, Autos.fourPieceSource(fourPieceSource13)); - - var fourPieceSource21 = Choreo.getTrajectoryGroup("FourPieceSource21"); - GRRDashboard.addAutoCommand("Four Piece Source: 2, 1", fourPieceSource21, Autos.fourPieceSource(fourPieceSource21)); - - var fourPieceSource23 = Choreo.getTrajectoryGroup("FourPieceSource23"); - GRRDashboard.addAutoCommand("Four Piece Source: 2, 3", fourPieceSource23, Autos.fourPieceSource(fourPieceSource23)); - - var fourPieceSource31 = Choreo.getTrajectoryGroup("FourPieceSource31"); - GRRDashboard.addAutoCommand("Four Piece Source: 3, 1", fourPieceSource31, Autos.fourPieceSource(fourPieceSource31)); - - var fourPieceSource32 = Choreo.getTrajectoryGroup("FourPieceSource32"); - GRRDashboard.addAutoCommand("Four Piece Source: 3, 2", fourPieceSource32, Autos.fourPieceSource(fourPieceSource32)); - } - - /** - * Gets the X axis drive speed from the driver's controller. - */ - private static double getDriveX() { - double multiplier = - ((driver.getHID().getLeftStickButton()) ? ControllerConstants.DRIVE_MULTIPLIER_MODIFIED : ControllerConstants.DRIVE_MULTIPLIER); - return -driver.getLeftY(multiplier, ControllerConstants.DRIVE_EXP); - } - - /** - * Gets the Y axis drive speed from the driver's controller. - */ - private static double getDriveY() { - double multiplier = - ((driver.getHID().getLeftStickButton()) ? ControllerConstants.DRIVE_MULTIPLIER_MODIFIED : ControllerConstants.DRIVE_MULTIPLIER); - return -driver.getLeftX(multiplier, ControllerConstants.DRIVE_EXP); - } - - /** - * Gets the rotational drive speed from the driver's controller. - */ - private static double getDriveRotate() { - return driver.getTriggerDifference(ControllerConstants.DRIVE_ROT_MULTIPLIER, ControllerConstants.DRIVE_ROT_EXP); - } - /** * Sets the rumble on the Co-Driver's controller. * @param type The rumble type. * @param value The normalized value to set the rumble to ({@code 0.0} to {@code 1.0}). */ - private static Command setCoDriverRumble(RumbleType type, double value) { + private Command setCoDriverRumble(RumbleType type, double value) { return runEnd(() -> coDriver.getHID().setRumble(type, value), () -> coDriver.getHID().setRumble(type, 0.0)); } } diff --git a/src/main/java/org/team340/robot/commands/Autos.java b/src/main/java/org/team340/robot/commands/Autos.java deleted file mode 100644 index a4561e5..0000000 --- a/src/main/java/org/team340/robot/commands/Autos.java +++ /dev/null @@ -1,121 +0,0 @@ -package org.team340.robot.commands; - -import static edu.wpi.first.wpilibj2.command.Commands.*; -import static org.team340.robot.RobotContainer.*; - -import com.choreo.lib.ChoreoTrajectory; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.List; -import org.team340.robot.Constants.PivotConstants; - -/** - * This class is used to declare autonomous routines. - */ -public class Autos { - - private Autos() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - public static Command fivePieceAmp(List traj) { - return parallel( - sequence( - pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), - pivot.maintainPosition().withTimeout(2.5), - pivot.targetDistance(swerve::getSpeakerDistance) - ), - sequence( - deadline( - swerve.followTrajectory(traj.get(0), true), - sequence(waitSeconds(1.7).deadlineWith(Routines.prepPoop()), Routines.poop(false), Routines.intake()) - ), - deadline( - swerve.followTrajectory(traj.get(1), 0.5, 1.7), - sequence(waitSeconds(1.3).deadlineWith(Routines.intake()), feeder.shoot().withTimeout(0.6), Routines.intake()) - ), - deadline( - swerve.followTrajectory(traj.get(2), 1.8, -1.0), - sequence(waitSeconds(2.3).deadlineWith(Routines.intake()), feeder.shoot().withTimeout(0.6), Routines.intake()) - ), - deadline( - swerve.followTrajectory(traj.get(3), 0.1, 1.1), - sequence( - waitSeconds(0.4).deadlineWith(Routines.intake()), - waitSeconds(0.6).deadlineWith(feeder.shoot()), - waitSeconds(0.5).deadlineWith(Routines.intake()), - feeder.shoot().withTimeout(0.6), - Routines.intake() - ) - ), - parallel(swerve.driveSpeaker(), sequence(waitSeconds(0.5).deadlineWith(Routines.intake()), feeder.shoot())) - ) - ); - } - - public static Command fourPieceFar(List traj) { - return parallel( - sequence( - pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), - pivot.maintainPosition().withTimeout(2.25), - pivot.targetDistance(swerve::getSpeakerDistance) - ), - sequence( - deadline( - swerve.followTrajectory(traj.get(0), -1.0, -1.0, true), - sequence(waitSeconds(1.7).deadlineWith(Routines.prepPoop()), Routines.poop(false), Routines.intake()) - ), - deadline( - swerve.followTrajectory(traj.get(1), 1.85, 2.5), - sequence(waitSeconds(2.1).deadlineWith(Routines.intake()), feeder.shoot().withTimeout(0.6), Routines.intake()) - ), - deadline( - swerve.followTrajectory(traj.get(2), 0.7, 1.7), - sequence(waitSeconds(1.2).deadlineWith(Routines.intake()), feeder.shoot().withTimeout(0.6), Routines.intake()) - ), - swerve.followTrajectory(traj.get(3), 1.1, -1.0).deadlineWith(Routines.intake()), - swerve.driveSpeaker().withTimeout(0.2), - feeder.shoot().withTimeout(0.6), - swerve.followTrajectory(traj.get(4)).deadlineWith(Routines.intake()), - parallel(swerve.driveSpeaker(), sequence(waitSeconds(0.6).deadlineWith(Routines.intake()), feeder.shoot())) - ) - ); - } - - public static Command fourPieceClose(List traj) { - return parallel( - pivot.targetDistance(swerve::getSpeakerDistance), - sequence( - deadline(swerve.followTrajectory(traj.get(0), -1.0, -1.0, true), intake.downPosition()), - deadline(sequence(waitSeconds(1.1), feeder.shoot().withTimeout(0.75)), swerve.driveSpeaker()), - deadline(swerve.followTrajectory(traj.get(1)), Routines.intake()), - Routines.intake().withTimeout(0.3), - deadline(sequence(waitSeconds(0.7), feeder.shoot().withTimeout(0.75)), swerve.driveSpeaker()), - deadline(swerve.followTrajectory(traj.get(2)), Routines.intake()), - Routines.intake().withTimeout(0.3), - deadline(sequence(waitSeconds(0.7), feeder.shoot().withTimeout(0.75)), swerve.driveSpeaker()), - deadline(swerve.followTrajectory(traj.get(3)), Routines.intake()), - Routines.intake().withTimeout(0.3), - deadline(sequence(waitSeconds(0.7), feeder.shoot().withTimeout(0.75)), swerve.driveSpeaker()) - ) - ); - } - - public static Command fourPieceSource(List traj) { - return parallel( - pivot.targetDistance(swerve::getSpeakerDistance), - sequence( - swerve.followTrajectory(traj.get(0), -1.0, -1.0, true).deadlineWith(intake.downPosition()), - deadline(sequence(waitSeconds(1.0), feeder.shoot().withTimeout(0.6)), swerve.driveSpeaker()), - swerve.followTrajectory(traj.get(1)).deadlineWith(Routines.intake()), - swerve.followTrajectory(traj.get(2)).deadlineWith(Routines.intake().withTimeout(1.5)), - deadline(sequence(waitSeconds(0.65), feeder.shoot().withTimeout(0.6)), swerve.driveSpeaker()), - swerve.followTrajectory(traj.get(3)).deadlineWith(Routines.intake()), - swerve.followTrajectory(traj.get(4)).deadlineWith(Routines.intake().withTimeout(1.5)), - deadline(sequence(waitSeconds(0.5), feeder.shoot().withTimeout(0.6)), swerve.driveSpeaker()), - swerve.followTrajectory(traj.get(5)).deadlineWith(Routines.intake()), - swerve.followTrajectory(traj.get(6)).deadlineWith(Routines.intake().withTimeout(1.5)), - deadline(sequence(waitSeconds(0.5), feeder.shoot().withTimeout(0.6)), swerve.driveSpeaker()) - ) - ); - } -} diff --git a/src/main/java/org/team340/robot/commands/Routines.java b/src/main/java/org/team340/robot/commands/Routines.java index c109a44..77d7ecb 100644 --- a/src/main/java/org/team340/robot/commands/Routines.java +++ b/src/main/java/org/team340/robot/commands/Routines.java @@ -1,114 +1,66 @@ package org.team340.robot.commands; import static edu.wpi.first.wpilibj2.command.Commands.*; -import static org.team340.robot.RobotContainer.*; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import java.util.function.Supplier; -import org.team340.robot.Constants; -import org.team340.robot.Constants.PivotConstants; +import org.team340.robot.RobotContainer; +import org.team340.robot.subsystems.Feeder; +import org.team340.robot.subsystems.Feeder.FeederSpeed; +import org.team340.robot.subsystems.Intake; +import org.team340.robot.subsystems.Pivot; +import org.team340.robot.subsystems.Pivot.PivotPosition; +import org.team340.robot.subsystems.Shooter; +import org.team340.robot.subsystems.Swerve; /** * This class is used to declare commands that require multiple subsystems. */ public class Routines { - private Routines() { - throw new UnsupportedOperationException("This is a utility class!"); - } + private final Feeder feeder; + private final Intake intake; + private final Pivot pivot; + private final Shooter shooter; + private final Swerve swerve; - /** - * Deploys and runs the intake. After a note is collected, it is seated by the feeder. - */ - public static Command intake() { - return sequence(waitUntil(pivot::isSafeForIntake), intake.downPosition(), race(feeder.receive(), intake.intake()), feeder.seat()) - .withName("Routines.intake()"); + public Routines(RobotContainer robotContainer) { + feeder = robotContainer.feeder; + intake = robotContainer.intake; + pivot = robotContainer.pivot; + shooter = robotContainer.shooter; + swerve = robotContainer.swerve; } - /** - * Finishes the intake sequence. - */ - public static Command finishIntake() { - return parallel(feeder.seat(), intake.safePosition()).withName("Routines.finishIntake()"); - } - - /** - * Intakes from the human player. - * @param x The desired {@code x} driving speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}. - */ - public static Command intakeHuman(Supplier x, Supplier y) { - return parallel( - deadline(waitUntil(feeder::hasNote).andThen(waitSeconds(0.1)), shooter.intakeHuman(), feeder.intakeHuman()), - sequence( - pivot.goTo(PivotConstants.INTAKE_SAFE_POSITION).unless(pivot::isSafeForIntake), - intake.uprightPosition(), - pivot.goTo(PivotConstants.MAX_POS) - ), - swerve.driveIntakeHuman(x, y) - ) - .withName("Routines.intakeHuman()"); - } - - /** - * Finishes the human player intake sequence. - */ - public static Command finishIntakeHuman() { - return parallel( - shooter.setSpeed(0).withTimeout(2.0), - pivot.goTo(PivotConstants.DOWN_POSITION), - sequence(waitUntil(pivot::isSafeForIntake), parallel(intake.safePosition(), sequence(feeder.reverseSeat(), feeder.seat()))) - ) - .withName("Routines.finishIntakeHuman()"); - } - - /** - * Intakes while ignoring note detectors. - */ - public static Command intakeOverride() { - return parallel(intake.intakeOverride(), feeder.shoot()).withName("Routines.intakeOverride()"); - } - - /** - * Prepares to score in speaker by facing the speaker and moving the pivot. - * @param x The desired {@code x} driving speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}. - */ - public static Command prepSpeaker(Supplier x, Supplier y) { - return parallel(swerve.driveSpeaker(x, y), pivot.targetDistance(swerve::getSpeakerDistance)).withName("Routines.prepSpeaker()"); - } - - /** - * Prepares to score in the amp. - * @param x The desired {@code x} driving speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}. - */ - public static Command prepAmp(Supplier x, Supplier y) { - return parallel( - swerve.driveAmpManual(x, y), - sequence( + public Command humanLoad() { + return sequence( + parallel( + feeder.apply(FeederSpeed.kBarfForward), + pivot.apply(PivotPosition.kHumanLoad), + shooter.humanLoad() + ).until(feeder::hasNote), + deadline( sequence( - parallel( - pivot.goTo(Constants.PivotConstants.AMP_HANDOFF_POSITION), - sequence(waitUntil(pivot::isSafeForIntake), intake.handoffPosition()) - ), - handoff() - ) - .unless(intake::hasNote), - intake.ampPosition() + feeder.apply(FeederSpeed.kBarfForward).until(feeder::noNote), + new ScheduleCommand(feeder.seat()) + ), + pivot.apply(PivotPosition.kDown) ) - ) - .withName("Routines.prepAmp()"); + ).withName("Routines.humanLoad()"); } /** - * Prepares for a climb by raising the arms and facing the stage. + * Prepares to score in speaker by facing the speaker and moving the pivot. * @param x The desired {@code x} driving speed from {@code -1.0} to {@code 1.0}. * @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}. */ - public static Command prepClimb(Supplier x, Supplier y) { - return parallel(swerve.driveClimb(x, y), intake.uprightPosition(), pivot.goTo(PivotConstants.DOWN_POSITION)) - .withName("Routines.prepClimb()"); + public Command prepSpeaker(Supplier x, Supplier y) { + return parallel( + swerve.driveSpeaker(x, y), + pivot.targetSpeaker(swerve::getSpeakerDistance), + shooter.targetSpeaker(swerve::getSpeakerDistance) + ).withName("Routines.prepSpeaker()"); } /** @@ -116,79 +68,31 @@ public static Command prepClimb(Supplier x, Supplier y) { * @param x The desired {@code x} driving speed from {@code -1.0} to {@code 1.0}. * @param y The desired {@code y} driving speed from {@code -1.0} to {@code 1.0}. */ - public static Command prepFeed(Supplier x, Supplier y) { - return parallel(swerve.driveFeed(x, y), shooter.feed(() -> true), pivot.feed(() -> true)); + public Command prepFeed(Supplier x, Supplier y) { + return parallel(swerve.driveFeed(x, y), shooter.feed(), pivot.apply(PivotPosition.kFeed)); } /** * Fixes the position of the note if it is in a deadzone. */ - public static Command fixDeadzone() { + public Command fixDeadzone() { return sequence( - deadline(feeder.reverseSeat(), shooter.fixDeadzone(), pivot.goTo(PivotConstants.FIX_DEADZONE_POSITION)), - parallel(feeder.seat(), pivot.goTo(PivotConstants.DOWN_POSITION)) - ) - .withName("Routines.fixDeadzone()"); - } - - /** - * Barfs the note forwards out of the intake. - */ - public static Command barfForward() { - return sequence( - parallel(pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), intake.barfPosition()).withTimeout(0.5), - parallel(pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), feeder.barfForward(), intake.barf()) - ) - .withName("Routines.barfForward()"); - } - - /** - * Barfs the note backwards out of the shooter. - */ - public static Command barfBackward() { - return parallel(shooter.barfBackward(), sequence(waitSeconds(0.35), parallel(feeder.barfBackward(), intake.intake()))) - .withName("Routines.barfBackward()"); - } - - /** - * Prepares to poop the note forwards out of the intake. - */ - public static Command prepPoop() { - return sequence(handoff(), intake.poopPosition()).withName("Routines.prepPoop()"); - } - - /** - * Poops the note out of the intake. - * @param includePrep If {@link Routines#prepPoop()} should be called first. - */ - public static Command poop(boolean includePrep) { - return sequence( - includePrep ? Routines.prepPoop() : none(), - deadline(sequence(waitUntil(() -> !intake.hasNote()), waitSeconds(0.15)), intake.poop()) - ) - .withName("Routines.poop(" + includePrep + ")"); - } - - /** - * Returns the note from the feeder back to the intake. - */ - public static Command handoff() { - return sequence( - intake.handoffPosition(), - deadline( - sequence(waitUntil(() -> intake.hasNote() && !feeder.hasNote()), waitSeconds(0.1)), - feeder.barfForward(), - intake.handoff() - ) - ); + parallel( + feeder.apply(FeederSpeed.kBarfForward), + shooter.fixDeadzone(), + pivot.apply(PivotPosition.kFixDeadzone) + ).until(feeder::noNote), + feeder.seat() + ).withName("Routines.fixDeadzone()"); } /** * Should be called when disabled, and cancelled when enabled. * Calls {@code onDisable()} for all subsystems. */ - public static Command onDisable() { - return sequence(waitSeconds(6.0), parallel(climber.onDisable(), feeder.onDisable(), intake.onDisable(), pivot.onDisable())) - .withName("Routines.onDisable()"); + public Command onDisable() { + return sequence(waitSeconds(6.0), parallel(feeder.onDisable(), intake.onDisable(), pivot.onDisable())).withName( + "Routines.onDisable()" + ); } } diff --git a/src/main/java/org/team340/robot/commands/SystemsCheck.java b/src/main/java/org/team340/robot/commands/SystemsCheck.java deleted file mode 100644 index d5f7b60..0000000 --- a/src/main/java/org/team340/robot/commands/SystemsCheck.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.team340.robot.commands; - -import static edu.wpi.first.wpilibj2.command.Commands.*; -import static org.team340.robot.RobotContainer.*; - -import edu.wpi.first.wpilibj2.command.Command; - -/** - * This class is used to declare the systems check command. - */ -public class SystemsCheck { - - private SystemsCheck() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - /** - * The systems check command. - * Runs all of the robot's mechanisms. - */ - public static Command command() { - return sequence(swerve.drive(() -> 0.1, () -> 0.0, () -> 0.0, true).withTimeout(1.0)); - } -} diff --git a/src/main/java/org/team340/robot/subsystems/Climber.java b/src/main/java/org/team340/robot/subsystems/Climber.java deleted file mode 100644 index 16ef139..0000000 --- a/src/main/java/org/team340/robot/subsystems/Climber.java +++ /dev/null @@ -1,125 +0,0 @@ -package org.team340.robot.subsystems; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.SparkLimitSwitch; -import com.revrobotics.SparkLimitSwitch.Type; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.Supplier; -import org.team340.lib.GRRSubsystem; -import org.team340.robot.Constants.ClimberConstants; -import org.team340.robot.Constants.RobotMap; - -/** - * The climber subsystem. Homed using limit switches, controlled - * with PID to climb on the chain in two stages. - */ -public class Climber extends GRRSubsystem { - - private final CANSparkFlex leftMotor; - private final CANSparkFlex rightMotor; - private final SparkLimitSwitch leftLimit; - private final SparkLimitSwitch rightLimit; - - /** - * Create the climber subsystem. - */ - public Climber() { - super("Climber"); - leftMotor = createSparkFlex("Left Motor", RobotMap.CLIMBER_LEFT_MOTOR, MotorType.kBrushless); - rightMotor = createSparkFlex("Right Motor", RobotMap.CLIMBER_RIGHT_MOTOR, MotorType.kBrushless); - leftLimit = leftMotor.getForwardLimitSwitch(Type.kNormallyOpen); - rightLimit = rightMotor.getForwardLimitSwitch(Type.kNormallyOpen); - - ClimberConstants.Configs.MOTOR.apply(leftMotor); - ClimberConstants.Configs.MOTOR.apply(rightMotor); - ClimberConstants.Configs.LIMIT.apply(leftMotor, leftLimit); - ClimberConstants.Configs.LIMIT.apply(rightMotor, rightLimit); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("atLeftLimit", this::getLeftLimit, null); - builder.addBooleanProperty("atRightLimit", this::getRightLimit, null); - } - - /** - * Returns {@code true} if the left limit is pressed. - */ - private boolean getLeftLimit() { - return leftLimit.isPressed(); - } - - /** - * Returns {@code true} if the right limit is pressed. - */ - private boolean getRightLimit() { - return rightLimit.isPressed(); - } - - /** - * Climbs the chain while balancing. - * @param robotRoll A supplier that returns the robot's roll in radians. - */ - public Command climb(Supplier robotRoll) { - return commandBuilder("climber.climb()") - .onExecute(() -> { - // leftMotor.set(ClimberConstants.CLIMBING_SPEED + (robotRoll.get() * ClimberConstants.BALANCE_COMPENSATION)); - // rightMotor.set(ClimberConstants.CLIMBING_SPEED + (-robotRoll.get() * ClimberConstants.BALANCE_COMPENSATION)); - leftMotor.set(ClimberConstants.CLIMBING_SPEED); - rightMotor.set(ClimberConstants.CLIMBING_SPEED); - }) - .onEnd(() -> { - leftMotor.stopMotor(); - rightMotor.stopMotor(); - }); - } - - /** - * Drives the climber motors manually. - * @param speed The speed of the climber as duty cycle. - */ - public Command driveManual(Supplier speed) { - return driveManual(speed, speed); - } - - /** - * Drives the climber motors manually. - * @param leftSpeed The speed of the left climber as duty cycle. - * @param rightSpeed The speed of the right climber as duty cycle. - * @return - */ - public Command driveManual(Supplier leftSpeed, Supplier rightSpeed) { - return commandBuilder() - .onExecute(() -> { - leftMotor.set(-leftSpeed.get()); - rightMotor.set(-rightSpeed.get()); - }) - .onEnd(() -> { - leftMotor.stopMotor(); - rightMotor.stopMotor(); - }); - } - - /** - * Should be called when disabled, and cancelled when enabled. - */ - public Command onDisable() { - return commandBuilder() - .onInitialize(() -> { - // leftMotor.setIdleMode(IdleMode.kCoast); - // rightMotor.setIdleMode(IdleMode.kCoast); - leftMotor.stopMotor(); - rightMotor.stopMotor(); - }) - .onEnd(() -> { - leftMotor.setIdleMode(IdleMode.kBrake); - rightMotor.setIdleMode(IdleMode.kBrake); - }) - .ignoringDisable(true) - .withName("climber.onDisable()"); - } -} diff --git a/src/main/java/org/team340/robot/subsystems/Feeder.java b/src/main/java/org/team340/robot/subsystems/Feeder.java index b968133..19f53dd 100644 --- a/src/main/java/org/team340/robot/subsystems/Feeder.java +++ b/src/main/java/org/team340/robot/subsystems/Feeder.java @@ -8,43 +8,76 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; -import org.team340.lib.GRRSubsystem; +import org.team340.lib.dashboard.Tunable; +import org.team340.lib.util.GRRSubsystem; import org.team340.lib.util.Math2; -import org.team340.robot.Constants.FeederConstants; +import org.team340.lib.util.rev.RelativeEncoderConfig; +import org.team340.lib.util.rev.SparkMaxConfig; +import org.team340.lib.util.rev.SparkPIDControllerConfig; import org.team340.robot.Constants.RobotMap; /** * This subsystem controls the feeder wheels, which accepts a note from the * intake and pushes it to be shot or back into the intake for amp scoring. */ +@Logged public class Feeder extends GRRSubsystem { - private final CANSparkMax feedMotor; - private final RelativeEncoder feedEncoder; + public static enum FeederSpeed { + /** Speed for receiving from the intake. */ + kReceive(6.0), + /** Speed for shooting. */ + kShoot(1.0), + /** Speed for detecting the rising edge of the note. */ + kRisingEdge(0.6), + /** Speed for detecting the falling edge of the note. */ + kFallingEdge(-0.75), + /** Speed for barfing forwards (towards the intake). */ + kBarfForward(-8.0), + /** Speed for barfing backwards (towards the shooter). */ + kBarfBackward(8.0); + + private final Tunable voltage; + + private FeederSpeed(double voltage) { + this.voltage = Tunable.doubleValue("Feeder/Speeds/" + this.name(), voltage); + } + + private double voltage() { + return voltage.get(); + } + } + + private static final Tunable kSeatPosition = Tunable.doubleValue("Feeder/kSeatPosition", 2.357); + private static final Tunable kClosedLoopErr = Tunable.doubleValue("Feeder/kClosedLoopErr", 0.125); + + private final CANSparkMax motor; + private final RelativeEncoder encoder; + private final SparkPIDController pid; private final DigitalInput noteDetector; - private final SparkPIDController feedPID; /** * Create the feeder subsystem. */ public Feeder() { - super("Feeder"); - feedMotor = createSparkMax("Motor", RobotMap.SHOOTER_FEEDER_MOTOR, MotorType.kBrushless); - feedEncoder = feedMotor.getEncoder(); - noteDetector = createDigitalInput("Note Detector", RobotMap.SHOOTER_NOTE_DETECTOR); - feedPID = feedMotor.getPIDController(); - - FeederConstants.Configs.MOTOR.apply(feedMotor); - FeederConstants.Configs.PID.apply(feedMotor, feedPID); - } + motor = new CANSparkMax(RobotMap.kFeederMotor, MotorType.kBrushless); + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + noteDetector = new DigitalInput(RobotMap.kNoteDetector); - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("hasNote", this::hasNote, null); + SparkMaxConfig.defaults().setSmartCurrentLimit(30).setInverted(true).setIdleMode(IdleMode.kBrake).apply(motor); + + new RelativeEncoderConfig() + .setPositionConversionFactor(1.0) + .setVelocityConversionFactor(1.0 / 60) + .apply(motor, encoder); + + new SparkPIDControllerConfig().setPID(0.15, 0.0, 0.0).apply(motor, pid); + + Tunable.pidController("Feeder/PID", pid); } /** @@ -55,94 +88,39 @@ public boolean hasNote() { } /** - * Receives a note from the intake. + * Returns {@code true} when the beam break does not detect a note. */ - public Command receive() { - return commandBuilder() - .onInitialize(() -> feedMotor.set(FeederConstants.RECEIVE_SPEED)) - .isFinished(() -> hasNote()) - .onEnd(() -> feedMotor.stopMotor()) - .onlyIf(() -> !hasNote()) - .withName("feeder.receive()"); + public boolean noNote() { + return !hasNote(); } /** - * Sets the feeder to receive a note through the shooter from the human player. + * Applies a speed to the feeder rollers. Does not end. + * @param speed The speed to apply. */ - public Command intakeHuman() { - return commandBuilder("feeder.intakeHuman()") - .onInitialize(() -> feedMotor.set(FeederConstants.INTAKE_HUMAN_SPEED)) - .onEnd(() -> feedMotor.stopMotor()); + public Command apply(FeederSpeed speed) { + return commandBuilder("Feeder.apply(" + speed.name() + ")") + .onExecute(() -> motor.setVoltage(speed.voltage())) + .onEnd(() -> motor.stopMotor()); } /** - * Seats the note in the shooter to a set position. + * Seats a note. If the feeder does not initially see a note, it will + * run at the {@link FeederSpeed#kReceive} speed until a note is detected. */ public Command seat() { return sequence( + apply(FeederSpeed.kReceive).until(this::hasNote).onlyIf(this::noNote), + apply(FeederSpeed.kFallingEdge).until(this::noNote), + apply(FeederSpeed.kRisingEdge).until(this::hasNote), commandBuilder() - .onInitialize(() -> feedMotor.set(FeederConstants.SEAT_SPEED)) - .isFinished(this::hasNote) - .onEnd(() -> feedEncoder.setPosition(0.0)), - commandBuilder() - .onInitialize(() -> feedPID.setReference(FeederConstants.SEAT_POSITION, ControlType.kPosition)) - .isFinished(() -> - Math2.epsilonEquals(feedEncoder.getPosition(), FeederConstants.SEAT_POSITION, FeederConstants.CLOSED_LOOP_ERR) - ) - .onEnd(() -> feedMotor.stopMotor()) + .onInitialize(() -> encoder.setPosition(0.0)) + .onExecute(() -> pid.setReference(kSeatPosition.get(), ControlType.kPosition)) + .isFinished(() -> Math2.epsilonEquals(encoder.getPosition(), kSeatPosition.get(), kClosedLoopErr.get())) + .onEnd(() -> motor.stopMotor()) ) - .onlyIf(() -> !hasNote()) - .withTimeout(2.0) - .withName("feeder.seat()"); - } - - /** - * Backs the note out of the shooter until the note detector is clear. - */ - public Command reverseSeat() { - return commandBuilder("feeder.reverseSeat()") - .onInitialize(() -> feedMotor.set(FeederConstants.REVERSE_SEAT_SPEED)) - .isFinished(() -> !hasNote()) - .onEnd(() -> feedMotor.stopMotor()); - } - - /** - * Feeds the note into the shooter wheels. Ends after the note has left the shooter. - */ - public Command shoot() { - return commandBuilder() - .onInitialize(() -> feedMotor.set(FeederConstants.SHOOT_SPEED)) - .isFinished(() -> !hasNote()) - .andThen(waitSeconds(FeederConstants.SHOOT_DELAY)) - .finallyDo(() -> feedMotor.stopMotor()) - .withName("feeder.shoot()"); - } - - /** - * Spits the note out of the feeder towards the intake. - */ - public Command poop() { - return commandBuilder("feeder.poop()") - .onInitialize(() -> feedMotor.set(FeederConstants.POOP_SPEED)) - .onEnd(() -> feedMotor.stopMotor()); - } - - /** - * Spits the note out of the feeder towards the intake. - */ - public Command barfForward() { - return commandBuilder("feeder.barfForward()") - .onInitialize(() -> feedMotor.set(FeederConstants.BARF_FORWARD_SPEED)) - .onEnd(() -> feedMotor.stopMotor()); - } - - /** - * Spits the note out of the feeder towards the shooter. - */ - public Command barfBackward() { - return commandBuilder("feeder.barfBackward()") - .onInitialize(() -> feedMotor.set(FeederConstants.BARF_BACKWARD_SPEED)) - .onEnd(() -> feedMotor.stopMotor()); + .withTimeout(2.5) + .withName("Feeder.seat()"); } /** @@ -151,11 +129,11 @@ public Command barfBackward() { public Command onDisable() { return commandBuilder() .onInitialize(() -> { - feedMotor.setIdleMode(IdleMode.kCoast); - feedMotor.stopMotor(); + motor.stopMotor(); + motor.setIdleMode(IdleMode.kCoast); }) - .onEnd(() -> feedMotor.setIdleMode(IdleMode.kBrake)) + .onEnd(() -> motor.setIdleMode(IdleMode.kBrake)) .ignoringDisable(true) - .withName("feeder.onDisable()"); + .withName("Feeder.onDisable()"); } } diff --git a/src/main/java/org/team340/robot/subsystems/Intake.java b/src/main/java/org/team340/robot/subsystems/Intake.java index 76f7f40..7319c8c 100644 --- a/src/main/java/org/team340/robot/subsystems/Intake.java +++ b/src/main/java/org/team340/robot/subsystems/Intake.java @@ -4,288 +4,143 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.Supplier; -import org.team340.lib.GRRSubsystem; +import org.team340.lib.dashboard.Tunable; +import org.team340.lib.util.GRRSubsystem; import org.team340.lib.util.Math2; +import org.team340.lib.util.rev.SparkAbsoluteEncoderConfig; +import org.team340.lib.util.rev.SparkFlexConfig; +import org.team340.lib.util.rev.SparkFlexConfig.Frame; +import org.team340.lib.util.rev.SparkPIDControllerConfig; import org.team340.robot.Constants; -import org.team340.robot.Constants.IntakeConstants; -import org.team340.robot.Constants.PivotConstants; import org.team340.robot.Constants.RobotMap; /** * The intake subsystem. Intakes notes from the floor and scores * them in the amp, or pass them to the shooter. */ +@Logged public class Intake extends GRRSubsystem { - private final CANSparkFlex armLeftMotor; - private final CANSparkFlex armRightMotor; - private final CANSparkMax rollerUpperMotor; - private final CANSparkMax rollerLowerMotor; - private final SparkAbsoluteEncoder armEncoder; - private final DigitalInput noteDetector; - private final SparkPIDController armPID; + public static enum IntakeState { + /** The intake is deployed and intaking. */ + kIntake(8.0, Math.toRadians(0.0)), + /** The intake is retracted and the rollers are stationary. */ + kRetract(0.0, Math.toRadians(85.0)), + /** The intake is barfing. */ + kBarf(-10.0, Math.toRadians(70.0)); - private double armMaintain = 0.0; - private double armTarget = 0.0; + private final Tunable voltage; + private final Tunable radians; - /** - * Create the intake subsystem. - */ - public Intake() { - super("Intake"); - armLeftMotor = createSparkFlex("Arm Left Motor", RobotMap.INTAKE_ARM_LEFT_MOTOR, MotorType.kBrushless); - armRightMotor = createSparkFlex("Arm Right Motor", RobotMap.INTAKE_ARM_RIGHT_MOTOR, MotorType.kBrushless); - rollerUpperMotor = createSparkMax("Roller Upper Motor", RobotMap.INTAKE_ROLLER_UPPER_MOTOR, MotorType.kBrushless); - rollerLowerMotor = createSparkMax("Roller Lower Motor", RobotMap.INTAKE_ROLLER_LOWER_MOTOR, MotorType.kBrushless); - armEncoder = createSparkFlexAbsoluteEncoder("Arm Encoder", armLeftMotor, Type.kDutyCycle); - noteDetector = createDigitalInput("Note Detector", RobotMap.INTAKE_NOTE_DETECTOR); - armPID = armLeftMotor.getPIDController(); - - armPID.setFeedbackDevice(armEncoder); - - IntakeConstants.ArmConfigs.LEFT_MOTOR.apply(armLeftMotor); - IntakeConstants.ArmConfigs.RIGHT_MOTOR.apply(armRightMotor); - IntakeConstants.RollerConfigs.MOTOR.apply(rollerUpperMotor); - IntakeConstants.RollerConfigs.MOTOR.apply(rollerLowerMotor); - IntakeConstants.ArmConfigs.ENCODER.apply(armLeftMotor, armEncoder); - IntakeConstants.ArmConfigs.PID.apply(armLeftMotor, armPID); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty("armTarget", () -> armTarget, null); - builder.addDoubleProperty("armMaintain", () -> armMaintain, null); - builder.addBooleanProperty("hasNote", this::hasNote, null); - } - - /** - * Returns {@code true} when the beam break detects a note. - */ - public boolean hasNote() { - return noteDetector.get(); - } - - /** - * Returns {@code true} if the arm is at the specified position. - * @param position The position to check for in radians. - */ - private boolean atPosition(double position) { - return Math2.epsilonEquals(MathUtil.angleModulus(armEncoder.getPosition()), position, IntakeConstants.CLOSED_LOOP_ERR); - } - - /** - * Sets the {@link #armPID arms' PIDs} to go to the specified position if it is valid - * (within the intake {@link IntakeConstants#MIN_POS minimum} and - * {@link IntakeConstants#MAX_POS maximum} angles). - * @param position The position to set. - */ - private void applyPosition(double position) { - if (position < IntakeConstants.MIN_POS || position > IntakeConstants.MAX_POS) { - DriverStation.reportWarning( - "Invalid intake arm position. " + - Math2.formatRadians(position) + - " degrees is not between " + - Math2.formatRadians(PivotConstants.MIN_POS) + - " and " + - Math2.formatRadians(PivotConstants.MAX_POS), - false - ); - } else { - armTarget = position; - if ( - position < IntakeConstants.PID_INACTIVE_POSITION && - MathUtil.angleModulus(armEncoder.getPosition()) < IntakeConstants.PID_INACTIVE_POSITION - ) { - armLeftMotor.stopMotor(); - armRightMotor.stopMotor(); - } else { - armPID.setReference(position, ControlType.kPosition); - } + private IntakeState(double voltage, double radians) { + this.voltage = Tunable.doubleValue("Intake/Speeds/" + this.name(), voltage); + this.radians = Tunable.doubleValue("Intake/Positions/" + this.name(), radians); } - } - - /** - * Sets the state of the intake. - * @param position The position for the arm to move to in radians. - * @param upperSpeed The duty cycle of the upper roller. - * @param lowerSpeed The duty cycle of the lower roller. - * @param willFinish If {@code true}, the command will end after the arm reaches the specified angle. - */ - private Command useState(double position, double upperSpeed, double lowerSpeed, boolean willFinish) { - return commandBuilder( - "intake.useState(" + Math2.formatRadians(position) + ", " + upperSpeed + ", " + lowerSpeed + ", " + willFinish + ")" - ) - .onInitialize(() -> { - rollerUpperMotor.set(upperSpeed); - rollerLowerMotor.set(lowerSpeed); - }) - .onExecute(() -> { - applyPosition(position); - armMaintain = armEncoder.getPosition(); - }) - .isFinished(() -> willFinish && atPosition(position)) - .onEnd(interrupted -> { - if (!interrupted || atPosition(position)) armMaintain = position; - armLeftMotor.stopMotor(); - armRightMotor.stopMotor(); - rollerUpperMotor.stopMotor(); - rollerLowerMotor.stopMotor(); - }); - } - /** - * Moves to the down position. Runs until the arm is at the position. - */ - public Command downPosition() { - return useState(IntakeConstants.DOWN_POSITION, 0, 0, true).withName("intake.downPosition()"); - } - - /** - * Moves to the handoff position. Runs until the arm is at the position. - */ - public Command handoffPosition() { - return useState(IntakeConstants.HANDOFF_POSITION, 0, 0, true).withName("intake.handoffPosition()"); - } - - /** - * Moves to the safe position. Runs until the arm is at the position. - */ - public Command safePosition() { - return useState(IntakeConstants.SAFE_POSITION, 0, 0, true).withName("intake.safePosition()"); - } + private double voltage() { + return voltage.get(); + } - /** - * Moves to the retract position. Runs until the arm is at the position. - */ - public Command retractPosition() { - return useState(IntakeConstants.RETRACT_POSITION, 0, 0, true).withName("intake.retractPosition()"); + private double radians() { + return radians.get(); + } } - /** - * Moves to the upright position. Runs until the arm is at the position. - */ - public Command uprightPosition() { - return useState(IntakeConstants.UPRIGHT_POSITION, 0.0, 0.0, true).withName("intake.uprightPosition()"); - } + private static final Tunable kMinPos = Tunable.doubleValue("Intake/kMinPos", 0.0); + private static final Tunable kMaxPos = Tunable.doubleValue("Intake/kMaxPos", 90.0); - /** - * Moves to the barf position. Runs until the arm is at the position. - */ - public Command barfPosition() { - return useState(IntakeConstants.BARF_POSITION, 0, 0, true).withName("intake.barfPosition()"); - } + private final CANSparkFlex rollerMotor; + private final CANSparkFlex pivotMotor; + private final SparkAbsoluteEncoder pivotEncoder; + private final SparkPIDController pivotPID; - /** - * Moves to the poop position. Runs until the arm is at the position. - */ - public Command poopPosition() { - return useState(IntakeConstants.POOP_POSITION, 0, 0, true).withName("intake.poopPosition()"); - } + private double pivotTarget = 0.0; /** - * Moves to the amp position. Runs until the arm is at the position. + * Create the intake subsystem. */ - public Command ampPosition() { - return useState(IntakeConstants.AMP_POSITION, 0, 0, true).withName("intake.ampPosition()"); - } + public Intake() { + rollerMotor = new CANSparkFlex(RobotMap.kIntakeRollerMotor, MotorType.kBrushless); + pivotMotor = new CANSparkFlex(RobotMap.kIntakePivotMotor, MotorType.kBrushless); + pivotEncoder = pivotMotor.getAbsoluteEncoder(); + pivotPID = pivotMotor.getPIDController(); - /** - * Intakes from the ground. Does not end. - */ - public Command intake() { - return useState(IntakeConstants.DOWN_POSITION, IntakeConstants.INTAKE_SPEED, IntakeConstants.INTAKE_SPEED * .6, false) - .withName("intake.intake()"); - } + pivotPID.setFeedbackDevice(pivotEncoder); - /** - * Receives a note back from the feeder. Does not end. - */ - public Command handoff() { - return useState(IntakeConstants.HANDOFF_POSITION, IntakeConstants.HANDOFF_SPEED, IntakeConstants.HANDOFF_SPEED, false) - .withName("intake.ampHandoff()"); - } + SparkFlexConfig.defaults() + .setSmartCurrentLimit(40) + .setIdleMode(IdleMode.kCoast) + .setInverted(false) + .apply(rollerMotor); - /** - * Scores in the amp. Does not end. - */ - public Command scoreAmp() { - return ampPosition() - .andThen(useState(IntakeConstants.AMP_POSITION, IntakeConstants.AMP_UPPER_SPEED, IntakeConstants.AMP_LOWER_SPEED, false)) - .withName("intake.scoreAmp()"); - } + SparkFlexConfig.defaults() + .setSmartCurrentLimit(40) + .setIdleMode(IdleMode.kBrake) + .setInverted(true) + .setPeriodicFramePeriod(Frame.S5, 20) + .setPeriodicFramePeriod(Frame.S6, 20) + .apply(pivotMotor); - /** - * Barfs the note out of the intake. Does not end. - */ - public Command barf() { - return useState(IntakeConstants.BARF_POSITION, IntakeConstants.BARF_SPEED, IntakeConstants.BARF_SPEED, false) - .withName("intake.barf()"); - } + new SparkAbsoluteEncoderConfig() + .setPositionConversionFactor(Math2.TWO_PI) + .setVelocityConversionFactor(Math2.TWO_PI / 60.0) + .setInverted(true) + .setZeroOffset(0.0) + .apply(pivotMotor, pivotEncoder); - /** - * Poops the note out of the intake. Does not end. - */ - public Command poop() { - return useState(IntakeConstants.POOP_POSITION, IntakeConstants.POOP_SPEED, IntakeConstants.POOP_SPEED, false) - .withName("intake.poop()"); - } + new SparkPIDControllerConfig() + .setPID(1.0, 0.0, 0.0) + .setPositionPIDWrappingEnabled(true) + .setPositionPIDWrappingInputLimits(0.0, Math2.TWO_PI) + .apply(pivotMotor, pivotPID); - /** - * Maintains the last set position of the arm. - */ - public Command maintainPosition() { - return commandBuilder("intake.maintainPosition()") - .onInitialize(() -> { - if (armMaintain > Math.PI && armMaintain < 3 * Math2.HALF_PI) armMaintain = Math.PI; else if ( - MathUtil.angleModulus(armMaintain) < 0.0 - ) armMaintain = 0.0; - }) - .onExecute(() -> applyPosition(armMaintain)); + Tunable.pidController("Intake/PID", pivotPID); } /** - * Runs rollers at a set speed to intake manually. + * Applies a state to the intake. Does not end. + * @param state The state to apply. */ - public Command intakeOverride() { - return commandBuilder("intake.intakeOverride()") + public Command apply(IntakeState state) { + return commandBuilder("Intake.apply(" + state.name() + ")") .onExecute(() -> { - rollerUpperMotor.set(IntakeConstants.OVERRIDE_INTAKE_SPEED); - rollerLowerMotor.set(IntakeConstants.OVERRIDE_INTAKE_SPEED); + rollerMotor.setVoltage(state.voltage()); + pivotTarget = MathUtil.clamp(state.radians(), kMinPos.get(), kMaxPos.get()); + pivotPID.setReference(pivotTarget, ControlType.kPosition); }) .onEnd(() -> { - rollerUpperMotor.stopMotor(); - rollerLowerMotor.stopMotor(); + rollerMotor.stopMotor(); + pivotMotor.stopMotor(); }); } /** - * Drives the arms manually. Will hold position. - * @param speed The speed of the arms in radians/second. + * Drives the pivot manually. Does not end. + * @param pivotSpeed The speed of the pivot in radians/second. */ - public Command driveArmManual(Supplier speed) { - return commandBuilder("intake.driveArmManual()") + public Command manual(Supplier pivotSpeed) { + return commandBuilder("Intake.manual()") + .onInitialize(() -> pivotTarget = pivotEncoder.getPosition()) .onExecute(() -> { - double diff = speed.get() * Constants.PERIOD; - double armPos = armEncoder.getPosition(); - if (armPos < IntakeConstants.MIN_POS) { + double diff = pivotSpeed.get() * Constants.kPeriod; + double pivotPos = pivotEncoder.getPosition(); + if (pivotPos <= kMinPos.get()) { diff = Math.max(diff, 0.0); - } else if (armPos > IntakeConstants.MAX_POS) { + } else if (pivotPos >= kMaxPos.get()) { diff = Math.min(diff, 0.0); } - armMaintain += diff; - applyPosition(armMaintain); + pivotTarget = MathUtil.clamp(pivotTarget + diff, kMinPos.get(), kMaxPos.get()); + pivotPID.setReference(pivotTarget, ControlType.kPosition); + }) + .onEnd(() -> { + pivotMotor.stopMotor(); }); } @@ -295,19 +150,12 @@ public Command driveArmManual(Supplier speed) { public Command onDisable() { return commandBuilder() .onInitialize(() -> { - armLeftMotor.setIdleMode(IdleMode.kCoast); - armRightMotor.setIdleMode(IdleMode.kCoast); - armLeftMotor.stopMotor(); - armRightMotor.stopMotor(); - rollerUpperMotor.stopMotor(); - rollerLowerMotor.stopMotor(); - }) - .onExecute(() -> armMaintain = armEncoder.getPosition()) - .onEnd(() -> { - armLeftMotor.setIdleMode(IdleMode.kBrake); - armRightMotor.setIdleMode(IdleMode.kBrake); + rollerMotor.stopMotor(); + pivotMotor.stopMotor(); + pivotMotor.setIdleMode(IdleMode.kCoast); }) + .onEnd(() -> pivotMotor.setIdleMode(IdleMode.kBrake)) .ignoringDisable(true) - .withName("intake.onDisable()"); + .withName("Intake.onDisable()"); } } diff --git a/src/main/java/org/team340/robot/subsystems/Lights.java b/src/main/java/org/team340/robot/subsystems/Lights.java deleted file mode 100644 index 96e2c95..0000000 --- a/src/main/java/org/team340/robot/subsystems/Lights.java +++ /dev/null @@ -1,141 +0,0 @@ -package org.team340.robot.subsystems; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.Supplier; -import org.team340.lib.GRRSubsystem; -import org.team340.lib.util.Alliance; -import org.team340.lib.util.Math2; -import org.team340.robot.Constants.LightsConstants; -import org.team340.robot.Constants.RobotMap; - -/** - * Controls the lights. - */ -public class Lights extends GRRSubsystem { - - private final AddressableLED lights; - private final AddressableLEDBuffer buffer; - private final Timer timer; - - /** - * Create the lights subsystem. - */ - public Lights() { - super("Lights"); - lights = new AddressableLED(RobotMap.LIGHTS); - buffer = new AddressableLEDBuffer(LightsConstants.LENGTH); - timer = new Timer(); - - lights.setLength(buffer.getLength()); - apply(); - lights.start(); - timer.start(); - } - - /** - * Applies the buffer to the LED strip. - */ - private void apply() { - lights.setData(buffer); - } - - /** - * Modifies the buffer to be a single color. - * @param r Red value from {@code 0} to {@code 255}. - * @param g Green value from {@code 0} to {@code 255}. - * @param b Blue value from {@code 0} to {@code 255}. - */ - private void set(int r, int g, int b) { - for (int i = 0; i < buffer.getLength(); i++) { - buffer.setRGB(i, r, g, b); - } - } - - /** - * Modifies the buffer with values mirrored across the center of the LED strip. - * @param i The index of the buffer to modify. - * @param r Red value from {@code 0} to {@code 255}. - * @param g Green value from {@code 0} to {@code 255}. - * @param b Blue value from {@code 0} to {@code 255}. - */ - private void setMirrored(int i, int r, int g, int b) { - buffer.setRGB(i, r, g, b); - buffer.setRGB(buffer.getLength() - i - 1, r, g, b); - } - - /** - * The default command for the lights. - */ - public Command defaultCommand(Supplier intakeNote, Supplier feederNote) { - int[] state = new int[buffer.getLength()]; - for (int i = 0; i < state.length; i++) { - state[i] = 0; - } - - return commandBuilder() - .onExecute(() -> { - if (DriverStation.isTeleopEnabled()) { - if (feederNote.get()) { - if (intakeNote.get()) { - double time = timer.get(); - for (int i = 0; i < buffer.getLength() / 2; i++) { - int v = (int) ((Math.cos((time * 60.0) + ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) * 100.0); - setMirrored(i, v, v, v); - } - } else { - double time = timer.get(); - for (int i = 0; i < buffer.getLength() / 2; i++) { - double v = (Math.cos((time * 20.0) - ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) / 2.0; - setMirrored(i, (int) (v * 255.0), (int) (v * 24.0), (int) (v * 2.0)); - } - } - } else { - int v = (int) (((Math.cos(timer.get() * 8.5) + 1.0) * 87.5) + 25.0); - if (Alliance.isBlue()) { - set(0, 0, v); - } else { - set(v, 0, 0); - } - } - } else if (DriverStation.isAutonomousEnabled()) { - for (int i = 0; i < buffer.getLength() / 2; i++) { - state[i] = (int) Math.max(0.0, state[i] - Math2.random((0.5 + (i / (buffer.getLength() * 0.25))) * 70.0) + 4.0); - } - for (int i = (buffer.getLength() / 2) - 1; i >= 2; i--) { - state[i] = (state[i - 1] + state[i - 2] + state[i - 2]) / 3; - } - if (Math.random() < 0.5) { - int i = (int) Math2.random(8.0); - state[i] = (int) (state[i] + Math2.random(160.0, 255.0)); - } - for (int i = 0; i < buffer.getLength() / 2; i++) { - int heat = (int) ((state[i] / 255.0) * 191.0); - int ramp = (heat & 63) << 2; - if (heat > 180) { - setMirrored(i, 255, 255, ramp); - } else if (heat > 60) { - setMirrored(i, 255, ramp, 0); - } else { - setMirrored(i, ramp, 0, 0); - } - } - } else if (DriverStation.isDisabled()) { - if (Alliance.isBlue()) set(0, 0, 150); else set(150, 0, 0); - } else { - set(0, 0, 0); - } - - apply(); - }) - .onEnd(() -> { - set(0, 0, 0); - apply(); - }) - .ignoringDisable(true) - .withName("lights.defaultCommand()"); - } -} diff --git a/src/main/java/org/team340/robot/subsystems/Pivot.java b/src/main/java/org/team340/robot/subsystems/Pivot.java index 738a2f5..da56b6c 100644 --- a/src/main/java/org/team340/robot/subsystems/Pivot.java +++ b/src/main/java/org/team340/robot/subsystems/Pivot.java @@ -8,220 +8,192 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.Supplier; -import org.team340.lib.GRRSubsystem; -import org.team340.lib.util.Math2; +import org.team340.lib.dashboard.Tunable; +import org.team340.lib.util.GRRSubsystem; +import org.team340.lib.util.Mutable; +import org.team340.lib.util.rev.RelativeEncoderConfig; +import org.team340.lib.util.rev.SparkFlexConfig; +import org.team340.lib.util.rev.SparkPIDControllerConfig; import org.team340.robot.Constants; -import org.team340.robot.Constants.PivotConstants; import org.team340.robot.Constants.RobotMap; /** * The pivot subsystem. */ +@Logged public class Pivot extends GRRSubsystem { - private final CANSparkFlex pivotMotor; - private final RelativeEncoder pivotEncoder; - private final SparkPIDController pivotPID; + public static enum PivotPosition { + /** Down position. */ + kDown(Math.toRadians(20.0)), + /** Position for scoring in the amp. */ + kAmp(Math.toRadians(42.0)), + /** Position for feeding. */ + kFeed(Math.toRadians(45.0)), + /** Position for human loading. */ + kHumanLoad(Math.toRadians(85.0)), + /** Position for fixing deadzone. */ + kFixDeadzone(Math.toRadians(60.0)); + + private final Tunable radians; + + private PivotPosition(double radians) { + this.radians = Tunable.doubleValue("Pivot/Positions/" + this.name(), radians); + } + + private double radians() { + return radians.get(); + } + } + + private static final double kEncoderFactor = Math.toRadians(2.84533); + + private static final Tunable kMinPos = Tunable.doubleValue("Pivot/kMinPos", 18.0); + private static final Tunable kMaxPos = Tunable.doubleValue("Pivot/kMinPos", 89.0); + private static final Tunable kHomingSpeed = Tunable.doubleValue("Pivot/kHomingSpeed", -1.5); + + private static final InterpolatingDoubleTreeMap kRegression = new InterpolatingDoubleTreeMap(); + + static { + kRegression.put(1.31, Math.toRadians(59.01)); + kRegression.put(1.53, Math.toRadians(52.90)); + kRegression.put(1.81, Math.toRadians(46.82)); + kRegression.put(2.09, Math.toRadians(42.10)); + kRegression.put(2.09, Math.toRadians(42.10)); + kRegression.put(2.71, Math.toRadians(37.76)); + kRegression.put(2.83, Math.toRadians(35.65)); + kRegression.put(3.05, Math.toRadians(33.54)); + kRegression.put(3.16, Math.toRadians(33.38)); + kRegression.put(3.45, Math.toRadians(31.25)); + kRegression.put(4.09, Math.toRadians(27.74)); + kRegression.put(4.40, Math.toRadians(26.46)); + kRegression.put(4.44, Math.toRadians(26.39)); + kRegression.put(4.60, Math.toRadians(25.72)); + kRegression.put(4.84, Math.toRadians(23.27)); + kRegression.put(5.07, Math.toRadians(23.15)); + kRegression.put(5.38, Math.toRadians(21.75)); + kRegression.put(5.63, Math.toRadians(21.12)); + kRegression.put(5.94, Math.toRadians(20.39)); + kRegression.put(6.38, Math.toRadians(19.61)); + kRegression.put(6.55, Math.toRadians(20.7)); + kRegression.put(6.87, Math.toRadians(19.76)); + kRegression.put(7.55, Math.toRadians(18.0)); + kRegression.put(8.88, Math.toRadians(16.9)); + kRegression.put(9.71, Math.toRadians(16.88)); + } + + private final CANSparkFlex motor; + private final RelativeEncoder encoder; + private final SparkPIDController pid; private final DigitalInput limit; private boolean isHomed = false; - private double maintain = 0.0; private double target = 0.0; public Pivot() { - super("Pivot"); - pivotMotor = createSparkFlex("Pivot Motor", RobotMap.SHOOTER_PIVOT_MOTOR, MotorType.kBrushless); - pivotEncoder = pivotMotor.getEncoder(); - pivotPID = pivotMotor.getPIDController(); - limit = createDigitalInput("Pivot Lower Limit", RobotMap.PIVOT_LOWER_LIMIT); - - PivotConstants.Configs.MOTOR.apply(pivotMotor); - PivotConstants.Configs.PID.apply(pivotMotor, pivotPID); - PivotConstants.Configs.ENCODER.apply(pivotMotor, pivotEncoder); - } + motor = new CANSparkFlex(RobotMap.kPivotMotor, MotorType.kBrushless); + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + limit = new DigitalInput(RobotMap.kPivotLimit); - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("atLimit", this::getLimit, null); - builder.addBooleanProperty("atPosition", this::atPosition, null); - builder.addBooleanProperty("isHomed", () -> isHomed, null); - builder.addDoubleProperty("maintain", () -> maintain, null); - builder.addDoubleProperty("target", () -> target, null); - builder.addBooleanProperty("safeForIntake", this::isSafeForIntake, null); - } + SparkFlexConfig.defaults().setSmartCurrentLimit(40).setIdleMode(IdleMode.kCoast).setInverted(true).apply(motor); - /** - * Returns {@code true} if the pivot is at its targeted position. - */ - public boolean atPosition() { - return Math2.epsilonEquals(target, pivotEncoder.getPosition(), PivotConstants.CLOSED_LOOP_ERR); - } + new RelativeEncoderConfig() + .setPositionConversionFactor(kEncoderFactor) + .setVelocityConversionFactor(kEncoderFactor / 60) + .apply(motor, encoder); - /** - * Returns {@code true} if the pivot is at a safe position for the intake. - */ - public boolean isSafeForIntake() { - return pivotEncoder.getPosition() <= PivotConstants.INTAKE_SAFE_POSITION; + new SparkPIDControllerConfig().setPID(1.2, 0.004, 0.1).setIZone(Math.toRadians(4.0)).apply(motor, pid); + Tunable.pidController("Pivot/PID", pid); } /** * Returns {@code true} if the limit is pressed. */ - private boolean getLimit() { + public boolean atLimit() { return !limit.get(); } - /** - * Sets the {@link #pivotPID} to go to the specified position if it is valid - * (within the pivot {@link PivotConstants#MIN_POS minimum} and - * {@link PivotConstants#MAX_POS maximum} angles). - * @param position The position to set. - */ - private void applyPosition(double position) { - if ( - position < PivotConstants.MIN_POS - PivotConstants.CLOSED_LOOP_ERR || - position > PivotConstants.MAX_POS + PivotConstants.CLOSED_LOOP_ERR - ) { - DriverStation.reportWarning( - "Invalid shooter pivot position. " + - Math2.toFixed(Math.toDegrees(position)) + - " degrees is not between " + - Math2.toFixed(Math.toDegrees(PivotConstants.MIN_POS)) + - " and " + - Math2.toFixed(Math.toDegrees(PivotConstants.MAX_POS)), - false - ); - } else { - pivotPID.setReference(position, ControlType.kPosition); - target = position; - } - } - /** * Homes the pivot using its limit switch. Doesn't home if the pivot * has already been homed, unless {@code withOverride} is {@code true}. * @param withOverride If {@code true}, ignores {@link #isHomed}. */ public Command home(boolean withOverride) { - return either( - commandBuilder() - .onInitialize(() -> target = PivotConstants.MIN_POS) - .onExecute(() -> pivotMotor.set(PivotConstants.HOMING_SPEED)) - .isFinished(() -> getLimit()) - .onEnd(() -> { - pivotMotor.stopMotor(); - - if (getLimit()) { - pivotEncoder.setPosition(PivotConstants.MIN_POS); - maintain = PivotConstants.MIN_POS; - isHomed = true; - } else { - maintain = pivotEncoder.getPosition(); - } - }), - none(), - () -> withOverride || !isHomed - ) - .withName("pivot.home(" + withOverride + ")"); + return commandBuilder() + .onExecute(() -> { + target = kMinPos.get(); + motor.setVoltage(kHomingSpeed.get()); + }) + .isFinished(this::atLimit) + .onEnd(interrupted -> { + motor.stopMotor(); + if (!interrupted) { + encoder.setPosition(kMinPos.get()); + isHomed = true; + } + }) + .onlyIf(() -> withOverride || !isHomed) + .withName("Pivot.home(" + withOverride + ")"); } /** - * Uses the {@link PivotConstants#DISTANCE_MAP distance map} to - * automatically target the speaker using the supplied distance. - * @param distance A supplier that returns the distance to the speaker in meters. + * Applies a position to the pivot. Does not end. */ - public Command targetDistance(Supplier distance) { - return goTo(() -> PivotConstants.DISTANCE_MAP.get(distance.get()), false).withName("pivot.targetDistance()"); + public Command apply(PivotPosition position) { + return goTo(position::radians).withName("Pivot.apply(" + position.name() + ")"); } /** - * Sets the pivot to feed. - * @param pastMidline A supplier that returns {@code true} when the robot is past the midline. + * Uses the {@link #kRegression} to automatically target the + * speaker using the supplied distance. Does not end. + * @param distance A supplier that returns the distance to the speaker in meters. */ - public Command feed(Supplier pastMidline) { - return goTo(() -> pastMidline.get() ? PivotConstants.MARY_POPPINS_POSITION : PivotConstants.ROCK_SKIP_POSITION, false) - .withName("pivot.feed()"); + public Command targetSpeaker(Supplier distance) { + return goTo(() -> kRegression.get(distance.get())).withName("Pivot.targetSpeaker()"); } /** - * Moves to a position. Ends after the position is reached. - * @param position The position for the pivot to move to in radians. + * Drives the pivot manually. Does not end. + * @param speed The speed of the pivot in radians/second. */ - public Command goTo(double position) { - return goTo(() -> position, true).withName("pivot.goTo(" + Math2.formatRadians(position) + ")"); + public Command manual(Supplier speed) { + Mutable last = new Mutable<>(0.0); + return goTo(() -> { + double diff = speed.get() * Constants.kPeriod; + if (atLimit() || encoder.getPosition() < kMinPos.get()) { + diff = Math.max(diff, 0.0); + } else if (encoder.getPosition() > kMaxPos.get()) { + diff = Math.min(diff, 0.0); + } + + last.accept(last.value + diff); + return last.value; + }) + .beforeStarting(() -> last.accept(encoder.getPosition())) + .withName("Pivot.manual()"); } /** - * Moves to supplied positions. + * Moves to supplied position. Does not end. * @param position A supplier that returns a position to target in radians. - * @param willFinish If {@code true}, the command will end after the current target position is reached. - */ - private Command goTo(Supplier position, boolean willFinish) { - return home(false) - .andThen(waitSeconds(0.1)) - .andThen( - commandBuilder() - .onExecute(() -> applyPosition(position.get())) - .isFinished(() -> - (getLimit() && pivotMotor.getAppliedOutput() - PivotConstants.AT_LIMIT_SPEED_ALLOWANCE <= 0.0) || - (willFinish && atPosition()) - ) - .onEnd(interrupted -> { - if (interrupted || getLimit()) { - maintain = pivotEncoder.getPosition(); - } else { - maintain = position.get(); - } - }) - ) - .withName("pivot.goTo(" + willFinish + ")"); - } - - /** - * Maintains the last set position. - */ - public Command maintainPosition() { - return commandBuilder("pivot.maintainPosition()") - .onExecute(() -> { - if (isHomed) { - applyPosition(maintain); - } else { - pivotMotor.stopMotor(); - } - }); - } - - /** - * Drives the pivot manually. Will hold position. - * @param speed The speed of the pivot in radians/second. */ - public Command driveManual(Supplier speed) { - return home(false) - .andThen(waitSeconds(0.1)) - .andThen( - commandBuilder() - .onExecute(() -> { - double diff = speed.get() * Constants.PERIOD; - if (getLimit()) { - diff = Math.max(diff, 0.0); - } else if (pivotEncoder.getPosition() > PivotConstants.MAX_POS) { - diff = Math.min(diff, 0.0); - } - - maintain += diff; - applyPosition(maintain); - }) - .onEnd(() -> { - if (getLimit()) maintain = pivotEncoder.getPosition(); - }) - ) - .withName("pivot.driveManual()"); + private Command goTo(Supplier position) { + return sequence( + home(false), + commandBuilder() + .onExecute(() -> { + target = MathUtil.clamp(position.get(), kMinPos.get(), kMaxPos.get()); + pid.setReference(target, ControlType.kPosition); + }) + .onEnd(() -> motor.stopMotor()) + ).withName("Pivot.goTo()"); } /** @@ -230,11 +202,11 @@ public Command driveManual(Supplier speed) { public Command onDisable() { return commandBuilder() .onInitialize(() -> { - pivotMotor.setIdleMode(IdleMode.kCoast); - pivotMotor.stopMotor(); + motor.stopMotor(); + motor.setIdleMode(IdleMode.kCoast); }) - .onEnd(() -> pivotMotor.setIdleMode(IdleMode.kBrake)) + .onEnd(() -> motor.setIdleMode(IdleMode.kBrake)) .ignoringDisable(true) - .withName("pivot.onDisable()"); + .withName("Pivot.onDisable()"); } } diff --git a/src/main/java/org/team340/robot/subsystems/Shooter.java b/src/main/java/org/team340/robot/subsystems/Shooter.java index 31223b5..95f0dc6 100644 --- a/src/main/java/org/team340/robot/subsystems/Shooter.java +++ b/src/main/java/org/team340/robot/subsystems/Shooter.java @@ -1,316 +1,214 @@ package org.team340.robot.subsystems; -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; - import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Velocity; -import edu.wpi.first.units.Voltage; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.function.Supplier; -import org.team340.lib.GRRSubsystem; -import org.team340.lib.util.Math2; +import org.team340.lib.dashboard.Tunable; +import org.team340.lib.util.GRRSubsystem; +import org.team340.lib.util.Mutable; +import org.team340.lib.util.rev.RelativeEncoderConfig; +import org.team340.lib.util.rev.SparkFlexConfig; +import org.team340.lib.util.rev.SparkPIDControllerConfig; import org.team340.robot.Constants; import org.team340.robot.Constants.RobotMap; -import org.team340.robot.Constants.ShooterConstants; /** * The shooter subsystem. */ +@Logged public class Shooter extends GRRSubsystem { - private final CANSparkFlex leftShootMotor; - private final CANSparkFlex rightShootMotor; - private final RelativeEncoder leftEncoder; - private final RelativeEncoder rightEncoder; - private final SparkPIDController leftShootPID; - private final SparkPIDController rightShootPID; - private final SimpleMotorFeedforward leftFeedForward; - private final SimpleMotorFeedforward rightFeedForward; - - private final SysIdRoutine sysIdRoutine; - private final MutableMeasure sysIdAppliedVoltage = mutable(Volts.of(0)); - private final MutableMeasure sysIdPosition = mutable(Rotations.of(0)); - private final MutableMeasure> sysIdVelocity = mutable(RotationsPerSecond.of(0)); - - private double leftTargetSpeed = 0.0; - private double rightTargetSpeed = 0.0; - private boolean leftPIDActive = false; - private boolean rightPIDActive = false; - - public Shooter() { - super("Shooter"); - leftShootMotor = createSparkFlex("Left Motor", RobotMap.SHOOTER_SHOOT_LEFT_MOTOR, MotorType.kBrushless); - rightShootMotor = createSparkFlex("Right Motor", RobotMap.SHOOTER_SHOOT_RIGHT_MOTOR, MotorType.kBrushless); - leftEncoder = leftShootMotor.getEncoder(); - rightEncoder = rightShootMotor.getEncoder(); - leftShootPID = leftShootMotor.getPIDController(); - rightShootPID = rightShootMotor.getPIDController(); - leftFeedForward = ShooterConstants.Configs.FEED_FORWARD_LEFT.simpleMotorFeedForward(); - rightFeedForward = ShooterConstants.Configs.FEED_FORWARD_RIGHT.simpleMotorFeedForward(); + public static enum ShooterSpeed { + /** Speed for scoring in the amp. */ + kAmp(true, 2000.0), + /** Speed for feeding. */ + kFeed(true, 4500.0), + /** Speed for human loading. */ + kHumanLoad(false, -4.0), + /** Speed for fixing deadzone. */ + kFixDeadzone(false, -8.0), + /** Speed for barfing forwards (towards the intake). */ + kBarfForward(false, -6.0), + /** Speed for barfing backwards (towards the shooter). */ + kBarfBackward(false, 6.0); + + private final boolean rpm; + private final Tunable value; + + private ShooterSpeed(boolean rpm, double value) { + this.rpm = rpm; + this.value = Tunable.doubleValue("Shooter/Speeds/" + this.name(), value); + } - sysIdRoutine = - new SysIdRoutine( - ShooterConstants.Configs.SYSID, - new SysIdRoutine.Mechanism( - (Measure volts) -> { - leftShootMotor.setVoltage(volts.in(Volts)); - rightShootMotor.setVoltage(volts.in(Volts)); - }, - log -> { - log - .motor("shooter-left") - .voltage( - sysIdAppliedVoltage.mut_replace( - leftShootMotor.getAppliedOutput() * RobotController.getBatteryVoltage(), - Volts - ) - ) - .angularPosition(sysIdPosition.mut_replace(leftEncoder.getPosition(), Rotations)) - .angularVelocity(sysIdVelocity.mut_replace(leftEncoder.getVelocity() / 60.0, RotationsPerSecond)); + private double value() { + return value.get(); + } + } - log - .motor("shooter-right") - .voltage( - sysIdAppliedVoltage.mut_replace( - rightShootMotor.getAppliedOutput() * RobotController.getBatteryVoltage(), - Volts - ) - ) - .angularPosition(sysIdPosition.mut_replace(rightEncoder.getPosition(), Rotations)) - .angularVelocity(sysIdVelocity.mut_replace(rightEncoder.getVelocity() / 60.0, RotationsPerSecond)); - }, - this - ) - ); + private static final double kRightLeftRatio = 0.5; - ShooterConstants.Configs.LEFT_MOTOR.apply(leftShootMotor); - ShooterConstants.Configs.RIGHT_MOTOR.apply(rightShootMotor); - ShooterConstants.Configs.ENCODER.apply(leftShootMotor, leftEncoder); - ShooterConstants.Configs.ENCODER.apply(rightShootMotor, rightEncoder); - ShooterConstants.Configs.PID.apply(leftShootMotor, leftShootPID); - ShooterConstants.Configs.PID.apply(rightShootMotor, rightShootPID); - } + private static final Tunable kLeftKs = Tunable.doubleValue("Shooter/LeftFF/kS", 0.11331 / 60.0); + private static final Tunable kLeftKv = Tunable.doubleValue("Shooter/LeftFF/kV", 0.065448 / 60.0); + private static final Tunable kRightKs = Tunable.doubleValue("Shooter/RightFF/kS", 0.11331 / 60.0); + private static final Tunable kRightKv = Tunable.doubleValue("Shooter/RightFF/kV", 0.065448 / 60.0); + private static final Tunable kPIDRange = Tunable.doubleValue("Shooter/kPIDRange", 750.0); - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("atSpeed", this::atSpeed, null); - builder.addDoubleProperty("leftTarget", () -> leftTargetSpeed, null); - builder.addDoubleProperty("rightTarget", () -> rightTargetSpeed, null); - builder.addBooleanProperty("leftPIDActive", () -> leftPIDActive, null); - builder.addBooleanProperty("rightPIDActive", () -> rightPIDActive, null); - } + private static final InterpolatingDoubleTreeMap kRegression = new InterpolatingDoubleTreeMap(); - /** - * Returns {@code true} if the shooter is at speed. - */ - public boolean atSpeed() { - return ( - Math2.epsilonEquals(leftTargetSpeed, leftEncoder.getVelocity(), ShooterConstants.CLOSED_LOOP_ERR) && - Math2.epsilonEquals(rightTargetSpeed, rightEncoder.getVelocity(), ShooterConstants.CLOSED_LOOP_ERR) - ); + static { + kRegression.put(0.0, 3000.0); + kRegression.put(6.0, 6500.0); + kRegression.put(10.0, 7750.0); } - /** - * Applies a specified speed to the shooter. - * @param speed The speed in RPM to be applied to the left motor, which is scaled by {@link ShooterConstants#RIGHT_PERCENT_OF_LEFT} and applied to the right motor. - */ - private void applySpeed(double speed) { - double leftSpeed = speed; - double rightSpeed = speed * ShooterConstants.RIGHT_PERCENT_OF_LEFT; - - if (speed == 0.0) { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); - leftPIDActive = false; - rightPIDActive = false; - } else { - double leftDelta = leftSpeed - leftEncoder.getVelocity(); - if (Math.abs(leftDelta) < ShooterConstants.PID_ACTIVE_RANGE) { - leftShootPID.setReference(leftSpeed, ControlType.kVelocity, 0, leftFeedForward.calculate(leftSpeed)); - leftPIDActive = true; - } else { - leftShootMotor.set( - (leftDelta * Math.signum(leftSpeed) > 0.0 ? ShooterConstants.RAMP_UP_SPEED : ShooterConstants.RAMP_DOWN_SPEED) * - Math.signum(leftSpeed) - ); - leftPIDActive = false; - } + private final CANSparkFlex leftMotor; + private final CANSparkFlex rightMotor; + private final RelativeEncoder leftEncoder; + private final RelativeEncoder rightEncoder; + private final SparkPIDController leftPID; + private final SparkPIDController rightPID; - double rightDelta = rightSpeed - rightEncoder.getVelocity(); - if (Math.abs(rightDelta) < ShooterConstants.PID_ACTIVE_RANGE) { - rightShootPID.setReference(rightSpeed, ControlType.kVelocity, 0, rightFeedForward.calculate(rightSpeed)); - rightPIDActive = true; - } else { - rightShootMotor.set( - (rightDelta * Math.signum(rightSpeed) > 0.0 ? ShooterConstants.RAMP_UP_SPEED : ShooterConstants.RAMP_DOWN_SPEED) * - Math.signum(rightSpeed) - ); - rightPIDActive = false; - } - } + private double leftTarget = 0.0; + private double rightTarget = 0.0; - leftTargetSpeed = leftSpeed; - rightTargetSpeed = rightSpeed; + public Shooter() { + leftMotor = new CANSparkFlex(RobotMap.kShooterLeftMotor, MotorType.kBrushless); + rightMotor = new CANSparkFlex(RobotMap.kShooterRightMotor, MotorType.kBrushless); + leftEncoder = leftMotor.getEncoder(); + rightEncoder = rightMotor.getEncoder(); + leftPID = leftMotor.getPIDController(); + rightPID = rightMotor.getPIDController(); + + SparkFlexConfig.defaults() + .setSmartCurrentLimit(50, 40) + .setIdleMode(IdleMode.kCoast) + .setInverted(true) + .apply(leftMotor); + + SparkFlexConfig.defaults() + .setSmartCurrentLimit(50, 40) + .setIdleMode(IdleMode.kCoast) + .setInverted(false) + .apply(rightMotor); + + new RelativeEncoderConfig() + .setPositionConversionFactor(1.0) + .setVelocityConversionFactor(1.0) + .setMeasurementPeriod(32) + .setAverageDepth(8) + .apply(leftMotor, leftEncoder); + + new RelativeEncoderConfig() + .setPositionConversionFactor(0.5) + .setVelocityConversionFactor(0.5) + .setMeasurementPeriod(32) + .setAverageDepth(8) + .apply(rightMotor, rightEncoder); + + new SparkPIDControllerConfig().setPID(0.00047, 0.000002, 0.0).setIZone(100.0).apply(leftMotor, leftPID); + + new SparkPIDControllerConfig().setPID(0.00047, 0.000002, 0.0).setIZone(100.0).apply(rightMotor, rightPID); + + Tunable.pidController("Shooter/LeftPID", leftPID); + Tunable.pidController("Shooter/RightPID", rightPID); } /** - * Uses the {@link ShooterConstants#DISTANCE_MAP distance map} to - * automatically target the speaker using the supplied distance. - * Does not end. - * @param distance A supplier that returns the distance to the speaker in meters. + * Applies a speed to the shooter. Does not end. + * @param speed The speed to apply. */ - public Command targetDistance(Supplier distance) { - return targetDistance(distance, 0.0, () -> false).withName("shooter.targetDistance()"); + public Command apply(ShooterSpeed speed) { + return applySpeed(speed.rpm, speed::value).withName("Shooter.apply(" + speed.name() + ")"); } /** - * Uses the {@link ShooterConstants#DISTANCE_MAP distance map} to - * automatically target the speaker using the supplied distance. - * If the idle supplier is {@code true}, the {@code idleSpeed} is used instead. - * Does not end. + * Uses the {@link #kRegression} to automatically target the + * speaker using the supplied distance. Does not end. * @param distance A supplier that returns the distance to the speaker in meters. - * @param idleSpeed The speed for the shooter to target in RPM while the idle supplier is {@code true}. - * @param idle A supplier that while {@code true} will set the shooter to target the specified idle speed. */ - public Command targetDistance(Supplier distance, double idleSpeed, Supplier idle) { - return setSpeed(() -> idle.get() ? idleSpeed : ShooterConstants.DISTANCE_MAP.get(distance.get())) - .withName("shooter.targetDistance(idleSpeed: " + idleSpeed + ")"); + public Command targetSpeaker(Supplier distance) { + return applySpeed(true, () -> kRegression.get(distance.get())).withName("Shooter.targetSpeaker()"); } /** - * Sets the speed of the shooter. Does not end. - * @param speed The speed for the shooter to target in RPM. - */ - public Command setSpeed(double speed) { - return setSpeed(() -> speed).withName("shooter.setSpeed(" + speed + ")"); - } - - /** - * Sets the speed of the shooter. Does not end. - * @param speed A supplier that returns a speed for the shooter to target in RPM. - */ - public Command setSpeed(Supplier speed) { - return commandBuilder("shooter.setSpeed()") - .onExecute(() -> applySpeed(speed.get())) - .onEnd(() -> { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); - }); - } - - /** - * Sets the shooter to receive a note from the human player. + * Drives shooter by modifying a moving target RPM. Does not end. + * @param rampSpeed The speed to ramp the shooter by in RPM/second. */ - public Command intakeHuman() { - return commandBuilder("shooter.intakeHuman()") - .onInitialize(() -> { - leftShootMotor.set(ShooterConstants.INTAKE_HUMAN_SPEED); - rightShootMotor.set(ShooterConstants.INTAKE_HUMAN_SPEED); - }) - .onEnd(() -> { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); - }); + public Command manual(Supplier rampSpeed) { + Mutable last = new Mutable<>(0.0); + return applySpeed(true, () -> { + last.accept(last.value + (rampSpeed.get() * Constants.kPeriod)); + return last.value; + }) + .beforeStarting(() -> last.accept(leftEncoder.getVelocity())) + .withName("Shooter.manual()"); } /** - * Sets the shooter to feed. - * @param pastMidline A supplier that returns {@code true} when the robot is past the midline. + * Applies a speed to the shooter. Does not end. + * @param rpm If the speed is in RPM. Otherwise, volts are expected. + * @param value The speed to apply. */ - public Command feed(Supplier pastMidline) { - return commandBuilder("shooter.feed()") + private Command applySpeed(boolean rpm, Supplier value) { + return commandBuilder("Shooter.applySpeed()") .onExecute(() -> { - double speed = pastMidline.get() ? ShooterConstants.MARY_POPPINS_SPEED : ShooterConstants.ROCK_SKIP_SPEED; - leftShootMotor.set(speed); - rightShootMotor.set(speed); - }) - .onEnd(() -> { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); - }); - } - - /** - * Runs the shooter towards the intake to fix deadzone. - */ - public Command fixDeadzone() { - return commandBuilder("shooter.fixDeadzone()") - .onInitialize(() -> { - leftShootMotor.set(ShooterConstants.FIX_DEADZONE_SPEED); - rightShootMotor.set(ShooterConstants.FIX_DEADZONE_SPEED); - }) - .onEnd(() -> { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); - }); - } - - /** - * Spits the note back towards the intake. - */ - public Command barfForward() { - return commandBuilder("shooter.barfForward()") - .onInitialize(() -> { - leftShootMotor.set(ShooterConstants.FORWARD_BARF_SPEED); - rightShootMotor.set(ShooterConstants.FORWARD_BARF_SPEED); + double v = value.get(); + leftTarget = 0.0; + rightTarget = 0.0; + + if (v == 0.0) { + leftMotor.stopMotor(); + rightMotor.stopMotor(); + return; + } + + if (!rpm) { + leftMotor.setVoltage(v); + rightMotor.setVoltage(v); + return; + } + + leftTarget = v; + rightTarget = v * kRightLeftRatio; + + double leftDelta = leftTarget - leftEncoder.getVelocity(); + if (Math.abs(leftDelta) < kPIDRange.get()) { + leftPID.setReference( + leftTarget, + ControlType.kVelocity, + 0, + kLeftKs.get() * Math.signum(leftTarget) + kLeftKv.get() * leftTarget + ); + } else { + leftMotor.setVoltage( + (Math.copySign(leftDelta, leftTarget) > 0.0 ? 12.0 : 0.0) * Math.signum(leftTarget) + ); + } + + double rightDelta = rightTarget - rightEncoder.getVelocity(); + if (Math.abs(rightDelta) < kPIDRange.get()) { + rightPID.setReference( + rightTarget, + ControlType.kVelocity, + 0, + kRightKs.get() * Math.signum(rightTarget) + kRightKv.get() * rightTarget + ); + } else { + rightMotor.setVoltage( + (Math.copySign(rightDelta, rightTarget) > 0.0 ? 12.0 : 0.0) * Math.signum(rightTarget) + ); + } }) .onEnd(() -> { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); + leftMotor.stopMotor(); + rightMotor.stopMotor(); }); } - - /** - * Spits the note out of the shooter. - */ - public Command barfBackward() { - return commandBuilder("shooter.barfBackward()") - .onInitialize(() -> { - leftShootMotor.set(ShooterConstants.BACKWARD_BARF_SPEED); - rightShootMotor.set(ShooterConstants.BACKWARD_BARF_SPEED); - }) - .onEnd(() -> { - leftShootMotor.stopMotor(); - rightShootMotor.stopMotor(); - }); - } - - /** - * Drives shooter by modifying a moving target RPM. - * @param rampSpeed The speed to ramp the shooter by in RPM/second. - */ - public Command driveManual(Supplier rampSpeed) { - return setSpeed(() -> leftTargetSpeed + rampSpeed.get() * Constants.PERIOD); - } - - /** - * Runs a SysId quasistatic test. - * @param direction The direction to run the test in. - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysIdRoutine.quasistatic(direction).withName("shooter.sysIdQuasistaitc(" + direction + ")"); - } - - /** - * Runs a SysId dynamic test. - * @param direction The direction to run the test in. - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysIdRoutine.dynamic(direction).withName("shooter.sysIdDynamic(" + direction + ")"); - } } diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index 239be92..cdb2a10 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -2,255 +2,132 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoTrajectory; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.Trajectory.State; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.ArrayList; import java.util.List; import java.util.Optional; -import java.util.function.BiConsumer; import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.team340.lib.commands.CommandBuilder; -import org.team340.lib.swerve.SwerveBase; -import org.team340.lib.swerve.SwerveModule; -import org.team340.lib.swerve.util.SwerveVisualizer; +import org.team340.lib.dashboard.Tunable; +import org.team340.lib.swerve.SwerveAPI; +import org.team340.lib.swerve.SwerveAPI.ForwardPerspective; import org.team340.lib.util.Alliance; -import org.team340.lib.util.Math2; -import org.team340.robot.Constants; +import org.team340.lib.util.GRRSubsystem; import org.team340.robot.Constants.FieldPositions; import org.team340.robot.Constants.SwerveConstants; /** * The swerve subsystem. */ -public class Swerve extends SwerveBase { - - private final PIDController xPIDTraj = SwerveConstants.TRAJ_XY_PID.pidController(); - private final PIDController yPIDTraj = SwerveConstants.TRAJ_XY_PID.pidController(); - private final PIDController rotPIDTraj = SwerveConstants.TRAJ_ROT_PID.pidController(); - private final PIDController targetPIDTraj = SwerveConstants.TRAJ_TARGET_PID.pidController(); - private final ProfiledPIDController rotPIDTrajProfiled = SwerveConstants.TRAJ_ROT_PID.profiledPIDController( - SwerveConstants.TRAJ_ROT_CONSTRAINTS - ); - - private final HolonomicDriveController trajectoryController = new HolonomicDriveController(xPIDTraj, yPIDTraj, rotPIDTrajProfiled); +@Logged +public class Swerve extends GRRSubsystem { - private final PIDController xPID = SwerveConstants.XY_PID.pidController(); - private final PIDController yPID = SwerveConstants.XY_PID.pidController(); - private final ProfiledPIDController rotPID = SwerveConstants.ROT_PID.profiledPIDController(SwerveConstants.ROT_CONSTRAINTS); + private final SwerveAPI api; - private final AprilTagFieldLayout blueAprilTags; - private final AprilTagFieldLayout redAprilTags; - private final PhotonPoseEstimator[] photonPoseEstimators; + private final ProfiledPIDController rotPID; - private double[] speaker = new double[0]; - private double tunableNoteVelocity = SwerveConstants.NOTE_VELOCITY; - private double tunableNormFudge = SwerveConstants.NORM_FUDGE; - private double tunableStrafeFudge = SwerveConstants.STRAFE_FUDGE; - private double tunableSpinCompensation = SwerveConstants.SPIN_COMPENSATION; - private double tunableDistanceFudge = 0.0; - private double tunableSpeakerXFudge = 0.0; - private double tunableSpeakerYFudge = 0.0; - private double tunableAmpXFudge = 0.0; - private double tunableAmpYFudge = 0.0; + private final AprilTagFieldLayout aprilTags; + private final PhotonPoseEstimator[] poseEstimators; + private final List measurements = new ArrayList<>(); + private final List targets = new ArrayList<>(); - private double effectiveWheelRadius = -1.0; + private Tunable noteVelocity = Tunable.doubleValue("Swerve/noteVelocity", SwerveConstants.NOTE_VELOCITY); + private Tunable normFudge = Tunable.doubleValue("Swerve/normFudge", SwerveConstants.NORM_FUDGE); + private Tunable strafeFudge = Tunable.doubleValue("Swerve/strafeFudge", SwerveConstants.STRAFE_FUDGE); + private Tunable spinCompensation = Tunable.doubleValue( + "Swerve/spinCompensation", + SwerveConstants.SPIN_COMPENSATION + ); + private Tunable speakerXFudge = Tunable.doubleValue("Swerve/speakerXFudge", 0.0); + private Tunable speakerYFudge = Tunable.doubleValue("Swerve/speakerYFudge", 0.0); /** * Create the swerve subsystem. */ public Swerve() { - super("Swerve Drive", SwerveConstants.CONFIG); - rotPIDTraj.enableContinuousInput(-Math.PI, Math.PI); - rotPIDTrajProfiled.enableContinuousInput(-Math.PI, Math.PI); + api = new SwerveAPI(SwerveConstants.CONFIG); + api.enableTunables("Swerve"); + + rotPID = new ProfiledPIDController( + SwerveConstants.ROT_PID_KP, + SwerveConstants.ROT_PID_KI, + SwerveConstants.ROT_PID_KD, + SwerveConstants.ROT_CONSTRAINTS + ); + rotPID.setIZone(SwerveConstants.ROT_PID_IZONE); rotPID.enableContinuousInput(-Math.PI, Math.PI); - - blueAprilTags = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - redAprilTags = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - blueAprilTags.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - redAprilTags.setOrigin(OriginPosition.kRedAllianceWallRightSide); - - photonPoseEstimators = - new PhotonPoseEstimator[] { - new PhotonPoseEstimator( - blueAprilTags, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - new PhotonCamera("FrontLeft"), - SwerveConstants.FRONT_LEFT_CAMERA - ), - new PhotonPoseEstimator( - blueAprilTags, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - new PhotonCamera("BackLeft"), - SwerveConstants.BACK_LEFT_CAMERA - ), - new PhotonPoseEstimator( - blueAprilTags, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - new PhotonCamera("BackRight"), - SwerveConstants.BACK_RIGHT_CAMERA - ), - new PhotonPoseEstimator( - blueAprilTags, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - new PhotonCamera("FrontRight"), - SwerveConstants.FRONT_RIGHT_CAMERA - ), - }; - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addBooleanProperty("facingSpeaker", this::facingSpeaker, null); - builder.addDoubleProperty("speakerX", () -> getSpeakerPosition().getX(), null); - builder.addDoubleProperty("speakerY", () -> getSpeakerPosition().getY(), null); - builder.addDoubleProperty("speakerDistance", this::getSpeakerDistance, null); - builder.addDoubleProperty("effectiveWheelRadius", () -> effectiveWheelRadius, null); - builder.addBooleanProperty("inOpponentWing", this::inOpponentWing, null); - builder.addDoubleArrayProperty("speaker", () -> speaker, null); - - builder.addDoubleProperty("tunableNoteVelocity", null, velocity -> tunableNoteVelocity = velocity); - builder.addDoubleProperty("tunableNormFudge", null, fudge -> tunableNormFudge = fudge); - builder.addDoubleProperty("tunableStrafeFudge", null, fudge -> tunableStrafeFudge = fudge); - builder.addDoubleProperty("tunableSpinCompensation", null, compensation -> tunableSpinCompensation = compensation); - builder.addDoubleProperty("tunableDistanceFudge", null, fudge -> tunableDistanceFudge = fudge); - builder.addDoubleProperty("tunableSpeakerXFudge", null, fudge -> tunableSpeakerXFudge = fudge); - builder.addDoubleProperty("tunableSpeakerYFudge", null, fudge -> tunableSpeakerYFudge = fudge); - builder.addDoubleProperty("tunableAmpXFudge", null, fudge -> tunableAmpXFudge = fudge); - builder.addDoubleProperty("tunableAmpYFudge", null, fudge -> tunableAmpYFudge = fudge); + Tunable.pidController("Swerve/rotPID", rotPID); + + aprilTags = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + aprilTags.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + poseEstimators = new PhotonPoseEstimator[] { + new PhotonPoseEstimator( + aprilTags, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + new PhotonCamera("backleft"), + SwerveConstants.BACK_LEFT_CAMERA + ), + new PhotonPoseEstimator( + aprilTags, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + new PhotonCamera("backright"), + SwerveConstants.BACK_RIGHT_CAMERA + ) + }; } @Override public void periodic() { - updateOdometry(poseEstimator -> { - List measurements = new ArrayList<>(); - List targets = new ArrayList<>(); - - Pose2d currentPose = getPosition(); - for (int i = 0; i < photonPoseEstimators.length; i++) { - Optional pose = photonPoseEstimators[i].update(); - if (pose.isPresent()) { - Pose3d raw = pose.get().estimatedPose; - Pose3d pose3d = Alliance.isBlue() - ? raw - : new Pose3d( - Constants.FIELD_LENGTH - raw.getX(), - Constants.FIELD_WIDTH - raw.getY(), - raw.getZ(), - raw.getRotation().minus(new Rotation3d(0.0, 0.0, Math.PI)) - ); - Pose2d pose2d = pose3d.toPose2d(); - if ( - pose3d.getX() >= -SwerveConstants.VISION_FIELD_MARGIN && - pose3d.getX() <= Constants.FIELD_LENGTH + SwerveConstants.VISION_FIELD_MARGIN && - pose3d.getY() >= -SwerveConstants.VISION_FIELD_MARGIN && - pose3d.getY() <= Constants.FIELD_WIDTH + SwerveConstants.VISION_FIELD_MARGIN && - pose3d.getZ() >= -SwerveConstants.VISION_Z_MARGIN && - pose3d.getZ() <= SwerveConstants.VISION_Z_MARGIN - ) { - double sum = 0.0; - for (PhotonTrackedTarget target : pose.get().targetsUsed) { - Optional tagPose = - (Alliance.isBlue() ? blueAprilTags : redAprilTags).getTagPose(target.getFiducialId()); - if (tagPose.isEmpty()) continue; - targets.add(tagPose.get()); - sum += currentPose.getTranslation().getDistance(tagPose.get().getTranslation().toTranslation2d()); - } - - int tagCount = pose.get().targetsUsed.size(); - double stdScale = Math.pow(sum / tagCount, 2.0) / tagCount; - double xyStd = SwerveConstants.VISION_STD_XY_SCALE * stdScale; - double rotStd = SwerveConstants.VISION_STD_ROT_SCALE * stdScale; - - poseEstimator.addVisionMeasurement(pose2d, pose.get().timestampSeconds, VecBuilder.fill(xyStd, xyStd, rotStd)); - measurements.add(pose2d); - continue; - } + api.refresh(); + + measurements.clear(); + targets.clear(); + + for (int i = 0; i < poseEstimators.length; i++) { + var estimate = poseEstimators[i].update(); + if (estimate.isPresent()) { + double weightedSum = 0.0; + for (var target : estimate.get().targetsUsed) { + int id = target.getFiducialId(); + Optional tagPose = aprilTags.getTagPose(id); + if (tagPose.isEmpty()) continue; + targets.add(tagPose.get()); + double distance = api.state.pose + .getTranslation() + .getDistance(tagPose.get().getTranslation().toTranslation2d()); + boolean important = id == 3 || id == 4 || id == 7 || id == 8; + weightedSum += distance * (important ? 0.65 : 1.0); } - } - - visualizer.updateVision(measurements.stream().toArray(Pose2d[]::new), targets.stream().toArray(Pose3d[]::new)); - }); - } - - /** - * Returns {@code true} if the robot is in the opponent's wing. - */ - public boolean inOpponentWing() { - return getPosition().getX() > FieldPositions.OPPONENT_WING_LINE; - } - - /** - * Returns {@code true} if the robot is past the midline. - */ - public boolean pastMidline() { - return getPosition().getX() > FieldPositions.MIDLINE; - } - /** - * Returns the distance from the speaker in meters, adjusted for the robot's movement. - */ - public double getSpeakerDistance() { - return ( - getPosition().getTranslation().getDistance(getSpeakerPosition()) + - (Alliance.isBlue() ? SwerveConstants.DISTANCE_FUDGE_BLUE : SwerveConstants.DISTANCE_FUDGE_RED) + - tunableDistanceFudge - ); - } + Pose3d estimatedPose = estimate.get().estimatedPose; + int tagCount = estimate.get().targetsUsed.size(); + double stdScale = Math.pow(weightedSum / tagCount, 2.0) / tagCount; + double xyStd = 0.2 * stdScale; + double angStd = 0.3 * stdScale; - /** - * Returns the distance to the amp in meters. - */ - public double getAmpDistance() { - return getPosition() - .getTranslation() - .getDistance((Alliance.isBlue() ? FieldPositions.AMP_SCORE_BLUE : FieldPositions.AMP_SCORE_RED).getTranslation()); - } - - /** - * Returns {@code true} if the robot is facing the speaker and on target. - */ - public boolean facingSpeaker() { - return Math2.epsilonEquals( - getPosition().getRotation().minus(new Rotation2d(getSpeakerAngle())).getRadians(), - 0.0, - SwerveConstants.FACING_SPEAKER_EPSILON - ); - } + api.addVisionMeasurement( + estimatedPose.toPose2d(), + estimate.get().timestampSeconds, + VecBuilder.fill(xyStd, xyStd, angStd) + ); - /** - * Returns the distance to the center of the stage in meters. - */ - public double getStageDistance() { - return getPosition().getTranslation().getDistance(FieldPositions.STAGE); + measurements.add(estimatedPose.toPose2d()); + } + } } /** @@ -259,185 +136,95 @@ public double getStageDistance() { * @return A {@link Translation2d} representing a field relative position in meters. */ public Translation2d getSpeakerPosition() { - Translation2d goalPose = Alliance.isBlue() ? FieldPositions.BLUE_SPEAKER : FieldPositions.RED_SPEAKER; - Translation2d robotPos = getPosition().getTranslation(); - ChassisSpeeds robotVel = getVelocity(true); + Translation2d goalPose = Alliance.isBlue() ? FieldPositions.kBlueSpeaker : FieldPositions.kRedSpeaker; + Translation2d robotPos = api.state.pose.getTranslation(); + ChassisSpeeds robotVel = api.state.speeds; double distance = robotPos.getDistance(goalPose); - double normFactor = Math.hypot(robotVel.vxMetersPerSecond, robotVel.vyMetersPerSecond) < SwerveConstants.NORM_FUDGE_MIN + double normFactor = Math.hypot(robotVel.vxMetersPerSecond, robotVel.vyMetersPerSecond) < + SwerveConstants.NORM_FUDGE_MIN ? 0.0 : Math.abs( MathUtil.angleModulus( - robotPos.minus(goalPose).getAngle().getRadians() - Math.atan2(robotVel.vyMetersPerSecond, robotVel.vxMetersPerSecond) + robotPos.minus(goalPose).getAngle().getRadians() - + Math.atan2(robotVel.vyMetersPerSecond, robotVel.vxMetersPerSecond) ) / Math.PI ); double x = goalPose.getX() + - tunableSpeakerXFudge - - (robotVel.vxMetersPerSecond * (distance / tunableNoteVelocity) * (1.0 - (tunableNormFudge * normFactor))); + speakerXFudge.get() - + (robotVel.vxMetersPerSecond * (distance / noteVelocity.get()) * (1.0 - (normFudge.get() * normFactor))); double y = goalPose.getY() + - (Alliance.isBlue() ? tunableSpeakerYFudge : -tunableSpeakerYFudge) - - (robotVel.vyMetersPerSecond * (distance / tunableNoteVelocity) * tunableStrafeFudge); + speakerYFudge.get() - + (robotVel.vyMetersPerSecond * (distance / noteVelocity.get()) * strafeFudge.get()); - speaker = SwerveVisualizer.pose3d(new Pose3d(x, y, FieldPositions.SPEAKER_HEIGHT, Math2.ROTATION3D_0)); return new Translation2d(x, y); } /** - * Gets the angle for the robot to face to score in the speaker, - * in radians. Compensates for note drift caused by spin. - */ - private double getSpeakerAngle() { - Translation2d speakerPosition = getSpeakerPosition(); - Translation2d robotPoint = getPosition().getTranslation(); - return MathUtil.angleModulus(speakerPosition.minus(robotPoint).getAngle().getRadians() + Math.PI + tunableSpinCompensation); - } - - /** - * Returns the robot's yaw in radians. - */ - public double getYaw() { - return imu.getYaw().getRadians(); - } - - /** - * Returns the robot's roll in radians. - */ - public double getRoll() { - return imu.getRoll().getRadians(); - } - - /** - * Zeroes the IMU to a specified yaw. + * Returns the distance from the speaker in meters, adjusted for the robot's movement. */ - public Command zeroIMU(Rotation2d yaw) { - return runOnce(() -> imu.setZero(yaw)).withName("swerve.zero(" + yaw.toString() + ")"); + public double getSpeakerDistance() { + return api.state.pose.getTranslation().getDistance(getSpeakerPosition()); } /** - * Dumps the current state of odometry and resets to 0. + * Gets the angle for the robot to face to score in the speaker, + * in radians. Compensates for note drift caused by spin. */ - public Command dumpOdometry() { - return commandBuilder("swerve.dumpOdometry()").onInitialize(() -> resetOdometry(new Pose2d())).isFinished(true); + private double getSpeakerAngle() { + Translation2d speakerPosition = getSpeakerPosition(); + Translation2d robotPoint = api.state.pose.getTranslation(); + return MathUtil.angleModulus( + speakerPosition.minus(robotPoint).getAngle().getRadians() + Math.PI + spinCompensation.get() + ); } /** - * Drives the robot. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param rot The desired rotational speed from {@code -1.0} to {@code 1.0}. - * @param fieldRelative If the robot should drive field relative. + * Tares the rotation of the robot. Useful for + * fixing an out of sync or drifting IMU. */ - public Command drive(Supplier x, Supplier y, Supplier rot, boolean fieldRelative) { - return commandBuilder("swerve.drive()").onExecute(() -> drive(x.get(), y.get(), rot.get(), fieldRelative)); + public Command tareRotation() { + return commandBuilder("Swerve.tareRotation()") + .onInitialize(() -> api.tareRotation(ForwardPerspective.OPERATOR)) + .isFinished(true); } /** - * Faces the robot towards the speaker. + * Drives the robot using driver input. + * @param x The X value from the driver's joystick. + * @param y The Y value from the driver's joystick. + * @param angular The CCW+ angular speed to apply, from {@code [-1.0, 1.0]}. */ - public Command driveSpeaker() { - return driveSpeaker(() -> 0.0, () -> 0.0); + public Command drive(Supplier x, Supplier y, Supplier angular) { + return commandBuilder("Swerve.drive()").onExecute(() -> + api.applyDriverInput(x.get(), y.get(), angular.get(), ForwardPerspective.OPERATOR, true, true) + ); } /** - * Allows the driver to keep driving, but forces the robot to face the speaker. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}. + * Drives the robot using driver input, while + * forcing the robot to face the speaker. + * @param x The X value from the driver's joystick. + * @param y The Y value from the driver's joystick. */ public Command driveSpeaker(Supplier x, Supplier y) { return commandBuilder("swerve.driveSpeaker()") - .onInitialize(() -> rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond)) + .onInitialize(() -> + rotPID.reset(api.state.pose.getRotation().getRadians(), api.state.speeds.omegaRadiansPerSecond) + ) .onExecute(() -> { double angle = getSpeakerAngle(); - visualizer.updateTarget(angle); - driveAngle(x.get(), y.get(), angle, rotPID, false); - }) - .onEnd(() -> visualizer.removeTarget()); - } - /** - * Drives to the amp. - * If PID is being used, the command will not end. - * If a trajectory is being used, the command ends after the trajectory is completed. - * @param useTrajectory If a trajectory should be used to translate to the amp. Otherwise, PID is used. - */ - public Command driveAmp(boolean useTrajectory) { - return defer(() -> { - double fudgeY = (Alliance.isBlue() ? tunableAmpYFudge : -tunableAmpYFudge); - Pose2d rawTarget = Alliance.isBlue() ? FieldPositions.AMP_SCORE_BLUE : FieldPositions.AMP_SCORE_RED; - Pose2d target = new Pose2d(rawTarget.getX() + tunableAmpXFudge, rawTarget.getY() + fudgeY, rawTarget.getRotation()); - if (useTrajectory) { - Pose2d rawApproach = Alliance.isBlue() ? FieldPositions.AMP_APPROACH_BLUE : FieldPositions.AMP_APPROACH_RED; - Pose2d approach = new Pose2d( - rawApproach.getX() + tunableAmpXFudge, - rawApproach.getY() + fudgeY, - rawApproach.getRotation() - ); - return followTrajectory(generateTrajectory(approach, target)); - } else { - return pidTo(target); + double angularVel = rotPID.calculate(api.state.pose.getRotation().getRadians(), angle); + if (x.get() == 0.0 && y.get() == 0.0 && Math.abs(angularVel) < SwerveConstants.ROT_PID_DEADBAND) { + angularVel = 0.0; } - }) - .withName("swerve.driveAmp(" + useTrajectory + ")"); - } - /** - * Allows the driver to keep driving, but forces the robot to face the amp. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}. - */ - public Command driveAmpManual(Supplier x, Supplier y) { - return commandBuilder("swerve.driveAmpManual()") - .onInitialize(() -> rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond)) - .onExecute(() -> { - double angle = Alliance.isBlue() ? Math2.HALF_PI : -Math2.HALF_PI; - visualizer.updateTarget(angle); - driveAngle(x.get(), y.get(), angle, rotPID, false); - }) - .onEnd(() -> visualizer.removeTarget()); - } - - /** - * Allows the driver to keep driving, but forces the robot to face the feeder. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}. - */ - public Command driveIntakeHuman(Supplier x, Supplier y) { - return commandBuilder("swerve.driveIntakeHuman()") - .onInitialize(() -> rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond)) - .onExecute(() -> { - double angle = Alliance.isBlue() ? -Math2.THIRD_PI : Math2.THIRD_PI; - visualizer.updateTarget(angle); - driveAngle(x.get(), y.get(), angle, rotPID, false); - }) - .onEnd(() -> visualizer.removeTarget()); - } - - /** - * Allows the driver to keep driving, but forces the robot to face the stage. - * @param x The desired {@code x} speed from {@code -1.0} to {@code 1.0}. - * @param y The desired {@code y} speed from {@code -1.0} to {@code 1.0}. - */ - public Command driveClimb(Supplier x, Supplier y) { - return commandBuilder("swerve.driveClimb()") - .onInitialize(() -> rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond)) - .onExecute(() -> { - double stageAngle = FieldPositions.STAGE.minus(getPosition().getTranslation()).getAngle().getRadians(); - double faceStageAngle; - if (stageAngle >= 0.0 && stageAngle <= Math2.TWO_THIRD_PI) { - faceStageAngle = Math2.THIRD_PI; - } else if (stageAngle >= -Math2.TWO_THIRD_PI && stageAngle <= 0.0) { - faceStageAngle = -Math2.THIRD_PI; - } else { - faceStageAngle = -Math.PI; - } - - visualizer.updateTarget(faceStageAngle); - driveAngle(x.get(), y.get(), faceStageAngle, rotPID, false); - }) - .onEnd(() -> visualizer.removeTarget()); + api.applyDriverXY(x.get(), y.get(), angularVel, ForwardPerspective.OPERATOR, true, true); + }); } /** @@ -447,33 +234,21 @@ public Command driveClimb(Supplier x, Supplier y) { */ public Command driveFeed(Supplier x, Supplier y) { return commandBuilder("swerve.driveFeed()") + .onInitialize(() -> + rotPID.reset(api.state.pose.getRotation().getRadians(), api.state.speeds.omegaRadiansPerSecond) + ) .onExecute(() -> { - Translation2d feedPoint = Alliance.isBlue() ? FieldPositions.FEED_BLUE : FieldPositions.FEED_RED; + Translation2d feedPoint = Alliance.isBlue() ? FieldPositions.kBlueFeed : FieldPositions.kRedFeed; double faceFeedAngle = MathUtil.angleModulus( - feedPoint.minus(getPosition().getTranslation()).getAngle().getRadians() + Math.PI + feedPoint.minus(api.state.pose.getTranslation()).getAngle().getRadians() + Math.PI ); - visualizer.updateTarget(faceFeedAngle); - driveAngle(x.get(), y.get(), faceFeedAngle, rotPID, false); - }) - .onEnd(() -> visualizer.removeTarget()); - } - /** - * Moves to a specified pose using PID. Does not end. - * @param pose The pose to translate to. - */ - public Command pidTo(Pose2d pose) { - return commandBuilder("swerve.pidTo(" + pose.toString() + ")") - .onInitialize(() -> { - visualizer.updateTarget(pose); - rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond); - xPID.reset(); - yPID.reset(); - }) - .onExecute(() -> driveToPose(pose, xPID, yPID, rotPID, false)) - .onEnd(() -> { - stop(); - visualizer.removeTarget(); + double angularVel = rotPID.calculate(api.state.pose.getRotation().getRadians(), faceFeedAngle); + if (x.get() == 0.0 && y.get() == 0.0 && Math.abs(angularVel) < SwerveConstants.ROT_PID_DEADBAND) { + angularVel = 0.0; + } + + api.applyDriverXY(x.get(), y.get(), angularVel, ForwardPerspective.OPERATOR, true, true); }); } @@ -495,15 +270,6 @@ public Command followTrajectory(ChoreoTrajectory traj, double targetTimeStart, d return followTrajectory(traj, targetTimeStart, targetTimeEnd, false); } - /** - * Follows a trajectory. - * @param traj The trajectory to follow. - * @param resetOdometry If the odometry should be reset to the first pose in the trajectory. - */ - public Command followTrajectory(ChoreoTrajectory traj, boolean resetOdometry) { - return followTrajectory(traj, -1.0, -1.0, resetOdometry); - } - /** * Follows a trajectory. * @param traj The trajectory to follow. @@ -511,164 +277,49 @@ public Command followTrajectory(ChoreoTrajectory traj, boolean resetOdometry) { * @param targetTimeEnd Time in seconds after the path starts to stop targeting the speaker. Only applied if {@code targetTimeStart} is greater than {@code 0.0}. {@code -1.0} will cause the robot to target the speaker indefinitely. * @param resetOdometry If the odometry should be reset to the first pose in the trajectory. */ - public Command followTrajectory(ChoreoTrajectory traj, double targetTimeStart, double targetTimeEnd, boolean resetOdometry) { - BiConsumer stateBlue = visualizer.addTrajectory(traj.getPoses()); - BiConsumer stateRed = visualizer.addTrajectory(traj.flipped().getPoses()); - - return Choreo - .choreoSwerveCommand( - traj, - this::getPosition, - targetTimeStart, - targetTimeEnd, - this::getSpeakerAngle, - targetPIDTraj, - xPIDTraj, - yPIDTraj, - rotPIDTraj, - speeds -> driveSpeeds(speeds, false, false), - targetPose -> { - if (Alliance.isBlue()) stateBlue.accept(true, targetPose); else stateRed.accept(true, targetPose); - }, - Alliance::isRed, - this - ) - .beforeStarting(() -> { - if (resetOdometry) { - Pose2d initialPose = Alliance.isBlue() ? traj.getInitialPose() : traj.getInitialState().flipped().getPose(); - zeroIMU(initialPose.getRotation()); - resetOdometry(initialPose); - } - - rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond); - xPIDTraj.reset(); - yPIDTraj.reset(); - rotPIDTraj.reset(); - }) - .finallyDo(() -> { - stateBlue.accept(false, Math2.POSE2D_0); - stateRed.accept(false, Math2.POSE2D_0); - }) - .withName("swerve.followTrajectory()"); - } - - /** - * Follows a trajectory. - * @param traj The trajectory to follow. - */ - public Command followTrajectory(Trajectory traj) { - if (traj.getStates().isEmpty()) return none(); - Rotation2d endRotation = traj.getStates().get(traj.getStates().size() - 1).poseMeters.getRotation(); - Timer timer = new Timer(); - - BiConsumer state = visualizer.addTrajectory( - traj.getStates().stream().map(s -> new Pose2d(s.poseMeters.getX(), s.poseMeters.getY(), endRotation)).toArray(Pose2d[]::new) - ); - - return commandBuilder("swerve.followTrajectory()") - .onInitialize(() -> { - rotPIDTrajProfiled.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond); - xPIDTraj.reset(); - yPIDTraj.reset(); - timer.restart(); - }) - .onExecute(() -> { - State goal = traj.sample(timer.get()); - ChassisSpeeds speeds = trajectoryController.calculate(getPosition(), goal, endRotation); - driveSpeeds(speeds, false, false); - state.accept(true, goal.poseMeters); - }) - .isFinished(() -> timer.hasElapsed(traj.getTotalTimeSeconds())) - .onEnd(() -> { - state.accept(false, Math2.POSE2D_0); - timer.stop(); - stop(); - }); - } - - /** - * Runs a SysId quasistatic test. - * @param direction The direction to run the test in. - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysIdRoutine.quasistatic(direction); - } - - /** - * Runs a SysId dynamic test. - * @param direction The direction to run the test in. - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysIdRoutine.dynamic(direction); - } - - public class WheelRadiusCharacterization extends Command { - - private final boolean forward; - private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1.0); - - private double lastGyroYawRads = 0.0; - private double accumGyroYawRads = 0.0; - - private double[] startWheelPositions; - - private double currentEffectiveWheelRadius = 0.0; - - public WheelRadiusCharacterization(boolean forward) { - this.forward = forward; - } - - @Override - public void initialize() { - lastGyroYawRads = getYaw(); - accumGyroYawRads = 0.0; - startWheelPositions = getModuleDistanceRad(); - - omegaLimiter.reset(0); - } - - @Override - public void execute() { - // Run drive at velocity - driveSpeeds(new ChassisSpeeds(0.0, 0.0, omegaLimiter.calculate((forward ? 1.0 : -1.0) * 2.0)), false, false); - - // Get yaw and wheel positions - double yaw = getYaw(); - accumGyroYawRads += MathUtil.angleModulus(yaw - lastGyroYawRads); - lastGyroYawRads = yaw; - double averageWheelPosition = 0.0; - double[] wheelPositions = getModuleDistanceRad(); - System.out.println(wheelPositions[0]); - for (int i = 0; i < 4; i++) { - averageWheelPosition += Math.abs(wheelPositions[i] - startWheelPositions[i]); - } - averageWheelPosition /= 4.0; - - currentEffectiveWheelRadius = (accumGyroYawRads * SwerveConstants.DRIVE_BASE_RADIUS) / averageWheelPosition; - } - - @Override - public void end(boolean interrupted) { - if (Math.abs(accumGyroYawRads) <= Math.PI * 2.0) { - DriverStation.reportWarning("Not enough data for characterization", false); - effectiveWheelRadius = -1.0; - } else { - double effectiveWheelRadiusInches = Units.metersToInches(currentEffectiveWheelRadius) * 2.0; - System.out.println("Effective Wheel Diameter: " + effectiveWheelRadiusInches + " inches"); - effectiveWheelRadius = effectiveWheelRadiusInches; - } - } - } - - public Command funAndGames() { - return new CommandBuilder() - .onInitialize(() -> { - for (SwerveModule module : modules) { - module.configMoveCurrentLimit(50.0); - } - }) - .isFinished(true) - .ignoringDisable(true) - .withName("swerve.funAndGames()"); + public Command followTrajectory( + ChoreoTrajectory traj, + double targetTimeStart, + double targetTimeEnd, + boolean resetOdometry + ) { + return none(); + // return Choreo.choreoSwerveCommand( + // traj, + // this::getPosition, + // targetTimeStart, + // targetTimeEnd, + // this::getSpeakerAngle, + // targetPIDTraj, + // xPIDTraj, + // yPIDTraj, + // rotPIDTraj, + // speeds -> driveSpeeds(speeds, false, false), + // targetPose -> { + // if (Alliance.isBlue()) stateBlue.accept(true, targetPose); + // else stateRed.accept(true, targetPose); + // }, + // Alliance::isRed, + // this + // ) + // .beforeStarting(() -> { + // if (resetOdometry) { + // Pose2d initialPose = Alliance.isBlue() + // ? traj.getInitialPose() + // : traj.getInitialState().flipped().getPose(); + // zeroIMU(initialPose.getRotation()); + // resetOdometry(initialPose); + // } + + // rotPID.reset(getPosition().getRotation().getRadians(), getVelocity(true).omegaRadiansPerSecond); + // xPIDTraj.reset(); + // yPIDTraj.reset(); + // rotPIDTraj.reset(); + // }) + // .finallyDo(() -> { + // stateBlue.accept(false, Math2.POSE2D_0); + // stateRed.accept(false, Math2.POSE2D_0); + // }) + // .withName("swerve.followTrajectory()"); } } diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json deleted file mode 100644 index ff7359e..0000000 --- a/vendordeps/Phoenix5.json +++ /dev/null @@ -1,151 +0,0 @@ -{ - "fileName": "Phoenix5.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.33.1" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.33.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.33.1", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.33.1", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d172..0322385 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.3.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf8..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 0000000..705596d --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.1.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.1.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.1.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 77641e4..0e80a16 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.9", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.9", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.9" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.9" + "version": "v2024.3.1" } ] } \ No newline at end of file