diff --git a/.vscode/launch.json b/.vscode/launch.json index 97a6e2a..6afb5c3 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,7 +1,4 @@ { - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ { diff --git a/.vscode/settings.json b/.vscode/settings.json index dccbc7c..2660a40 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,12 +18,15 @@ { "name": "WPIlibUnitTests", "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" } }, + null ], "java.test.defaultConfig": "WPIlibUnitTests", "java.import.gradle.annotationProcessing.enabled": false, @@ -56,5 +59,7 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + null + ], + "java.debug.settings.onBuildFailureProceed": true } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index d6f5ffd..adfa0ab 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2025", - "teamNumber": 8736 -} \ No newline at end of file + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 8736 +} diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json new file mode 100644 index 0000000..5907df3 --- /dev/null +++ b/AdvantageScope Swerve Calibration.json @@ -0,0 +1,463 @@ +{ + "hubs": [ + { + "x": 139, + "y": 95, + "width": 1100, + "height": 650, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/Drive", + "/RealOutputs", + "/RealOutputs/SwerveStates", + "/RealOutputs/SwerveChassisSpeeds", + "/RealOutputs/Odometry" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h", + "renderer": "#/", + "controlsHeight": 0 + }, + { + "type": 9, + "title": "Drive Overview", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ffff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#00ffff" + } + }, + { + "type": "rotation", + "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "prdx7t2eedan6n46dxrjfu7eisf16f9o", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 3, + "title": "Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot" + } + }, + { + "type": "trajectory", + "logKey": "/RealOutputs/Odometry/Trajectory", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#ff8c00", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "/RealOutputs/Odometry/TrajectorySetpoint", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#ff8c00" + } + } + ], + "game": "2025 Field", + "origin": "blue" + }, + "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 1.4695761589768238e-15, + 5.999999999999999, + -12 + ], + "cameraTarget": [ + 0, + 0.5, + 0 + ] + }, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/TurnPosition/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "ol8lk80m83ma3aegae849d6k1d29sp7d", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive Calibration", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module1/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module2/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/Drive/Module3/DrivePositionRad", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "7jj1f83nsv59dd2ou11ls3ccz5tq1293", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Turn PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "kj6tf6sxm5g04wnjrhed6mx2wpccmqtn", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Drive PID Tuning", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/Measured/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "bctxg9pmwpo31m9bv9ofr92oqqtbpqzp", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Max Speed Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "oy3vmjuhr7g36evikwnmjhfacl30mzn5", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Slip Current Measurement", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveCurrentAmps", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/Drive/Module0/DriveVelocityRadPerSec", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "type": null, + "factor": 1 + }, + "rightUnitConversion": { + "type": null, + "factor": 1 + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "efudylbvebbv1ga2kosknov0uegmtj7h", + "renderer": null, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "4.0.0-beta-3" +} diff --git a/README.md b/README.md index f8684da..2c1f13e 100644 --- a/README.md +++ b/README.md @@ -1,110 +1,110 @@ -# Team 8736 The Mechanisms - Modular Robot Architecture -### A reusable, modular, and extendable code framework for FRC robots. - -This repository contains the foundational code architecture for Team 8736. It is designed to be the **starting point** for each new competition season, allowing us to build reliable, maintainable, and sophisticated robot code faster. - -**Note:** This repository is a **foundational framework**. Do not commit game-specific code directly here. Use this to start your new season's project. - -## 🏛️ Core Philosophy -The goal of this architecture is to solve common problems once, so we can focus on new challenges each year. Our core principles are: - -* **Modularity:** Each robot mechanism (e.g., Drivetrain, Intake, Arm) is an isolated `Subsystem`. -* **Reusability:** Code for common components (e.g., PID controllers, logging, motor wrappers) is built here and reused. -* **Testability:** The architecture is structured to support unit Testing and simulation to verify logic without a physical robot. -* **Clarity:** We enforce a strict code structure and style guide to make code easy to read and understand for new members. - -## ✨ Key Features -This framework provides several key features out-of-the-box: - -* **Advanced Logging:** A built-in logging utility that [e.g., writes to NetworkTables and integrates with AdvantageScope]. -* **Constants Management:** A centralized `Constants.java` structure that separates tuning values from logic, with different sets of constants for the competition robot vs. the offseason robot. -* **Modules:** Common helper functions for pose estimators and PID controllers. - -## 🚀 Getting Started: Using this for a New Season - -Follow these steps to start your new season's codebase from this architecture. - -1. **Create a new, blank repository** on GitHub for your new season. - * Name it `8736-FRC-[Year]-[GameName]` (e.g., `8736-FRC-2024-Crescendo`). - -2. **Clone this architecture repository** to your local machine. - ```bash - git clone https://github.com/Mechanisms-Robotics/modular-robot-reference-architecture.git - ``` - *This will create a folder named `[architecture-repo-name]`.* - -3. **Navigate into the cloned directory.** - ```bash - cd [architecture-repo-name] - ``` - -4. **Rename the remote** from `origin` (which points to this repo) to `template`. - ```bash - git remote rename origin template - ``` - -5. **Link your new season's repository** as the new `origin`. - ```bash - git remote add origin [URL-of-your-new-empty-repo.git] - ``` - -6. **Push the code** to your new season's repository. - ```bash - git push -u origin main - ``` - *(You may need to use `-f` or `--force` for the first push if your new repo is not completely empty)*. - -7. **Build the Project:** - * Open the cloned folder in WPILib VS Code. - * Run the `WPILib: Build Robot Code` command from the command palette (Ctrl+Shift+P). - * Ensure it builds successfully before making any changes. - -## 💻 Team 8736 Coding Standards -All code contributed to this repository (and to season repositories) **must** follow the team's official coding standards. This is critical for maintainability and onboarding new members. - -**[➡️ Find the official Team 8736 Coding Standards here](https://docs.google.com/document/d/1i7vQb6MojpwAiYMbqEBVxQlvooK5sS3bgigXcNyty4E/edit?tab=t.0#heading=h.8b2xyd82witc)** - -## 🔧 How to Add a New Mechanism (e.g., an Intake) - -Here is the standard workflow for adding a new part to the robot using this architecture: - -### 1. Create the Subsystem -* Create a new file: `src/main/java/frc/robot/subsystems/IntakeSubsystem.java` -* Add your hardware components (motors, sensors) as private variables. -* Write public methods for all actions (e.g., `runIntake()`, `stopIntake()`). -* Implement any required methods from the base class (like `periodic()` or `logData()`). - - -### 2. Add Constants -* TODO: fill this in - - -### 3. Create Commands -* Create new files in `src/main/java/frc/robot/commands/` for intake actions (e.g., `RunIntake.java`, `StopIntake.java`). -* In your command, `require` the `IntakeSubsystem` in the constructor. -* Call the subsystem's public methods in the `execute()` and `end()` methods. - -### 4. Register the Subsystem -* In `src/main/a/frc/robot/RobotContainer.java`, create a new **private final** instance of your `IntakeSubsystem`. - ```java - private final IntakeSubsystem m_intake = new IntakeSubsystem(); - ``` - -### 5. Bind Controls -* In the `RobotContainer` constructor, use the `configureButtonBindings()` method to link a controller button to your new commands. - ```java - private void configureButtonBindings() { - // ...other bindings... - m_operatorController.a().onTrue(new RunIntake(m_intake, IntakeConstants.kDefaultIntakeSpeed)); - m_operatorController.a().onFalse(new StopIntake(m_intake)); - } - ``` - -## 🤝 Contributing to this Architecture -This is a foundational repository, so changes must be reviewed carefully. - -* **DO NOT** push directly to the `main` branch. -* Create a new branch for your feature or bugfix (e.g., `feature/new-logging-util` or `fix/swerve-bug`). -* **Follow the [Team 8736 Coding Standards]([https://docs.google.com/document/d/1i7vQb6MojpwAiYMbqEBVxQlvooK5sS3bgigXcNyty4E/edit?tab=t.0#heading=h.8b2xyd82witc])**. -* Write clear comments for all new methods. -* Submit a **Pull Request (PR)** and request a review from a mentor and other programmers. +# Team 8736 The Mechanisms - Modular Robot Architecture +### A reusable, modular, and extendable code framework for FRC robots. + +This repository contains the foundational code architecture for Team 8736. It is designed to be the **starting point** for each new competition season, allowing us to build reliable, maintainable, and sophisticated robot code faster. + +**Note:** This repository is a **foundational framework**. Do not commit game-specific code directly here. Use this to start your new season's project. + +## 🏛️ Core Philosophy +The goal of this architecture is to solve common problems once, so we can focus on new challenges each year. Our core principles are: + +* **Modularity:** Each robot mechanism (e.g., Drivetrain, Intake, Arm) is an isolated `Subsystem`. +* **Reusability:** Code for common components (e.g., PID controllers, logging, motor wrappers) is built here and reused. +* **Testability:** The architecture is structured to support unit Testing and simulation to verify logic without a physical robot. +* **Clarity:** We enforce a strict code structure and style guide to make code easy to read and understand for new members. + +## ✨ Key Features +This framework provides several key features out-of-the-box: + +* **Advanced Logging:** A built-in logging utility that [e.g., writes to NetworkTables and integrates with AdvantageScope]. +* **Constants Management:** A centralized `Constants.java` structure that separates tuning values from logic, with different sets of constants for the competition robot vs. the offseason robot. +* **Modules:** Common helper functions for pose estimators and PID controllers. + +## 🚀 Getting Started: Using this for a New Season + +Follow these steps to start your new season's codebase from this architecture. + +1. **Create a new, blank repository** on GitHub for your new season. + * Name it `8736-FRC-[Year]-[GameName]` (e.g., `8736-FRC-2024-Crescendo`). + +2. **Clone this architecture repository** to your local machine. + ```bash + git clone https://github.com/Mechanisms-Robotics/modular-robot-reference-architecture.git + ``` + *This will create a folder named `[architecture-repo-name]`.* + +3. **Navigate into the cloned directory.** + ```bash + cd [architecture-repo-name] + ``` + +4. **Rename the remote** from `origin` (which points to this repo) to `template`. + ```bash + git remote rename origin template + ``` + +5. **Link your new season's repository** as the new `origin`. + ```bash + git remote add origin [URL-of-your-new-empty-repo.git] + ``` + +6. **Push the code** to your new season's repository. + ```bash + git push -u origin main + ``` + *(You may need to use `-f` or `--force` for the first push if your new repo is not completely empty)*. + +7. **Build the Project:** + * Open the cloned folder in WPILib VS Code. + * Run the `WPILib: Build Robot Code` command from the command palette (Ctrl+Shift+P). + * Ensure it builds successfully before making any changes. + +## 💻 Team 8736 Coding Standards +All code contributed to this repository (and to season repositories) **must** follow the team's official coding standards. This is critical for maintainability and onboarding new members. + +**[➡️ Find the official Team 8736 Coding Standards here](https://docs.google.com/document/d/1i7vQb6MojpwAiYMbqEBVxQlvooK5sS3bgigXcNyty4E/edit?tab=t.0#heading=h.8b2xyd82witc)** + +## 🔧 How to Add a New Mechanism (e.g., an Intake) + +Here is the standard workflow for adding a new part to the robot using this architecture: + +### 1. Create the Subsystem +* Create a new file: `src/main/java/frc/robot/subsystems/IntakeSubsystem.java` +* Add your hardware components (motors, sensors) as private variables. +* Write public methods for all actions (e.g., `runIntake()`, `stopIntake()`). +* Implement any required methods from the base class (like `periodic()` or `logData()`). + + +### 2. Add Constants +* TODO: fill this in + + +### 3. Create Commands +* Create new files in `src/main/java/frc/robot/commands/` for intake actions (e.g., `RunIntake.java`, `StopIntake.java`). +* In your command, `require` the `IntakeSubsystem` in the constructor. +* Call the subsystem's public methods in the `execute()` and `end()` methods. + +### 4. Register the Subsystem +* In `src/main/a/frc/robot/RobotContainer.java`, create a new **private final** instance of your `IntakeSubsystem`. + ```java + private final IntakeSubsystem m_intake = new IntakeSubsystem(); + ``` + +### 5. Bind Controls +* In the `RobotContainer` constructor, use the `configureButtonBindings()` method to link a controller button to your new commands. + ```java + private void configureButtonBindings() { + // ...other bindings... + m_operatorController.a().onTrue(new RunIntake(m_intake, IntakeConstants.kDefaultIntakeSpeed)); + m_operatorController.a().onFalse(new StopIntake(m_intake)); + } + ``` + +## 🤝 Contributing to this Architecture +This is a foundational repository, so changes must be reviewed carefully. + +* **DO NOT** push directly to the `main` branch. +* Create a new branch for your feature or bugfix (e.g., `feature/new-logging-util` or `fix/swerve-bug`). +* **Follow the [Team 8736 Coding Standards]([https://docs.google.com/document/d/1i7vQb6MojpwAiYMbqEBVxQlvooK5sS3bgigXcNyty4E/edit?tab=t.0#heading=h.8b2xyd82witc])**. +* Write clear comments for all new methods. +* Submit a **Pull Request (PR)** and request a review from a mentor and other programmers. diff --git a/build.gradle b/build.gradle index 48a2abe..31d5877 100644 --- a/build.gradle +++ b/build.gradle @@ -1,104 +1,176 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.2" -} - -java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 -} - -def ROBOT_MAIN_CLASS = "frc.robot.Main" - -// Define my targets (RoboRIO) and artifacts (deployable files) -// This is added by GradleRIO's backing project DeployUtils. -deploy { - targets { - roborio(getTargetTypeClass('RoboRIO')) { - // Team number is loaded either from the .wpilib/wpilib_preferences.json - // or from command line. If not found an exception will be thrown. - // You can use getTeamOrDefault(team) instead of getTeamNumber if you - // want to store a team number in this file. - team = project.frc.getTeamNumber() - debug = project.frc.getDebugOrDefault(false) - - artifacts { - // First part is artifact name, 2nd is artifact type - // getTargetTypeClass is a shortcut to get the class type using a string - - frcJava(getArtifactTypeClass('FRCJavaArtifact')) { - } - - // Static files artifact - frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { - files = project.fileTree('src/main/deploy') - directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no - // longer exist in deploy directory of this project - } - } - } - } -} - -def deployArtifact = deploy.targets.roborio.artifacts.frcJava - -// Set to true to use debug for JNI. -wpi.java.debugJni = false - -// Set this to true to enable desktop support. -def includeDesktopSupport = false - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 5. -dependencies { - annotationProcessor wpi.java.deps.wpilibAnnotations() - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() - - roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) - roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) - - roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) - roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) - - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() - - nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() - - 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' -} - -// Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() - -// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') -// in order to make them all available at runtime. Also adding the manifest so WPILib -// knows where to look for our Robot Class. -jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - from sourceSets.main.allSource - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) - duplicatesStrategy = DuplicatesStrategy.INCLUDE -} - -// Configure jar and deploy tasks -deployArtifact.jarTask = jar -wpi.java.configureExecutableTasks(jar) -wpi.java.configureTestTasks(test) - -// Configure string concat to always inline compile -tasks.withType(JavaCompile) { - options.compilerArgs.add '-XDstringConcat=inline' -} +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "com.peterabeles.gversion" version "1.10" + id "com.diffplug.spotless" version "6.12.0" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // The options below may improve performance, but should only be enabled on the RIO 2 + // + // final MAX_JAVA_HEAP_SIZE_MB = 100; + // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + // jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + // jvmArgs.add("-XX:+AlwaysPreTouch") + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + // Change to true to delete files on roboRIO that no + // longer exist in deploy directory on roboRIO + deleteOldFiles = false + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Configuration for AdvantageKit +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +// +// The sim GUI is *disabled* by default to support running +// AdvantageKit log replay from the command line. Set the +// value to "true" to enable the sim GUI by default (this +// is the standard WPILib behavior). +wpi.sim.addGui().defaultEnabled = false +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" + indent = " " +} + +// Create commit with working changes on event branches +task(eventDeploy) { + doLast { + if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { + def branchPrefix = "event" + def branch = 'git branch --show-current'.execute().text.trim() + def commitMessage = "Update at '${new Date().toString()}'" + + if (branch.startsWith(branchPrefix)) { + exec { + workingDir(projectDir) + executable 'git' + args 'add', '-A' + } + exec { + workingDir(projectDir) + executable 'git' + args 'commit', '-m', commitMessage + ignoreExitValue = true + } + + println "Committed to branch: '$branch'" + println "Commit message: '$commitMessage'" + } else { + println "Not on an event branch, skipping commit" + } + } else { + println "Not running deploy task, skipping commit" + } + } +} +createVersionFile.dependsOn(eventDeploy) diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 index ec19934..f5feea6 --- a/gradlew +++ b/gradlew @@ -1,252 +1,252 @@ -#!/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. -# -# SPDX-License-Identifier: Apache-2.0 -# - -############################################################################## -# -# 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/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/. -# -############################################################################## - -# 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 -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 - -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" "$@" +#!/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. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# 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/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/. +# +############################################################################## + +# 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 -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 + +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/settings.gradle b/settings.gradle index 3bc070a..c493958 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,30 +1,30 @@ -import org.gradle.internal.os.OperatingSystem - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2025' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - def homeRoot = new File(publicFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } else { - def userFolder = System.getProperty("user.home") - def homeRoot = new File(userFolder, "wpilib") - frcHome = new File(homeRoot, frcYear) - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name = 'frcHome' - url = frcHomeMaven - } - } -} - -Properties props = System.getProperties(); -props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2025' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name = 'frcHome' + url = frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..6ce412b --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "030000004c050000e60c000011810000" + } + ] +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java new file mode 100644 index 0000000..7f59786 --- /dev/null +++ b/src/main/java/frc/robot/BuildConstants.java @@ -0,0 +1,19 @@ +package frc.robot; + +/** + * Automatically generated file containing build version information. + */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "modular-robot-reference-architecture"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 45; + public static final String GIT_SHA = "0ff9288df56965734e9bcd0eca8a71163d304c49"; + public static final String GIT_DATE = "2025-12-13 11:03:42 EST"; + public static final String GIT_BRANCH = "HEAD"; + public static final String BUILD_DATE = "2025-12-15 07:28:21 EST"; + public static final long BUILD_UNIX_TIME = 1765801701838L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} diff --git a/src/main/java/frc/robot/CONSTANTS.java b/src/main/java/frc/robot/CONSTANTS.java deleted file mode 100644 index f6dc38f..0000000 --- a/src/main/java/frc/robot/CONSTANTS.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.robot; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Translation2d; - -public class CONSTANTS { - //RobotContainer - public static final int CONTROLLER_PORT = 0; - - // Vision Constants - public static AprilTagFieldLayout APRILTAG_FIELD_LAYOUT - = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - //Drivetrain - public static final Translation2d FRONT_LEFT_MODULE_LOCATION - = new Translation2d(0.33, 0.23); - public static final Translation2d FRONT_RIGHT_MODULE_LOCATION - = new Translation2d(0.33, -0.23); - public static final Translation2d BACK_LEFT_MODULE_LOCATION - = new Translation2d(-0.33, 0.23); - public static final Translation2d BACK_RIGHT_MODULE_LOCATION - = new Translation2d(-0.33, -0.23); - - public static final int FRONT_LEFT_STEERING_CAN_ID = 5; - public static final int FRONT_LEFT_DRIVE_CAN_ID = 1; - public static final int FRONT_LEFT_ENCODER_CAN_ID = 1; - - public static final int FRONT_RIGHT_STEERING_CAN_ID = 6; - public static final int FRONT_RIGHT_DRIVE_CAN_ID = 2; - public static final int FRONT_RIGHT_ENCODER_CAN_ID = 2; - - public static final int BACK_LEFT_STEERING_CAN_ID = 4; - public static final int BACK_LEFT_DRIVE_CAN_ID = 8; - public static final int BACK_LEFT_ENCODER_CAN_ID = 4; - - public static final int BACK_RIGHT_STEERING_CAN_ID = 3; - public static final int BACK_RIGHT_DRIVE_CAN_ID = 7; - public static final int BACK_RIGHT_ENCODER_CAN_ID = 3; - - // TODO: This should be set to the robot's actual max speeds and then we should set the controller's - // response curves. Determine these experimentally. - public static final double MAX_SPEED_METERS_PER_SEC = 5.0; - public static final double MAX_ANGULAR_RAD_PER_SEC = 3*Math.PI; - public static final double DEADBAND = 0.08; - - public final static long SWERVE_ENCODER_SET_FREQUECY_SECONDS = 1; - - public static final double STEERING_GEAR_RATIO = 150.0/7.0; // from SDS website - - // TODO: Determine this experimentally and not using math, just change everything - public static final double EFFECTIVE_GEAR_RATIO = 21.0; - - //Localization - public static final int GYRO_CAN_ID = 9; - -} - - diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..dc802c4 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,386 @@ +package frc.robot; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.subsystems.drivetrain.Drive; + +/** + * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running + * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay" + * (log replay from a file). + */ +public final class Constants { + + public static final double ROBOT_LOOP_PERIOD = 0.02; + public static final Mode SIM_MODE = Mode.SIM; + public static final Mode CURRENT_MODE = RobotBase.isReal() + ? Mode.REAL + : SIM_MODE; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY, + } + + public static class DriveConstants { + + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs STEER_GAINS = new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(1.91) + .withKA(0) + .withStaticFeedforwardSign( + StaticFeedforwardSignValue.UseClosedLoopSign + ); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs DRIVE_GAINS = new Slot0Configs() + .withKP(0.1) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = + ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = + ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement DRIVE_MOTOR_TYPE = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement STEER_MOTOR_TYPE = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + private static final SteerFeedbackType STEER_FEEDBACK_TYPE = + SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current SLIP_CURRENT = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = + new TalonFXConfiguration(); + private static final TalonFXConfiguration STEER_INITIAL_CONFIGS = + new TalonFXConfiguration().withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration ENCODER_INITIAL_CONFIGS = + new CANcoderConfiguration(); + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus CAN_BUS = new CANBus( + "canivore", + "./logs/example.hoot" + ); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity SPEED_AT_12_VOLTS = + MetersPerSecond.of(4.69); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double COUPLE_RATIO = 3.8181818181818183; + + private static final double DRIVE_GEAR_RATIO = 7.363636363636365; + private static final double STEER_GEAR_RATIO = 15.42857142857143; + private static final Distance WHEEL_RADIUS = Inches.of(2.167); + + private static final boolean INVERT_LEFT_SIDE = false; + private static final boolean INVERT_RIGHT_SIDE = true; + + // These are only used for simulation + private static final MomentOfInertia STEER_INERTIA = + KilogramSquareMeters.of(0.004); + private static final MomentOfInertia DRIVE_INERTIA = + KilogramSquareMeters.of(0.025); + // Simulated voltage necessary to overcome friction + private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); + private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = + new SwerveDrivetrainConstants().withCANBusName(CAN_BUS.getName()); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > ConstantCreator = new SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + >() + .withDriveMotorGearRatio(DRIVE_GEAR_RATIO) + .withSteerMotorGearRatio(STEER_GEAR_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(STEER_GAINS) + .withDriveMotorGains(DRIVE_GAINS) + .withSteerMotorClosedLoopOutput(STEER_CLOSED_LOOP_OUTPUT) + .withDriveMotorClosedLoopOutput(DRIVE_CLOSED_LOOP_OUTPUT) + .withSlipCurrent(SLIP_CURRENT) + .withSpeedAt12Volts(SPEED_AT_12_VOLTS) + .withDriveMotorType(DRIVE_MOTOR_TYPE) + .withSteerMotorType(STEER_MOTOR_TYPE) + .withFeedbackSource(STEER_FEEDBACK_TYPE) + .withDriveMotorInitialConfigs(DRIVE_INITIAL_CONFIGS) + .withSteerMotorInitialConfigs(STEER_INITIAL_CONFIGS) + .withEncoderInitialConfigs(ENCODER_INITIAL_CONFIGS) + .withSteerInertia(STEER_INERTIA) + .withDriveInertia(DRIVE_INERTIA) + .withSteerFrictionVoltage(STEER_FRICTION_VOLTAGE) + .withDriveFrictionVoltage(DRIVE_FRICTION_VOLTAGE); + + // Front Left + private static final int FRONT_LEFT_DRIVE_MOTOR_ID = 3; + private static final int FRONT_LEFT_STEER_MOTOR_ID = 2; + private static final int FORNT_LEFT_ENCODER_ID = 1; + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of( + 0.15234375 + ); + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + + private static final Distance FRONT_LEFT_X_POS = Inches.of(10); + private static final Distance FRONT_LEFT_Y_POS = Inches.of(10); + + // Front Right + private static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 1; + private static final int FRONT_RIGHT_STEER_MOTOR_ID = 0; + private static final int FRONT_RIGHT_ENOCDER_ID = 0; + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of( + -0.4873046875 + ); + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + + private static final Distance FRONT_RIGHT_X_POS = Inches.of(10); + private static final Distance FRONT_RIGHT_Y_POS = Inches.of(-10); + + // Back Left + private static final int BACK_LEFT_DRIVE_MOTOR_ID = 7; + private static final int BACK_LEFT_STEER_MOTOR_ID = 6; + private static final int BACK_LEFT_ENCODERID = 3; + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of( + -0.219482421875 + ); + private static final boolean BACK_LELFT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + + private static final Distance BACK_LEFT_X_POS = Inches.of(-10); + private static final Distance BACK_LEFT_Y_POS = Inches.of(10); + + // Back Right + private static final int BACK_RIGHT_DRIVE_MOTOR_ID = 5; + private static final int BACK_RIGHT_STEER_MOTOR_ID = 4; + private static final int BACK_RIGHT_ENCODER_ID = 2; + private static final Angle BACK_RIGHT_ENOCDER_OFFSET = Rotations.of( + 0.17236328125 + ); + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + + private static final Distance BACK_RIGHT_X_POS = Inches.of(-10); + private static final Distance BACK_RIGHT_Y_POS = Inches.of(-10); + + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > FRONT_LEFT = ConstantCreator.createModuleConstants( + FRONT_LEFT_STEER_MOTOR_ID, + FRONT_LEFT_DRIVE_MOTOR_ID, + FORNT_LEFT_ENCODER_ID, + FRONT_LEFT_ENCODER_OFFSET, + FRONT_LEFT_X_POS, + FRONT_LEFT_Y_POS, + INVERT_LEFT_SIDE, + FRONT_LEFT_STEER_MOTOR_INVERTED, + FRONT_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > FRONT_RIGHT = ConstantCreator.createModuleConstants( + FRONT_RIGHT_STEER_MOTOR_ID, + FRONT_RIGHT_DRIVE_MOTOR_ID, + FRONT_RIGHT_ENOCDER_ID, + FRONT_RIGHT_ENCODER_OFFSET, + FRONT_RIGHT_X_POS, + FRONT_RIGHT_Y_POS, + INVERT_RIGHT_SIDE, + FRONT_RIGHT_STEER_MOTOR_INVERTED, + FRONT_RIGHT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > BACK_LEFT = ConstantCreator.createModuleConstants( + BACK_LEFT_STEER_MOTOR_ID, + BACK_LEFT_DRIVE_MOTOR_ID, + BACK_LEFT_ENCODERID, + BACK_LEFT_ENCODER_OFFSET, + BACK_LEFT_X_POS, + BACK_LEFT_Y_POS, + INVERT_LEFT_SIDE, + BACK_LELFT_STEER_MOTOR_INVERTED, + BACK_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > BACK_RIGHT = ConstantCreator.createModuleConstants( + BACK_RIGHT_STEER_MOTOR_ID, + BACK_RIGHT_DRIVE_MOTOR_ID, + BACK_RIGHT_ENCODER_ID, + BACK_RIGHT_ENOCDER_OFFSET, + BACK_RIGHT_X_POS, + BACK_RIGHT_Y_POS, + INVERT_RIGHT_SIDE, + BACK_RIGHT_STEER_MOTOR_INVERTED, + BACK_RIGHT_ENCODER_INVERTED + ); + + public static final double ODOMETRY_FREQUENCY = new CANBus( + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ).isNetworkFD() + ? 250.0 + : 100.0; + + public static final double DRIVE_BASE_RADIUS = Math.max( + Math.max( + Math.hypot( + DriveConstants.FRONT_LEFT.LocationX, + DriveConstants.FRONT_LEFT.LocationY + ), + Math.hypot( + DriveConstants.FRONT_RIGHT.LocationX, + DriveConstants.FRONT_RIGHT.LocationY + ) + ), + Math.max( + Math.hypot( + DriveConstants.BACK_LEFT.LocationX, + DriveConstants.BACK_LEFT.LocationY + ), + Math.hypot( + DriveConstants.BACK_RIGHT.LocationX, + DriveConstants.BACK_RIGHT.LocationY + ) + ) + ); + + // PathPlanner config constants + public static final double ROBOT_MASS_KG = 74.088; + public static final double ROBOT_MOI = 6.883; + public static final double WHEEL_COF = 1.2; + public static final RobotConfig PLANER_CONFIG = new RobotConfig( + ROBOT_MASS_KG, + ROBOT_MOI, + new ModuleConfig( + DriveConstants.FRONT_LEFT.WheelRadius, + DriveConstants.SPEED_AT_12_VOLTS.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1).withReduction( + DriveConstants.FRONT_LEFT.DriveMotorGearRatio + ), + DriveConstants.FRONT_LEFT.SlipCurrent, + 1 + ), + Drive.getModuleTranslations() + ); + + public static final double DEADBAND = 0.1; + public static final double ANGLE_KP = 5.0; + public static final double ANGLE_KD = 0.4; + public static final double ANGLE_MAX_VELOCITY = 8.0; + public static final double ANGLE_MAX_ACCELERATION = 20.0; + public static final double FF_START_DELAY = 2.0; // Secs + public static final double FF_RAMP_RATE = 0.1; // Volts/Sec + public static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec + public static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + + public static final double PATH_PLANNER_KP = 5.0; + public static final double PATH_PLANNER_KI = 0.0; + public static final double PATH_PLANNER_KD = 0.0; + + public static final double MM_CRUISE_VELOCITY = + 100.0 / STEER_GEAR_RATIO; + public static final double MM_ACCELERATION = MM_CRUISE_VELOCITY / 0.100; + public static final double MM_EXPO_KV = 0.12 * STEER_GEAR_RATIO; + public static final double MM_EXPO_KA = 0.1; + + public static final int GRYO_CAN_ID = 30; + + public static final double DRIVE_CAN_FRAME_FREQUENCY = 50.0; + public static final double GRYO_CAN_FRAME_FREQUENCY = 0.01; + } + + public static class Timeouts { + + public static final double STD_DEBOUNCE_TIME = 0.5; // Secs + public static final int STD_RETRY_ATTEMPTS = 5; + public static final double STD_TIMEOUT = 0.1; // Secs + public static final double STD_TIMEOUT_LONG = 0.25; // Secs + public static final double SYSID_TIMEOUT = 1.0; // Secs + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 4f0d9ac..8f90eb8 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -1,15 +1,22 @@ -// 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 frc.robot; - -import edu.wpi.first.wpilibj.RobotBase; - -public final class Main { - private Main() {} - - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } -} +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/PoseEstimator8736.java b/src/main/java/frc/robot/PoseEstimator8736.java deleted file mode 100644 index ea1ad50..0000000 --- a/src/main/java/frc/robot/PoseEstimator8736.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot; - -import com.reduxrobotics.sensors.canandgyro.Canandgyro; -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.kinematics.SwerveModulePosition; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.drivetrain.Drivetrain; - -/* - * TEST PLAN: - * - * Determine where to initialize the pose estimator. - * Test output of getModulePositions to make sure they are correct. - */ - -public class PoseEstimator8736 { - private static final int GYRO_CAN_ID = 9; - private final Canandgyro gyro = new Canandgyro(GYRO_CAN_ID); - - private SwerveDrivePoseEstimator swervePoseEstimator; - private Drivetrain drivetrain; - - // TODO: Where do we call this from and do we need to make sure drivetrain encoders are set first? - // Can we just call this whenever we call setModulesToEncoders? Should they be called together? - public void initialize(Pose2d pose, Drivetrain drivetrain) { - double yawInRotations = pose.getRotation().getDegrees() / 360.0; // between 0.0 and 1.0 - this.gyro.setYaw(yawInRotations); - this.drivetrain = drivetrain; - this.swervePoseEstimator = new SwerveDrivePoseEstimator( - drivetrain.getKinematics(), - pose.getRotation(), // initial gyro angle TODO: Should I use the actual gyro angle instead? - drivetrain.getModulePositions(), - pose); - } - - public void zeroGyro() { - // also resets the pose estimator, if it has been previously initialized - - Pose2d startingPose = null; - if (this.swervePoseEstimator != null) { - startingPose = getPose(); - startingPose = new Pose2d( - startingPose.getX(), - startingPose.getY(), - new Rotation2d(0.0)); - } - - this.gyro.setYaw(0.0); - - if (startingPose != null && this.drivetrain != null) { - initialize(startingPose, this.drivetrain); - } - } - - public Pose2d getPose() { - return this.swervePoseEstimator.getEstimatedPosition(); - } - - public double getGyroYaw() { - return this.gyro.getYaw(); // returns yaw in rotations - } - - public void addVisionMeasurement(Pose2d pose, double timestampSeconds) { - this.swervePoseEstimator.addVisionMeasurement(pose, timestampSeconds); - } - - public void addOdometryMeasurement(SwerveModulePosition[] positions) { - this.swervePoseEstimator.update(this.gyro.getRotation2d(), positions); - } -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3e26c13..07e8aa8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,88 +1,147 @@ -// 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 frc.robot; - -import java.time.Duration; -import java.time.Instant; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import static frc.robot.CONSTANTS.*; - -public class Robot extends TimedRobot { - public Instant lastSwerveModuleSetTime = Instant.MIN; - - private Command autonomousCommand; - private final RobotContainer robotContainer; - - public Robot() { - this.robotContainer = new RobotContainer(); - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - } - - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() { - // set the swerve modules to the external encoders periodically - Instant now = Instant.now(); - Duration duration = Duration.between(this.lastSwerveModuleSetTime, now); - if (duration.compareTo(Duration.ofSeconds(SWERVE_ENCODER_SET_FREQUECY_SECONDS)) >= 0) { - this.robotContainer.setSwerveModulesToEncoders(); - this.lastSwerveModuleSetTime = now; - } - } - - @Override - public void disabledExit() { - // Zero the gyro when we enable. We will probably have to start setting this - // differently at the start of autos. This clearly will have to change. - this.robotContainer.zeroGyro(); - } - - @Override - public void autonomousInit() { - this.autonomousCommand = this.robotContainer.getAutonomousCommand(); - if (this.autonomousCommand != null) { - this.autonomousCommand.schedule(); - } - } - - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - - @Override - public void teleopInit() { - if (this.autonomousCommand != null) { - this.autonomousCommand.cancel(); - } - } - - @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} -} +package frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends LoggedRobot { + + private Command autonomousCommand; + private RobotContainer robotContainer; + + public Robot() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (Constants.CURRENT_MODE) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver( + new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")) + ); + break; + } + + // Start AdvantageKit logger + Logger.start(); + + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + robotContainer = new RobotContainer(); + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { + // Optionally switch the thread to high priority to improve loop + // timing (see the template project documentation for details) + // Threads.setCurrentThreadPriority(true, 99); + + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. + CommandScheduler.getInstance().run(); + + // Return to non-RT thread priority (do not modify the first argument) + // Threads.setCurrentThreadPriority(false, 10); + } + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() {} + + /** This function is called periodically when disabled. */ + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + autonomousCommand = robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (autonomousCommand != null) { + autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + /** This function is called once when teleop is enabled. */ + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() {} + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8811fb9..c94aa0b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,80 +1,168 @@ -// 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 frc.robot; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +import com.pathplanner.lib.auto.AutoBuilder; + import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.drivetrain.DrivetrainController; -import static frc.robot.CONSTANTS.*; - +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.DriveConstants; +import frc.robot.controllers.DriveController; +import frc.robot.subsystems.drivetrain.Drive; +import frc.robot.subsystems.drivetrain.GyroIO; +import frc.robot.subsystems.drivetrain.GyroIORedux; +import frc.robot.subsystems.drivetrain.ModuleIO; +import frc.robot.subsystems.drivetrain.ModuleIOSim; +import frc.robot.subsystems.drivetrain.ModuleIOTalonFX; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and button mappings) should be declared here. + */ public class RobotContainer { - private final Drivetrain drivetrain = new Drivetrain(); - private final PoseEstimator8736 poseEstimator = new PoseEstimator8736(); - private final DrivetrainController drivetrainController = new DrivetrainController(poseEstimator); - - private final CommandPS4Controller controller = new CommandPS4Controller(CONTROLLER_PORT); - - public RobotContainer() { - // TODO: Think about where to initialize all of this properly - this.poseEstimator.initialize(new Pose2d(), this.drivetrain); - this.drivetrain.setPoseEstimator(this.poseEstimator); - configureBindings(); - } - - public void setSwerveModulesToEncoders() { - this.drivetrain.setModulesToEncoders(); - } - - public void zeroGyro() { - this.poseEstimator.zeroGyro(); - } - - private void configureBindings() { - controller.cross().onTrue(new InstantCommand( - () -> { - this.poseEstimator.zeroGyro(); - } - )); - - drivetrain.setDefaultCommand( - new RunCommand( - () -> { - double forward = -this.controller.getLeftY(); // Negative to match FRC convention - double strafe = -this.controller.getLeftX(); - double rotation = -this.controller.getRightX(); - - // apply deadbands and scaling - - forward = Math.abs(forward) > DEADBAND ? forward : 0.0; - strafe = Math.abs(strafe) > DEADBAND ? strafe : 0.0; - rotation = Math.abs(rotation) > DEADBAND ? rotation : 0.0; - - ChassisSpeeds speeds = new ChassisSpeeds( - forward*forward*forward*MAX_SPEED_METERS_PER_SEC, - strafe*strafe*strafe* MAX_SPEED_METERS_PER_SEC, - rotation*rotation*rotation*MAX_ANGULAR_RAD_PER_SEC - ); - - // convert to robot-oriented coordinates and pass to swerve subsystem - ChassisSpeeds robotOriented = drivetrainController.fieldToRobotChassisSpeeds(speeds); - drivetrain.setDesiredState(robotOriented); - }, - drivetrain - ) - ); + // Subsystems + private final Drive drive; + + // Controller + private final CommandPS5Controller controller = new CommandPS5Controller(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + switch (Constants.CURRENT_MODE) { + case REAL: + // Real robot, instantiate hardware IO implementations + // ModuleIOTalonFX is intended for modules with TalonFX drive, TalonFX turn, and a CANcoder + this.drive = new Drive( + new GyroIORedux(), + new ModuleIOTalonFX(DriveConstants.FRONT_LEFT), + new ModuleIOTalonFX(DriveConstants.FRONT_RIGHT), + new ModuleIOTalonFX(DriveConstants.BACK_LEFT), + new ModuleIOTalonFX(DriveConstants.BACK_RIGHT) + ); + break; + case SIM: + // Sim robot, instantiate physics sim IO implementations + this.drive = new Drive( + new GyroIO() {}, // Gryo is simulated via kinematics + new ModuleIOSim(DriveConstants.FRONT_LEFT), + new ModuleIOSim(DriveConstants.FRONT_RIGHT), + new ModuleIOSim(DriveConstants.BACK_LEFT), + new ModuleIOSim(DriveConstants.BACK_RIGHT) + ); + break; + default: + // Replayed robot, disable IO implementations + this.drive = new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {} + ); + break; + } + + // Set up auto routines + this.autoChooser = new LoggedDashboardChooser<>( + "Auto Choices", + AutoBuilder.buildAutoChooser() + ); + + // Set up SysId routines + this.autoChooser.addOption( + "Drive Wheel Radius Characterization", + DriveController.wheelRadiusCharacterization(this.drive) + ); + this.autoChooser.addOption( + "Drive Simple FF Characterization", + DriveController.feedforwardCharacterization(this.drive) + ); + this.autoChooser.addOption( + "Drive SysId (Quasistatic Forward)", + this.drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + ); + this.autoChooser.addOption( + "Drive SysId (Quasistatic Reverse)", + this.drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + ); + this.autoChooser.addOption( + "Drive SysId (Dynamic Forward)", + this.drive.sysIdDynamic(SysIdRoutine.Direction.kForward) + ); + this.autoChooser.addOption( + "Drive SysId (Dynamic Reverse)", + this.drive.sysIdDynamic(SysIdRoutine.Direction.kReverse) + ); + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Default command, normal field-relative drive + this.drive.setDefaultCommand( + DriveController.joystickDrive( + this.drive, + () -> this.controller.getLeftY(), + () -> this.controller.getLeftX(), + () -> -this.controller.getRawAxis(3) // For some reason the axis mapping is wrong + ) + ); + + // Lock to 0 degrees when A button is held + this.controller.circle().whileTrue( + DriveController.joystickDriveAtAngle( + this.drive, + () -> this.controller.getLeftY(), + () -> this.controller.getLeftX(), + () -> Rotation2d.kZero + ) + ); + + // Switch to X pattern when X button is pressed + this.controller.square().onTrue( + Commands.runOnce(this.drive::stopWithX, this.drive) + ); + + // Reset gyro to 0 degrees when B button is pressed + this.controller.cross().onTrue( + Commands.runOnce( + () -> + this.drive.setPose( + new Pose2d( + this.drive.getPose().getTranslation(), + Rotation2d.kZero + ) + ), + this.drive + ).ignoringDisable(true) + ); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return this.autoChooser.get(); + } } - - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } -} - diff --git a/src/main/java/frc/robot/controllers/DriveController.java b/src/main/java/frc/robot/controllers/DriveController.java new file mode 100644 index 0000000..be7d009 --- /dev/null +++ b/src/main/java/frc/robot/controllers/DriveController.java @@ -0,0 +1,328 @@ +package frc.robot.controllers; + +import edu.wpi.first.math.MathUtil; +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.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drivetrain.Drive; +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class DriveController { + + private DriveController() {} + + private static Translation2d getLinearVelocityFromJoysticks( + double x, + double y + ) { + // Apply deadband + double linearMagnitude = MathUtil.applyDeadband( + Math.hypot(x, y), + DriveConstants.DEADBAND + ); + Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x)); + + // Square magnitude for more precise control + linearMagnitude = linearMagnitude * linearMagnitude; + + // Return new linear velocity + return new Pose2d(Translation2d.kZero, linearDirection) + .transformBy( + new Transform2d(linearMagnitude, 0.0, Rotation2d.kZero) + ) + .getTranslation(); + } + + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier + ) { + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = getLinearVelocityFromJoysticks( + xSupplier.getAsDouble(), + ySupplier.getAsDouble() + ); + + // Apply rotation deadband + double omega = MathUtil.applyDeadband( + omegaSupplier.getAsDouble(), + DriveConstants.DEADBAND + ); + + // Square rotation value for more precise control + omega = Math.copySign(omega * omega, omega); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec(), + omega * drive.getMaxAngularSpeedRadPerSec() + ); + boolean isFlipped = + DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation() + ) + ); + }, + drive + ); + } + + /** + * Field relative drive command using joystick for linear control and PID for angular control. + * Possible use cases include snapping to an angle, aiming at a vision target, or controlling + * absolute rotation with a joystick. + */ + public static Command joystickDriveAtAngle( + Drive drive, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + Supplier rotationSupplier + ) { + // Create PID controller + ProfiledPIDController angleController = new ProfiledPIDController( + DriveConstants.ANGLE_KP, + 0.0, + DriveConstants.ANGLE_KD, + new TrapezoidProfile.Constraints( + DriveConstants.ANGLE_MAX_VELOCITY, + DriveConstants.ANGLE_MAX_ACCELERATION + ) + ); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // Construct command + return Commands.run( + () -> { + // Get linear velocity + Translation2d linearVelocity = getLinearVelocityFromJoysticks( + xSupplier.getAsDouble(), + ySupplier.getAsDouble() + ); + + // Calculate angular speed + double omega = angleController.calculate( + drive.getRotation().getRadians(), + rotationSupplier.get().getRadians() + ); + + // Convert to field relative speeds & send command + ChassisSpeeds speeds = new ChassisSpeeds( + linearVelocity.getX() * + drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * + drive.getMaxLinearSpeedMetersPerSec(), + omega + ); + boolean isFlipped = + DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, + isFlipped + ? drive.getRotation().plus(new Rotation2d(Math.PI)) + : drive.getRotation() + ) + ); + }, + drive + ) + // Reset PID controller when command starts + .beforeStarting(() -> + angleController.reset(drive.getRotation().getRadians()) + ); + } + + /** + * Measures the velocity feedforward constants for the drive motors. + * + *

This command should only be used in voltage control mode. + */ + public static Command feedforwardCharacterization(Drive drive) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce(() -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + // Allow modules to orient + Commands.run( + () -> { + drive.runCharacterization(0.0); + }, + drive + ).withTimeout(DriveConstants.FF_START_DELAY), + // Start timer + Commands.runOnce(timer::restart), + // Accelerate and gather data + Commands.run( + () -> { + double voltage = timer.get() * DriveConstants.FF_RAMP_RATE; + drive.runCharacterization(voltage); + velocitySamples.add(drive.getFFCharacterizationVelocity()); + voltageSamples.add(voltage); + }, + drive + ) + // When cancelled, calculate and print results + .finallyDo(() -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += + velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = + (sumY * sumX2 - sumX * sumXY) / + (n * sumX2 - sumX * sumX); + double kV = + (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println( + "********** Drive FF Characterization Results **********" + ); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + }) + ); + } + + /** Measures the robot's wheel radius by spinning in a circle. */ + public static Command wheelRadiusCharacterization(Drive drive) { + SlewRateLimiter limiter = new SlewRateLimiter( + DriveConstants.WHEEL_RADIUS_RAMP_RATE + ); + WheelRadiusCharacterizationState state = + new WheelRadiusCharacterizationState(); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce(() -> { + limiter.reset(0.0); + }), + // Turn in place, accelerating up to full speed + Commands.run( + () -> { + double speed = limiter.calculate( + DriveConstants.WHEEL_RADIUS_MAX_VELOCITY + ); + drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); + }, + drive + ) + ), + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + // Record starting measurement + Commands.runOnce(() -> { + state.positions = + drive.getWheelRadiusCharacterizationPositions(); + state.lastAngle = drive.getRotation(); + state.gyroDelta = 0.0; + }), + // Update gyro delta + Commands.run(() -> { + var rotation = drive.getRotation(); + state.gyroDelta += Math.abs( + rotation.minus(state.lastAngle).getRadians() + ); + state.lastAngle = rotation; + }) + // When cancelled, calculate and print results + .finallyDo(() -> { + double[] positions = + drive.getWheelRadiusCharacterizationPositions(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += + Math.abs(positions[i] - state.positions[i]) / + 4.0; + } + double wheelRadius = + (state.gyroDelta * + DriveConstants.DRIVE_BASE_RADIUS) / + wheelDelta; + + NumberFormat formatter = new DecimalFormat("#0.000"); + System.out.println( + "********** Wheel Radius Characterization Results **********" + ); + System.out.println( + "\tWheel Delta: " + + formatter.format(wheelDelta) + + " radians" + ); + System.out.println( + "\tGyro Delta: " + + formatter.format(state.gyroDelta) + + " radians" + ); + System.out.println( + "\tWheel Radius: " + + formatter.format(wheelRadius) + + " meters, " + + formatter.format( + Units.metersToInches(wheelRadius) + ) + + " inches" + ); + }) + ) + ); + } + + private static class WheelRadiusCharacterizationState { + + double[] positions = new double[4]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/PoseCamera.java b/src/main/java/frc/robot/subsystems/PoseCamera.java deleted file mode 100644 index 5eb0c7f..0000000 --- a/src/main/java/frc/robot/subsystems/PoseCamera.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.robot.subsystems; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.PhotonUtils; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.CONSTANTS; -import frc.robot.PoseEstimator8736; - -public class PoseCamera extends SubsystemBase { - private final PhotonCamera camera; - private final String cameraName; - private final Transform3d cameraToRobot; - - private PoseEstimator8736 poseEstimator; - private PhotonPoseEstimator photonEstimator; - - public PoseCamera(String cameraName, Transform3d cameraToRobot, PoseEstimator8736 poseEstimator) { - this.camera = new PhotonCamera(cameraName); - this.cameraName = cameraName; - this.cameraToRobot = cameraToRobot; - - this.poseEstimator = poseEstimator; - - this.photonEstimator = new PhotonPoseEstimator( - CONSTANTS.APRILTAG_FIELD_LAYOUT, - PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - this.cameraToRobot); - - this.photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - } - - @Override - public void periodic() { - List results = this.camera.getAllUnreadResults(); - Optional visionEstimate = Optional.empty(); - - for (PhotonPipelineResult result : results) { - - visionEstimate = this.photonEstimator.update(result); - if (visionEstimate.isPresent()) { - Pose3d poseEstimate = visionEstimate.get().estimatedPose; - - SmartDashboard.putNumber(cameraName + "/pose/x", poseEstimate.getX()); - SmartDashboard.putNumber(cameraName + "/pose/y", poseEstimate.getY()); - SmartDashboard.putNumber(cameraName + "/pose/heading", poseEstimate.getRotation().getAngle()); - SmartDashboard.putNumber(cameraName + "/timestamp", visionEstimate.get().timestampSeconds); - - this.poseEstimator.addVisionMeasurement(poseEstimate.toPose2d(), - visionEstimate.get().timestampSeconds); - } - } - } -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drive.java b/src/main/java/frc/robot/subsystems/drivetrain/Drive.java new file mode 100644 index 0000000..2abfa5a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drive.java @@ -0,0 +1,359 @@ +package frc.robot.subsystems.drivetrain; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.Matrix; +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.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Mode; +import frc.robot.Constants.Timeouts; +import frc.robot.util.LocalADStarAK; +import frc.robot.util.PoseEstimator; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Drive extends SubsystemBase { + + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = + new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = new Alert( + "Disconnected gyro, using kinematics as fallback.", + AlertType.kError + ); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics( + getModuleTranslations() + ); + private PoseEstimator poseEstimator = new PoseEstimator( + kinematics, + Rotation2d.kZero, + Pose2d.kZero + ); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO + ) { + this.gyroIO = gyroIO; + this.modules[0] = new Module(flModuleIO, 0, DriveConstants.FRONT_LEFT); + this.modules[1] = new Module(frModuleIO, 1, DriveConstants.FRONT_RIGHT); + this.modules[2] = new Module(blModuleIO, 2, DriveConstants.BACK_LEFT); + this.modules[3] = new Module(brModuleIO, 3, DriveConstants.BACK_RIGHT); + + // Start odometry thread + PhoenixOdometryThread.getInstance().start(); + + // Configure AutoBuilder for PathPlanner + AutoBuilder.configure( + this::getPose, + this::setPose, + this::getChassisSpeeds, + this::runVelocity, + new PPHolonomicDriveController( + new PIDConstants( + DriveConstants.PATH_PLANNER_KP, + DriveConstants.PATH_PLANNER_KI, + DriveConstants.PATH_PLANNER_KD + ), + new PIDConstants( + DriveConstants.PATH_PLANNER_KP, + DriveConstants.PATH_PLANNER_KI, + DriveConstants.PATH_PLANNER_KD + ) + ), + DriveConstants.PLANER_CONFIG, + () -> + DriverStation.getAlliance().orElse(Alliance.Blue) == + Alliance.Red, + this + ); + Pathfinding.setPathfinder(new LocalADStarAK()); + PathPlannerLogging.setLogActivePathCallback(activePath -> { + Logger.recordOutput( + "Odometry/Trajectory", + activePath.toArray(new Pose2d[activePath.size()]) + ); + }); + PathPlannerLogging.setLogTargetPoseCallback(targetPose -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + // Configure SysId + this.sysId = new SysIdRoutine( + new SysIdRoutine.Config(null, null, null, state -> + Logger.recordOutput("Drive/SysIdState", state.toString()) + ), + new SysIdRoutine.Mechanism( + voltage -> runCharacterization(voltage.in(Volts)), + null, + this + ) + ); + } + + @Override + public void periodic() { + odometryLock.lock(); // Prevents odometry updates while reading data + this.gyroIO.updateInputs(this.gyroInputs); + Logger.processInputs("Drive/Gyro", this.gyroInputs); + for (var module : this.modules) { + module.periodic(); + } + odometryLock.unlock(); + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : this.modules) { + module.stop(); + } + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput( + "SwerveStates/Setpoints", + new SwerveModuleState[] {} + ); + Logger.recordOutput( + "SwerveStates/SetpointsOptimized", + new SwerveModuleState[] {} + ); + } + + // Update odometry + double[] sampleTimestamps = this.modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions from each module + SwerveModulePosition[] modulePositions = + new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = + this.modules[moduleIndex].getOdometryPositions()[i]; + } + + // Update pose estimator with gyro rotation (or null to use kinematics) + Rotation2d gyroRotation = this.gyroInputs.connected + ? this.gyroInputs.odometryYawPositions[i] + : null; + this.poseEstimator.updateOdometry( + modulePositions, + gyroRotation, + sampleTimestamps[i] + ); + } + + // Update gyro alert + this.gyroDisconnectedAlert.set( + !this.gyroInputs.connected && Constants.CURRENT_MODE != Mode.SIM + ); + } + + /** + * Runs the drive at the desired velocity. + * + * @param speeds Speeds in meters/sec + */ + public void runVelocity(ChassisSpeeds speeds) { + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize( + speeds, + Constants.ROBOT_LOOP_PERIOD + ); + SwerveModuleState[] setpointStates = + this.kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, + DriveConstants.SPEED_AT_12_VOLTS + ); + + // Log unoptimized setpoints and setpoint speeds + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + + // Send setpoints to modules + for (int i = 0; i < 4; i++) { + this.modules[i].runSetpoint(setpointStates[i]); + } + + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + } + + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + this.modules[i].runCharacterization(output); + } + } + + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = getModuleTranslations()[i].getAngle(); + } + this.kinematics.resetHeadings(headings); + stop(); + } + + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(Timeouts.SYSID_TIMEOUT) + .andThen(this.sysId.quasistatic(direction)); + } + + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(Timeouts.SYSID_TIMEOUT) + .andThen(this.sysId.dynamic(direction)); + } + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = this.modules[i].getState(); + } + return states; + } + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = this.modules[i].getPosition(); + } + return states; + } + + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + private ChassisSpeeds getChassisSpeeds() { + return this.kinematics.toChassisSpeeds(getModuleStates()); + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = + this.modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += this.modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return this.poseEstimator.getEstimatedPose(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + this.poseEstimator.resetPose(pose, getModulePositions()); + } + + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + this.poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, + timestampSeconds, + visionMeasurementStdDevs + ); + } + + /** Returns the maximum linear speed in meters per sec. */ + public double getMaxLinearSpeedMetersPerSec() { + return DriveConstants.SPEED_AT_12_VOLTS.in(MetersPerSecond); + } + + /** Returns the maximum angular speed in radians per sec. */ + public double getMaxAngularSpeedRadPerSec() { + return ( + getMaxLinearSpeedMetersPerSec() / DriveConstants.DRIVE_BASE_RADIUS + ); + } + + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d( + DriveConstants.FRONT_LEFT.LocationX, + DriveConstants.FRONT_LEFT.LocationY + ), + new Translation2d( + DriveConstants.FRONT_RIGHT.LocationX, + DriveConstants.FRONT_RIGHT.LocationY + ), + new Translation2d( + DriveConstants.BACK_LEFT.LocationX, + DriveConstants.BACK_LEFT.LocationY + ), + new Translation2d( + DriveConstants.BACK_RIGHT.LocationX, + DriveConstants.BACK_RIGHT.LocationY + ), + }; + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java deleted file mode 100644 index 4918bd9..0000000 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ /dev/null @@ -1,149 +0,0 @@ -package frc.robot.subsystems.drivetrain; - -import edu.wpi.first.math.geometry.Pose2d; -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.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.PoseEstimator8736; -import static frc.robot.CONSTANTS.*; - -public class Drivetrain extends SubsystemBase { - // Per WPILib documentation +X is forward and +Y is left (oriented to the robot) - // Positive rotation is counterclockwise - - - - SwerveDriveKinematics kinematics; - ChassisSpeeds desiredChassisSpeeds; - PoseEstimator8736 poseEstimator; - //private final StructArrayPublisher publisher; - - private final SwerveModule frontLeftModule = new SwerveModule( - FRONT_LEFT_STEERING_CAN_ID, FRONT_LEFT_DRIVE_CAN_ID, FRONT_LEFT_ENCODER_CAN_ID); - - private final SwerveModule frontRightModule = new SwerveModule( - FRONT_RIGHT_STEERING_CAN_ID, FRONT_RIGHT_DRIVE_CAN_ID, FRONT_RIGHT_ENCODER_CAN_ID); - - private final SwerveModule backLeftModule = new SwerveModule( - BACK_LEFT_STEERING_CAN_ID, BACK_LEFT_DRIVE_CAN_ID, BACK_LEFT_ENCODER_CAN_ID); - - private final SwerveModule backRightModule = new SwerveModule( - BACK_RIGHT_STEERING_CAN_ID, BACK_RIGHT_DRIVE_CAN_ID, BACK_RIGHT_ENCODER_CAN_ID); - - /** - * Remember that the front of the robot is +X and the left side of the robot is - * +Y. - */ - public Drivetrain() { - this.kinematics = new SwerveDriveKinematics( - FRONT_LEFT_MODULE_LOCATION, FRONT_RIGHT_MODULE_LOCATION, - BACK_LEFT_MODULE_LOCATION, BACK_RIGHT_MODULE_LOCATION); - - this.desiredChassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); - - // this.publisher = NetworkTableInstance.getDefault().getStructArrayTopic( - // "/SwerveStates", SwerveModuleState.struct).publish(); - } - - public void setDesiredState(ChassisSpeeds desiredChassisSpeeds) { - this.desiredChassisSpeeds = desiredChassisSpeeds; - } - - public ChassisSpeeds getDesiredState() { - return this.desiredChassisSpeeds; - } - - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } - - public SwerveModulePosition[] getModulePositions() { - return new SwerveModulePosition[] { - this.frontLeftModule.getModulePosition(), - this.frontRightModule.getModulePosition(), - this.backLeftModule.getModulePosition(), - this.backRightModule.getModulePosition() - }; - } - - public void setModulesToEncoders() { - this.frontLeftModule.setModuleToEncoder(); - this.frontRightModule.setModuleToEncoder(); - this.backLeftModule.setModuleToEncoder(); - this.backRightModule.setModuleToEncoder(); - } - - public void setPoseEstimator(PoseEstimator8736 poseEstimator) { - this.poseEstimator = poseEstimator; - } - - @Override - public void periodic() { - // update the pose estimator - - this.poseEstimator.addOdometryMeasurement(this.getModulePositions()); - - Pose2d pose = this.poseEstimator.getPose(); - SmartDashboard.putNumber("Pose Estimator/X", pose.getX()); - SmartDashboard.putNumber("Pose Estimator/Y", pose.getY()); - SmartDashboard.putNumber("Pose Estimator/Rotation Degrees", pose.getRotation().getDegrees()); - - // send the new desired states down to the modules - - SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates( - this.desiredChassisSpeeds); - - SwerveModuleState frontLeftState = moduleStates[0]; - SwerveModuleState frontRightState = moduleStates[1]; - SwerveModuleState backLeftState = moduleStates[2]; - SwerveModuleState backRightState = moduleStates[3]; - - // publisher.set(new SwerveModuleState[] { - // frontLeftState, - // frontRightState, - // backLeftState, - // backRightState - // }); - - this.frontLeftModule.setModuleState(frontLeftState); - this.frontRightModule.setModuleState(frontRightState); - this.backLeftModule.setModuleState(backLeftState); - this.backRightModule.setModuleState(backRightState); - } - - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a - * digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - this.periodic(); - } -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java deleted file mode 100644 index 377f76d..0000000 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainController.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.subsystems.drivetrain; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.PoseEstimator8736; - -public class DrivetrainController { - private final PoseEstimator8736 poseEstimator; - - public DrivetrainController(PoseEstimator8736 poseEstimator) { - this.poseEstimator = poseEstimator; - } - - public ChassisSpeeds fieldToRobotChassisSpeeds(ChassisSpeeds fieldOriented) - { - double angle = 2*Math.PI*poseEstimator.getGyroYaw(); - - double cosA = Math.cos(angle); - double sinA = Math.sin(angle); - - double vxRobot = fieldOriented.vxMetersPerSecond*cosA + fieldOriented.vyMetersPerSecond*sinA; - double vyRobot = -fieldOriented.vxMetersPerSecond*sinA + fieldOriented.vyMetersPerSecond*cosA; - - return new ChassisSpeeds(vxRobot, vyRobot, fieldOriented.omegaRadiansPerSecond); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java new file mode 100644 index 0000000..c508c89 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface GyroIO { + @AutoLog + public static class GyroIOInputs { + + public boolean connected = false; + public Rotation2d yawPosition = Rotation2d.kZero; + public double yawVelocityRadPerSec = 0.0; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + } + + public default void updateInputs(GyroIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIORedux.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIORedux.java new file mode 100644 index 0000000..1985d87 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIORedux.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.drivetrain; + +import com.reduxrobotics.sensors.canandgyro.Canandgyro; +import com.reduxrobotics.sensors.canandgyro.CanandgyroSettings; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Timeouts; +import java.util.Queue; + +public class GyroIORedux implements GyroIO { + + private final Canandgyro gyro = new Canandgyro(DriveConstants.GRYO_CAN_ID); + + private final Queue yawTimestampQueue; + private final Queue yawPositionQueue; + + public GyroIORedux() { + // Configure the gyro + CanandgyroSettings settings = new CanandgyroSettings() + .setYawFramePeriod(1.0 / DriveConstants.ODOMETRY_FREQUENCY) + .setAngularPositionFramePeriod( + DriveConstants.GRYO_CAN_FRAME_FREQUENCY + ) + .setAngularVelocityFramePeriod( + DriveConstants.GRYO_CAN_FRAME_FREQUENCY + ); + gyro.setSettings( + settings, + Timeouts.STD_TIMEOUT_LONG, + Timeouts.STD_RETRY_ATTEMPTS + ); + gyro.setYaw(0.0, Timeouts.STD_TIMEOUT); + gyro.clearStickyFaults(); + + // Register the gyro signals + yawTimestampQueue = + PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal( + gyro::getYaw + ); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = gyro.isConnected(); + inputs.yawPosition = Rotation2d.fromRotations(gyro.getYaw()); + inputs.yawVelocityRadPerSec = Units.rotationsToRadians( + gyro.getAngularVelocityYaw() + ); + + inputs.odometryYawTimestamps = yawTimestampQueue + .stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryYawPositions = yawPositionQueue + .stream() + .map(Rotation2d::fromRotations) + .toArray(Rotation2d[]::new); + + yawTimestampQueue.clear(); + yawPositionQueue.clear(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java new file mode 100644 index 0000000..6eb6629 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -0,0 +1,153 @@ +package frc.robot.subsystems.drivetrain; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +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.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import org.littletonrobotics.junction.Logger; + +public class Module { + + private final ModuleIO io; + private final ModuleIOInputsAutoLogged inputs = + new ModuleIOInputsAutoLogged(); + private final int index; + private final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants; + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + private SwerveModulePosition[] odometryPositions = + new SwerveModulePosition[] {}; + + public Module( + ModuleIO io, + int index, + SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants + ) { + this.io = io; + this.index = index; + this.constants = constants; + this.driveDisconnectedAlert = new Alert( + "Disconnected drive motor on module " + index + ".", + AlertType.kError + ); + this.turnDisconnectedAlert = new Alert( + "Disconnected turn motor on module " + index + ".", + AlertType.kError + ); + this.turnEncoderDisconnectedAlert = new Alert( + "Disconnected turn encoder on module " + index + ".", + AlertType.kError + ); + } + + public void periodic() { + this.io.updateInputs(this.inputs); + Logger.processInputs("Drive/Module" + this.index, this.inputs); + + // Calculate positions for odometry + int sampleCount = this.inputs.odometryTimestamps.length; // All signals are sampled together + this.odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = + this.inputs.odometryDrivePositionsRad[i] * + this.constants.WheelRadius; + Rotation2d angle = this.inputs.odometryTurnPositions[i]; + this.odometryPositions[i] = new SwerveModulePosition( + positionMeters, + angle + ); + } + + // Update alerts + this.driveDisconnectedAlert.set(!this.inputs.driveConnected); + this.turnDisconnectedAlert.set(!this.inputs.turnConnected); + this.turnEncoderDisconnectedAlert.set( + !this.inputs.turnEncoderConnected + ); + } + + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + public void runSetpoint(SwerveModuleState state) { + // Optimize velocity setpoint + state.optimize(getAngle()); + state.cosineScale(this.inputs.turnPosition); + + // Apply setpoints + this.io.setDriveVelocity( + state.speedMetersPerSecond / this.constants.WheelRadius + ); + this.io.setTurnPosition(state.angle); + } + + /** Runs the module with the specified output while controlling to zero degrees. */ + public void runCharacterization(double output) { + this.io.setDriveOpenLoop(output); + this.io.setTurnPosition(Rotation2d.kZero); + } + + /** Disables all outputs to motors. */ + public void stop() { + this.io.setDriveOpenLoop(0.0); + this.io.setTurnOpenLoop(0.0); + } + + /** Returns the current turn angle of the module. */ + public Rotation2d getAngle() { + return this.inputs.turnPosition; + } + + /** Returns the current drive position of the module in meters. */ + public double getPositionMeters() { + return this.inputs.drivePositionRad * this.constants.WheelRadius; + } + + /** Returns the current drive velocity of the module in meters per second. */ + public double getVelocityMetersPerSec() { + return this.inputs.driveVelocityRadPerSec * this.constants.WheelRadius; + } + + /** Returns the module position (turn angle and drive position). */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return this.odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return this.inputs.odometryTimestamps; + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return this.inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(this.inputs.driveVelocityRadPerSec); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java new file mode 100644 index 0000000..1227ac3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIO.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.drivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs { + + public boolean driveConnected = false; + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; + + public boolean turnConnected = false; + public boolean turnEncoderConnected = false; + public Rotation2d turnAbsolutePosition = Rotation2d.kZero; + public Rotation2d turnPosition = Rotation2d.kZero; + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; + + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} + + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} + + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} + + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOSim.java new file mode 100644 index 0000000..1799292 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOSim.java @@ -0,0 +1,162 @@ +package frc.robot.subsystems.drivetrain; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module + * constants from Phoenix. Simulation is always based on voltage control. + */ +public class ModuleIOSim implements ModuleIO { + + // TunerConstants doesn't support separate sim constants, so they are declared locally + private static final double DRIVE_KP = 0.05; + private static final double DRIVE_KD = 0.0; + private static final double DRIVE_KS = 0.0; + private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = + 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + private static final double TURN_KP = 8.0; + private static final double TURN_KD = 0.0; + private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); + private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); + + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private PIDController driveController = new PIDController( + DRIVE_KP, + 0, + DRIVE_KD + ); + private PIDController turnController = new PIDController( + TURN_KP, + 0, + TURN_KD + ); + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + public ModuleIOSim( + SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants + ) { + // Create drive and turn sim models + driveSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DRIVE_GEARBOX, + constants.DriveInertia, + constants.DriveMotorGearRatio + ), + DRIVE_GEARBOX + ); + turnSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem( + TURN_GEARBOX, + constants.SteerInertia, + constants.SteerMotorGearRatio + ), + TURN_GEARBOX + ); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = + driveFFVolts + + driveController.calculate( + driveSim.getAngularVelocityRadPerSec() + ); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate( + turnSim.getAngularPositionRad() + ); + } else { + turnController.reset(); + } + + // Update simulation state + driveSim.setInputVoltage( + MathUtil.clamp(driveAppliedVolts, -12.0, 12.0) + ); + turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0)); + driveSim.update(Constants.ROBOT_LOOP_PERIOD); + turnSim.update(Constants.ROBOT_LOOP_PERIOD); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = driveSim.getAngularPositionRad(); + inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = new Rotation2d( + turnSim.getAngularPositionRad() + ); + inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad()); + inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = new double[] { Timer.getFPGATimestamp() }; + inputs.odometryDrivePositionsRad = new double[] { + inputs.drivePositionRad, + }; + inputs.odometryTurnPositions = new Rotation2d[] { inputs.turnPosition }; + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = + DRIVE_KS * Math.signum(velocityRadPerSec) + + DRIVE_KV * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFX.java new file mode 100644 index 0000000..9783773 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleIOTalonFX.java @@ -0,0 +1,348 @@ +package frc.robot.subsystems.drivetrain; + +import static frc.robot.util.PhoenixUtil.tryUntilOk; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.Timeouts; +import java.util.Queue; + +/** + * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and + * CANcoder. Configured using a set of module constants from Phoenix. + * + *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + */ +public class ModuleIOTalonFX implements ModuleIO { + + private final SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants; + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage( + 0.0 + ); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage( + 0.0 + ); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC( + 0 + ); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Timestamp inputs from Phoenix thread + private final Queue timestampQueue; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final Queue turnPositionQueue; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + private final Debouncer turnConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer( + Timeouts.STD_DEBOUNCE_TIME, + Debouncer.DebounceType.kFalling + ); + + public ModuleIOTalonFX( + SwerveModuleConstants< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration + > constants + ) { + this.constants = constants; + this.driveTalon = new TalonFX( + constants.DriveMotorId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + this.turnTalon = new TalonFX( + constants.SteerMotorId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + this.cancoder = new CANcoder( + constants.EncoderId, + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ); + + // Configure drive motor + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = + constants.DriveMotorGearRatio; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = + constants.SlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = + -constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> + this.driveTalon.getConfigurator().apply(driveConfig, 0.25) + ); + tryUntilOk(5, () -> this.driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor + var turnConfig = new TalonFXConfiguration(); + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.Feedback.FeedbackSensorSource = switch ( + constants.FeedbackSource + ) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; + default -> throw new RuntimeException( + "You have selected a turn feedback source that is not supported by the default implementation of ModuleIOTalonFX. Please check the AdvantageKit documentation for more information on alternative configurations: https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#custom-module-implementations" + ); + }; + turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + DriveConstants.MM_CRUISE_VELOCITY; + turnConfig.MotionMagic.MotionMagicAcceleration = + DriveConstants.MM_ACCELERATION; + turnConfig.MotionMagic.MotionMagicExpo_kV = DriveConstants.MM_EXPO_KV; + turnConfig.MotionMagic.MotionMagicExpo_kA = DriveConstants.MM_EXPO_KA; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; // ALWAYS TRUE FOR STEER MOTOR + turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> + this.turnTalon.getConfigurator().apply(turnConfig, 0.25) + ); + + // Configure CANCoder + CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; + cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + this.cancoder.getConfigurator().apply(cancoderConfig); + + // Create timestamp queue + this.timestampQueue = + PhoenixOdometryThread.getInstance().makeTimestampQueue(); + + // Create drive status signals + this.drivePosition = this.driveTalon.getPosition(); + this.drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal( + this.drivePosition.clone() + ); + this.driveVelocity = this.driveTalon.getVelocity(); + this.driveAppliedVolts = this.driveTalon.getMotorVoltage(); + this.driveCurrent = this.driveTalon.getStatorCurrent(); + + // Create turn status signals + this.turnAbsolutePosition = this.cancoder.getAbsolutePosition(); + this.turnPosition = this.turnTalon.getPosition(); + this.turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal( + this.turnPosition.clone() + ); + this.turnVelocity = this.turnTalon.getVelocity(); + this.turnAppliedVolts = this.turnTalon.getMotorVoltage(); + this.turnCurrent = this.turnTalon.getStatorCurrent(); + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.ODOMETRY_FREQUENCY, + this.drivePosition, + this.turnPosition + ); + BaseStatusSignal.setUpdateFrequencyForAll( + DriveConstants.DRIVE_CAN_FRAME_FREQUENCY, + this.driveVelocity, + this.driveAppliedVolts, + this.driveCurrent, + this.turnAbsolutePosition, + this.turnVelocity, + this.turnAppliedVolts, + this.turnCurrent + ); + ParentDevice.optimizeBusUtilizationForAll( + this.driveTalon, + this.turnTalon + ); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Refresh all signals + var driveStatus = BaseStatusSignal.refreshAll( + this.drivePosition, + this.driveVelocity, + this.driveAppliedVolts, + this.driveCurrent + ); + var turnStatus = BaseStatusSignal.refreshAll( + this.turnPosition, + this.turnVelocity, + this.turnAppliedVolts, + this.turnCurrent + ); + var turnEncoderStatus = BaseStatusSignal.refreshAll( + this.turnAbsolutePosition + ); + + // Update drive inputs + inputs.driveConnected = this.driveConnectedDebounce.calculate( + driveStatus.isOK() + ); + inputs.drivePositionRad = Units.rotationsToRadians( + this.drivePosition.getValueAsDouble() + ); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians( + this.driveVelocity.getValueAsDouble() + ); + inputs.driveAppliedVolts = this.driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = this.driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = this.turnConnectedDebounce.calculate( + turnStatus.isOK() + ); + inputs.turnEncoderConnected = + this.turnEncoderConnectedDebounce.calculate( + turnEncoderStatus.isOK() + ); + inputs.turnAbsolutePosition = Rotation2d.fromRotations( + this.turnAbsolutePosition.getValueAsDouble() + ); + inputs.turnPosition = Rotation2d.fromRotations( + this.turnPosition.getValueAsDouble() + ); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians( + this.turnVelocity.getValueAsDouble() + ); + inputs.turnAppliedVolts = this.turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = this.turnCurrent.getValueAsDouble(); + + // Update odometry inputs + inputs.odometryTimestamps = this.timestampQueue.stream() + .mapToDouble((Double value) -> value) + .toArray(); + inputs.odometryDrivePositionsRad = this.drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = this.turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); + this.timestampQueue.clear(); + this.drivePositionQueue.clear(); + this.turnPositionQueue.clear(); + } + + @Override + public void setDriveOpenLoop(double output) { + this.driveTalon.setControl( + switch (this.constants.DriveMotorClosedLoopOutput) { + case Voltage -> this.voltageRequest.withOutput(output); + case TorqueCurrentFOC -> this.torqueCurrentRequest.withOutput( + output + ); + } + ); + } + + @Override + public void setTurnOpenLoop(double output) { + this.turnTalon.setControl( + switch (this.constants.SteerMotorClosedLoopOutput) { + case Voltage -> this.voltageRequest.withOutput(output); + case TorqueCurrentFOC -> this.torqueCurrentRequest.withOutput( + output + ); + } + ); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + this.driveTalon.setControl( + switch (this.constants.DriveMotorClosedLoopOutput) { + case Voltage -> this.velocityVoltageRequest.withVelocity( + velocityRotPerSec + ); + case TorqueCurrentFOC -> this.velocityTorqueCurrentRequest.withVelocity( + velocityRotPerSec + ); + } + ); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + this.turnTalon.setControl( + switch (this.constants.SteerMotorClosedLoopOutput) { + case Voltage -> this.positionVoltageRequest.withPosition( + rotation.getRotations() + ); + case TorqueCurrentFOC -> this.positionTorqueCurrentRequest.withPosition( + rotation.getRotations() + ); + } + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drivetrain/PhoenixOdometryThread.java new file mode 100644 index 0000000..b6f01b5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/PhoenixOdometryThread.java @@ -0,0 +1,179 @@ +// Copyright (c) 2021-2025 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drivetrain; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants.DriveConstants; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + + private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + private final List genericSignals = new ArrayList<>(); + private final List> phoenixQueues = new ArrayList<>(); + private final List> genericQueues = new ArrayList<>(); + private final List> timestampQueues = new ArrayList<>(); + + private static boolean isCANFD = new CANBus( + DriveConstants.DRIVETRAIN_CONSTANTS.CANBusName + ).isNetworkFD(); + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + } + + @Override + public void start() { + if (this.timestampQueues.size() > 0) { + super.start(); + } + } + + /** Registers a Phoenix signal to be read from the thread. */ + public Queue registerSignal(StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + this.signalsLock.lock(); + Drive.odometryLock.lock(); + try { + BaseStatusSignal[] newSignals = + new BaseStatusSignal[this.phoenixSignals.length + 1]; + System.arraycopy( + this.phoenixSignals, + 0, + newSignals, + 0, + this.phoenixSignals.length + ); + newSignals[this.phoenixSignals.length] = signal; + this.phoenixSignals = newSignals; + this.phoenixQueues.add(queue); + } finally { + this.signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Registers a generic signal to be read from the thread. */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + this.signalsLock.lock(); + Drive.odometryLock.lock(); + try { + this.genericSignals.add(signal); + this.genericQueues.add(queue); + } finally { + this.signalsLock.unlock(); + Drive.odometryLock.unlock(); + } + return queue; + } + + /** Returns a new queue that returns timestamp values for each sample. */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + Drive.odometryLock.lock(); + try { + this.timestampQueues.add(queue); + } finally { + Drive.odometryLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + this.signalsLock.lock(); + try { + if (isCANFD && this.phoenixSignals.length > 0) { + BaseStatusSignal.waitForAll( + 2.0 / DriveConstants.ODOMETRY_FREQUENCY, + this.phoenixSignals + ); + } else { + // "waitForAll" does not support blocking on multiple signals with a bus + // that is not CAN FD, regardless of Pro licensing. No reasoning for this + // behavior is provided by the documentation. + Thread.sleep( + (long) (1000.0 / DriveConstants.ODOMETRY_FREQUENCY) + ); + if (this.phoenixSignals.length > 0) { + BaseStatusSignal.refreshAll(this.phoenixSignals); + } + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + this.signalsLock.unlock(); + } + + // Save new data to queues + Drive.odometryLock.lock(); + try { + // Sample timestamp is current FPGA time minus average CAN latency + // Default timestamps from Phoenix are NOT compatible with + // FPGA timestamps, this solution is imperfect but close + double timestamp = RobotController.getFPGATime() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : this.phoenixSignals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (this.phoenixSignals.length > 0) { + timestamp -= totalLatency / this.phoenixSignals.length; + } + + // Add new samples to queues + for (int i = 0; i < this.phoenixSignals.length; i++) { + this.phoenixQueues.get(i).offer( + this.phoenixSignals[i].getValueAsDouble() + ); + } + for (int i = 0; i < this.genericSignals.size(); i++) { + this.genericQueues.get(i).offer( + this.genericSignals.get(i).getAsDouble() + ); + } + for (int i = 0; i < this.timestampQueues.size(); i++) { + this.timestampQueues.get(i).offer(timestamp); + } + } finally { + Drive.odometryLock.unlock(); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java deleted file mode 100644 index 7b84e06..0000000 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveModule.java +++ /dev/null @@ -1,107 +0,0 @@ -package frc.robot.subsystems.drivetrain; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.VelocityDutyCycle; -import com.ctre.phoenix6.hardware.TalonFX; -import com.reduxrobotics.sensors.canandmag.Canandmag; -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.smartdashboard.SmartDashboard; -import static frc.robot.CONSTANTS.*; - -public class SwerveModule { - private static final boolean OUTPUT_TO_SMART_DASH = true; - - private final int steeringMotorCANId; - - private final TalonFX steeringMotor; - private final TalonFX driveMotor; - - private final Canandmag encoder; - - public SwerveModule(int steeringMotorCANId, int driveMotorCANId, int encoderCANId) { - this.steeringMotorCANId = steeringMotorCANId; - - this.steeringMotor = new TalonFX(steeringMotorCANId); - this.driveMotor = new TalonFX(driveMotorCANId); - this.encoder = new Canandmag(encoderCANId); - - var steeringConfigs = new TalonFXConfiguration(); - steeringConfigs.Slot0.kP = 0.16; - steeringConfigs.Slot0.kI = 0.0; - steeringConfigs.Slot0.kD = 0.0; - steeringConfigs.Slot0.kV = 0.0; // feedforward term - - //experimenting with motion magic to smooth steering - /* var motionMagicConfigs = steeringConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = 500; //Target cruise velocity of 500rps - motionMagicConfigs.MotionMagicAcceleration = 1000; //Target acceleration of 1000rps/s (0.5 sec) - motionMagicConfigs.MotionMagicJerk = 0; //Target jerk of 0 rps/s/s (0 sec) */ - - this.steeringMotor.getConfigurator().apply(steeringConfigs); - - var driveConfigs = new TalonFXConfiguration(); - driveConfigs.Slot0.kP = 0.02; - driveConfigs.Slot0.kI = 0.0; - driveConfigs.Slot0.kD = 0.0; - driveConfigs.Slot0.kV = 0.0; // feedforward term - this.driveMotor.getConfigurator().apply(driveConfigs); - } - - public void setModuleToEncoder() { - // set the internal encoder based on the absolution position of the external encoder - double encPosition = this.encoder.getAbsPosition(); // 0.0 to 1.0, inclusive, increasing counterclockwise - this.steeringMotor.setPosition(STEERING_GEAR_RATIO*encPosition); - - // set the steering motor to the current position so it doesn't try to move - double positionInRotations = STEERING_GEAR_RATIO*encPosition; - ControlRequest steeringControlRequest = new PositionDutyCycle(positionInRotations); - this.steeringMotor.setControl(steeringControlRequest); - } - - public SwerveModulePosition getModulePosition() { - double positionOfSteeringRad = 2*Math.PI*this.steeringMotor.getPosition().getValueAsDouble() / STEERING_GEAR_RATIO; - double wheelRotations = this.driveMotor.getPosition().getValueAsDouble(); - return new SwerveModulePosition( - wheelRotations / EFFECTIVE_GEAR_RATIO, new Rotation2d(-positionOfSteeringRad)); - } - - public void setModuleState(SwerveModuleState state) { - // get the current position of the steering motor and optimize the state - SwerveModulePosition currentPosition = getModulePosition(); - state.optimize(currentPosition.angle); - - // set the position of the steering motor - // remember that angle is the negative of what the motors want, hence the minus - double positionInRotations = -STEERING_GEAR_RATIO*state.angle.getDegrees()/360.0; - ControlRequest steeringControlRequest = new PositionDutyCycle(positionInRotations); - this.steeringMotor.setControl(steeringControlRequest); - - // calculate a speed scale factor (cosine compensation) - double scaleFactor = state.angle.minus(currentPosition.angle).getCos(); - - // set the speed of the drive motor - ControlRequest driveControlRequest = new VelocityDutyCycle( - EFFECTIVE_GEAR_RATIO*state.speedMetersPerSecond*scaleFactor); - this.driveMotor.setControl(driveControlRequest); - - // this is probably not the best place for this code, but this is a sandbox project - - // if (OUTPUT_TO_SMART_DASH) { - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/Drive/demand_wheelRotationsPerSecond", wheelRotationsPerSecond); - - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/Steering/demand_positionInRotations", positionInRotations); - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/Steering/actual_positionOfSteering", positionOfSteeringRad); - - // double encPosition = encoder.getAbsPosition(); // 0.0 to 1.0, inclusive, increasing counterclockwise - // SmartDashboard.putNumber( - // "Swerve States/" + this.steeringMotorCANId + "/external_encoderPosition", encPosition); - // } - } -} diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java new file mode 100644 index 0000000..07939ea --- /dev/null +++ b/src/main/java/frc/robot/util/LocalADStarAK.java @@ -0,0 +1,160 @@ +// Copyright (c) 2021-2025 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinder; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +// NOTE: This file is available at +// https://gist.github.com/mjansen4857/a8024b55eb427184dbd10ae8923bd57d + +public class LocalADStarAK implements Pathfinder { + private final ADStarIO io = new ADStarIO(); + + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ + @Override + public boolean isNewPathAvailable() { + if (!Logger.hasReplaySource()) { + io.updateIsNewPathAvailable(); + } + + Logger.processInputs("LocalADStarAK", io); + + return io.isNewPathAvailable; + } + + /** + * Get the most recently calculated path + * + * @param constraints The path constraints to use when creating the path + * @param goalEndState The goal end state to use when creating the path + * @return The PathPlannerPath created from the points calculated by the pathfinder + */ + @Override + public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { + if (!Logger.hasReplaySource()) { + io.updateCurrentPathPoints(constraints, goalEndState); + } + + Logger.processInputs("LocalADStarAK", io); + + if (io.currentPathPoints.isEmpty()) { + return null; + } + + return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState); + } + + /** + * Set the start position to pathfind from + * + * @param startPosition Start position on the field. If this is within an obstacle it will be + * moved to the nearest non-obstacle node. + */ + @Override + public void setStartPosition(Translation2d startPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setStartPosition(startPosition); + } + } + + /** + * Set the goal position to pathfind to + * + * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved + * to the nearest non-obstacle node. + */ + @Override + public void setGoalPosition(Translation2d goalPosition) { + if (!Logger.hasReplaySource()) { + io.adStar.setGoalPosition(goalPosition); + } + } + + /** + * Set the dynamic obstacles that should be avoided while pathfinding. + * + * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents + * opposite corners of a bounding box. + * @param currentRobotPos The current position of the robot. This is needed to change the start + * position of the path to properly avoid obstacles + */ + @Override + public void setDynamicObstacles( + List> obs, Translation2d currentRobotPos) { + if (!Logger.hasReplaySource()) { + io.adStar.setDynamicObstacles(obs, currentRobotPos); + } + } + + private static class ADStarIO implements LoggableInputs { + public LocalADStar adStar = new LocalADStar(); + public boolean isNewPathAvailable = false; + public List currentPathPoints = Collections.emptyList(); + + @Override + public void toLog(LogTable table) { + table.put("IsNewPathAvailable", isNewPathAvailable); + + double[] pointsLogged = new double[currentPathPoints.size() * 2]; + int idx = 0; + for (PathPoint point : currentPathPoints) { + pointsLogged[idx] = point.position.getX(); + pointsLogged[idx + 1] = point.position.getY(); + idx += 2; + } + + table.put("CurrentPathPoints", pointsLogged); + } + + @Override + public void fromLog(LogTable table) { + isNewPathAvailable = table.get("IsNewPathAvailable", false); + + double[] pointsLogged = table.get("CurrentPathPoints", new double[0]); + + List pathPoints = new ArrayList<>(); + for (int i = 0; i < pointsLogged.length; i += 2) { + pathPoints.add( + new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null)); + } + + currentPathPoints = pathPoints; + } + + public void updateIsNewPathAvailable() { + isNewPathAvailable = adStar.isNewPathAvailable(); + } + + public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) { + PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState); + + if (currentPath != null) { + currentPathPoints = currentPath.getAllPathPoints(); + } else { + currentPathPoints = Collections.emptyList(); + } + } + } +} diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java new file mode 100644 index 0000000..ba68f1a --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -0,0 +1,21 @@ +// Copyright (c) 2021-2025 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util; + +import com.ctre.phoenix6.StatusCode; +import java.util.function.Supplier; + +public class PhoenixUtil { + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) break; + } + } +} diff --git a/src/main/java/frc/robot/util/PoseEstimator.java b/src/main/java/frc/robot/util/PoseEstimator.java new file mode 100644 index 0000000..f1a14f2 --- /dev/null +++ b/src/main/java/frc/robot/util/PoseEstimator.java @@ -0,0 +1,148 @@ +// Copyright (c) 2021-2025 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD-// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.Matrix; +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.Twist2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** + * Utility class for managing swerve drive pose estimation. Encapsulates the SwerveDrivePoseEstimator + * and handles odometry updates, vision measurements, and gyro integration. + */ +public class PoseEstimator { + + private final SwerveDriveKinematics kinematics; + private final SwerveDrivePoseEstimator poseEstimator; + + private Rotation2d rawGyroRotation = Rotation2d.kZero; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; + + /** + * Creates a new PoseEstimator. + * + * @param kinematics The swerve drive kinematics + * @param initialGyroRotation The initial gyro rotation + * @param initialPose The initial pose estimate + */ + public PoseEstimator( + SwerveDriveKinematics kinematics, + Rotation2d initialGyroRotation, + Pose2d initialPose + ) { + this.kinematics = kinematics; + this.rawGyroRotation = initialGyroRotation; + this.poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + lastModulePositions, + initialPose + ); + } + + /** + * Updates the pose estimator with odometry data. + * + * @param modulePositions The current module positions + * @param gyroRotation The current gyro rotation (or null to use kinematics) + * @param timestamp The timestamp of this sample + */ + public void updateOdometry( + SwerveModulePosition[] modulePositions, + Rotation2d gyroRotation, + double timestamp + ) { + // Calculate module deltas + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters - + lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle + ); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + // Update gyro angle + if (gyroRotation != null) { + // Use the real gyro angle + rawGyroRotation = gyroRotation; + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus( + new Rotation2d(twist.dtheta) + ); + } + + // Apply update to pose estimator + poseEstimator.updateWithTime( + timestamp, + rawGyroRotation, + modulePositions + ); + } + + /** + * Adds a vision measurement to the pose estimator. + * + * @param visionRobotPoseMeters The vision-measured robot pose + * @param timestampSeconds The timestamp of the vision measurement + * @param visionMeasurementStdDevs The standard deviations of the vision measurement + */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + poseEstimator.addVisionMeasurement( + visionRobotPoseMeters, + timestampSeconds, + visionMeasurementStdDevs + ); + } + + /** + * Resets the pose estimator to a specific pose. + * + * @param pose The new pose + * @param modulePositions The current module positions + */ + public void resetPose(Pose2d pose, SwerveModulePosition[] modulePositions) { + poseEstimator.resetPosition(rawGyroRotation, modulePositions, pose); + } + + /** + * Returns the current estimated pose. + * + * @return The current pose estimate + */ + public Pose2d getEstimatedPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** + * Returns the current raw gyro rotation. + * + * @return The raw gyro rotation + */ + public Rotation2d getRawGyroRotation() { + return rawGyroRotation; + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..e5934b3 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json new file mode 100644 index 0000000..d5ad2ab --- /dev/null +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.2.7.json", + "name": "PathplannerLib", + "version": "2025.2.7", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.2.7" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.2.7", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index bf6fdf4..f6f3b7f 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,479 +1,479 @@ { - "fileName": "Phoenix6-frc2025-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.4.0", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.4.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "25.4.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.4.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.4.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.4.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.4.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.4.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.4.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.4.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.4.0", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.4.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.4.0", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.4.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.4.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.4.0", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.4.0", - "libName": "CTRE_SimProCANdi", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "25.4.0", - "libName": "CTRE_SimProCANdle", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.4.0", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/ReduxLib-2025.0.1.json b/vendordeps/ReduxLib-2025.0.1.json index 806b684..d144dfd 100644 --- a/vendordeps/ReduxLib-2025.0.1.json +++ b/vendordeps/ReduxLib-2025.0.1.json @@ -1,72 +1,72 @@ { - "fileName": "ReduxLib-2025.0.1.json", - "name": "ReduxLib", - "version": "2025.0.1", - "frcYear": "2025", - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2025.0.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2025.0.1", - "libName": "ReduxLib", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - }, - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.1", - "libName": "ReduxCore", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file + "fileName": "ReduxLib-2025.0.1.json", + "name": "ReduxLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.1", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 2d7b1d8..36e9416 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,71 +1,71 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.3.1" - } - ] -} \ No newline at end of file +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1" + } + ] +}