diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 00000000..b1de0b1f --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1,7 @@ +{ + "download": { + "localDir": "C:\\Users\\robot\\Downloads\\AllLogs", + "remoteDir": "/u/logs", + "serverTeam": "5010" + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 00000000..0967ef42 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 00000000..02b78d36 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,18 @@ +name: Build + +on: + push: + pull_request: + +jobs: + build: + name: Build + runs-on: ubuntu-latest + container: wpilib/roborio-cross-ubuntu:2024-22.04 + steps: + - name: Checkout repository + uses: actions/checkout@v4 + - name: Grant execute permission + run: chmod +x gradlew + - name: Build robot code + run: ./gradlew build diff --git a/.gitignore b/.gitignore index 34cbaac1..165ff422 100644 --- a/.gitignore +++ b/.gitignore @@ -185,3 +185,7 @@ compile_commands.json # Eclipse generated file for annotation processors .factorypath +/results +/src/main/resources/pose_optimizer +src/main/java/frc/robot/BuildConstants.java +src/main/java/frc/robot/BuildConstants.java diff --git a/.mcp.json b/.mcp.json new file mode 100644 index 00000000..f2c3a91b --- /dev/null +++ b/.mcp.json @@ -0,0 +1,10 @@ +{ + "mcpServers": { + "frc-docs": { + "command": "uvx", + "args": [ + "first-agentic-csa" + ] + } + } +} diff --git a/.vscode/settings.json b/.vscode/settings.json index 8328c83c..de5cd2c4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -29,7 +29,7 @@ null ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.import.gradle.annotationProcessing.enabled": false, + "java.import.gradle.annotationProcessing.enabled": true, "java.completion.favoriteStaticMembers": [ "org.junit.Assert.*", "org.junit.Assume.*", @@ -163,5 +163,5 @@ "url": "./src/main/resources/schemas/yams-shooter.schema.json" } ], - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 00000000..93b1604c --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,80 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project overview + +FRC Team 5010 (Rebuilt2026) WPILib robot code, built on GradleRIO 2026 with Java 17. The code is split into two layers: + +- `frc.robot.rebuilt.*` — season-specific robot code (subsystems, commands, constants for the 2026 game). +- `org.frc5010.common.*` — team's reusable library (`FRC5010Lib`): config-driven robot/subsystem creation, motor/sensor abstractions, drivetrains, vision, telemetry. Treat this as a stable library shared across seasons. + +The entry point is `frc.robot.Main` → `frc.robot.Robot` (an AdvantageKit `LoggedRobot`) → `RobotContainer`, which uses `RobotsParser` to pick a concrete `GenericRobot` subclass based on the RoboRIO's MAC address (see `src/main/deploy/robots.json`). For this season the active robot class is `frc.robot.rebuilt.Rebuilt`. + +## Commands + +All build commands run through the Gradle wrapper. Use `./gradlew` on Unix or `gradlew.bat` on Windows (PowerShell: `.\gradlew.bat`). + +- Build: `./gradlew build` +- Deploy to roboRIO: `./gradlew deploy` +- Run desktop simulation (with Glass GUI + DriverStation): `./gradlew simulateJava` +- Run tests: `./gradlew test` +- Run a single test: `./gradlew test --tests "org.frc5010.common.arch.GenericRobotTest"` +- Format (auto-applied before compile): `./gradlew spotlessApply`. Check only: `./gradlew spotlessCheck`. +- Generate Javadocs: `./gradlew javadoc` → output in `build/docs/javadoc/`. +- Replay an AdvantageKit log: `./gradlew replayWatch` (runs `org.littletonrobotics.junction.ReplayWatch`). + +Note: `spotlessApply` is wired as a dependency of `compileJava`, so a normal build will reformat sources. The `eventDeploy` task auto-commits working changes when on an `event*` branch during a deploy. + +### Simulation vs. real vs. replay + +`frc.robot.Constants.CURRENT_MODE` is set from `RobotBase.isReal()` and `SIM_MODE`. To switch between Glass simulation and AdvantageKit log replay, edit `SIM_MODE` in `src/main/java/frc/robot/Constants.java` (`Mode.SIM` or `Mode.REPLAY`). + +## Architecture + +### Robot selection by MAC address + +`src/main/deploy/robots.json` maps a roboRIO MAC address to a `robotClass` and a deploy config directory. `RobotsParser` looks up the running robot's MAC and instantiates the matching `GenericRobot` subclass, passing the config directory (e.g., `rebuilt_robot`) to its constructor. When simulating, the `simulate: true` robot is picked. To add a new robot variant, add an entry to `robots.json` and create a matching `src/main/deploy//` tree. + +### Config-driven subsystems + +Robots and most hardware are not hard-coded — they're loaded from JSON in `src/main/deploy//`. The relevant parsers live in `org.frc5010.common.config`: + +- `RobotParser` reads `robot.json` (drivetrain type, dimensions, units, game pieces, user mode). +- `SubsystemParser` reads files under `subsystems/` and constructs `GenericSubsystem`s, populating `GenericRobot.subsystems`, `controllers`, and `devices` maps. +- `RobotsParser` selects the robot class. +- `UnitsParser` + the enums in `org.frc5010.common.config.units` parse unit strings (e.g., `"in"`, `"m/s"`) on JSON fields like `trackWidthUom`. See `docs/UNIT_ENUM_DESIGN_PATTERN.md` for the alias-based parsing pattern. + +Each season subsystem typically calls `super(".json")` in its constructor (see `frc.robot.rebuilt.subsystems.Climb.Climb`) which loads its device map from JSON via `GenericSubsystem`. + +JSON schemas live in `src/main/java/org/frc5010/common/config/schemas/` and are wired into VSCode validation; see `docs/JSON_SCHEMAS.md` for the schema-to-class map. Keep the schema in sync when changing a `*ConfigurationJson` class. + +### AdvantageKit IO pattern + +Hardware-touching subsystems follow the AdvantageKit IO split: + +``` +SubsystemName.java // logic, owns an IO instance and *InputsAutoLogged +SubsystemNameIO.java // interface + @AutoLog inputs class +SubsystemNameIOReal.java // real hardware +SubsystemNameIOSim.java // simulation +``` + +The subsystem picks `IOReal` vs `IOSim` from `RobotBase.isSimulation()`. The `*InputsAutoLogged` class is generated by the `akit-autolog` annotation processor — run a build before referencing newly added `@AutoLog` fields. Examples: `subsystems/Climb/`, `subsystems/Launcher/`, `subsystems/Indexer/`, `subsystems/intake/`. + +### Commands + +Per-subsystem command containers live in `frc.robot.rebuilt.commands.*Commands` (e.g., `LauncherCommands`, `ClimbCommands`). Each container receives the `subsystems` map, owns `configureButtonBindings(driver, operator)`, and may own `setupDefaultCommands()`. `Rebuilt.configureButtonBindings` calls each container once; the `isButtonsConfigured` guard prevents re-binding between teleop init calls. `NamedCommandsReg` registers commands for PathPlanner auto routines. + +### Logging and build metadata + +The build runs `gversion` and writes `frc.robot.BuildConstants` with git SHA, branch, and dirty state. `Robot.` records this as AdvantageKit metadata. On real robots logs go to `/U/logs` (USB) + NetworkTables; in sim they go to NT + a local WPILOG. JVM args in `build.gradle` enable JMX (port 1198) for VisualVM and tune the serial GC for the roboRIO's real-time loop — don't change these without measuring. + +## Conventions + +- Java 17, Lombok available (`io.freefair.lombok` plugin) — use `@Getter`/`@RequiredArgsConstructor` etc. where it reads better. +- Spotless uses Google Java Format. Don't fight the formatter; `compileJava` will reformat anyway. +- JSON is also formatted (Gson, 2-space indent). Markdown is trimmed/indented by Spotless too. +- New public APIs in `org.frc5010.common.*` should be Javadoc'd — see `docs/JAVADOC_GUIDELINES.md` for the team's expected format. +- Units: prefer the WPILib units library (`edu.wpi.first.units.*`) over bare doubles in new code (`Distance`, `Voltage`, etc.), matching the style in `Climb.java`/`Rebuilt.java`. +- `AllianceFlipUtil.configure(...)` must be called before any auto-pose flipping; `Rebuilt` does this in its constructor with `FieldConstants.FIELD_WIDTH`/`FIELD_LENGTH`. diff --git a/build.gradle b/build.gradle index d555a2bf..88eb8652 100644 --- a/build.gradle +++ b/build.gradle @@ -47,10 +47,18 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Heap: pre-size to 32m so the JVM doesn't start with a tiny default heap + // and immediately GC. Max 128m keeps us well within the roboRIO's ~400m + // available to user processes. + jvmArgs.add("-Xms32m") + jvmArgs.add("-Xmx128m") jvmArgs.add("-XX:+UnlockExperimentalVMOptions") - jvmArgs.add("-XX:GCTimeRatio=5") + // GCTimeRatio=5 means 1/(1+5)=16% of CPU time is allowed for GC. + // Increase to 19 (≈5%) to starve GC less on a real-time loop. + jvmArgs.add("-XX:GCTimeRatio=19") jvmArgs.add("-XX:+UseSerialGC") - jvmArgs.add("-XX:MaxGCPauseMillis=50") + // Keep MaxGCPauseMillis — hint to GC to try to stay under 4ms. + jvmArgs.add("-XX:MaxGCPauseMillis=4") // Enable VisualVM connection jvmArgs.add("-Dcom.sun.management.jmxremote=true") jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") @@ -86,6 +94,50 @@ task(replayWatch, type: JavaExec) { classpath = sourceSets.main.runtimeClasspath } +// Analyze a .wpilog file and print a summary of key swerve/vision signals. +// Usage: +// .\gradlew.bat logSummary # most recent log in logs/ +// .\gradlew.bat logSummary -PlogFile=logs/foo.wpilog # specific file +task(logSummary, type: JavaExec) { + group = 'FRC5010' + description = 'Print a diagnostic summary of a .wpilog file' + mainClass = "org.frc5010.common.util.LogSummary" + classpath = sourceSets.main.runtimeClasspath + if (project.hasProperty('logFile')) { + args = [project.logFile] + } +} + +// Run logSummary on the most recent replay (_sim.wpilog) in logs/. +// Usage: .\gradlew.bat replayValidate +// Typical workflow: +// 1. .\gradlew.bat simulateJava # produce a live log +// 2. .\gradlew.bat simulateJava -Plog= # replay it (Ctrl-C when done) +// 3. .\gradlew.bat replayValidate # check the replay log +task(replayValidate, type: JavaExec) { + group = 'FRC5010' + description = 'Run logSummary on the most recent replay (_sim.wpilog) in logs/' + mainClass = "org.frc5010.common.util.LogSummary" + classpath = sourceSets.main.runtimeClasspath + doFirst { + def logsDir = file('logs') + if (!logsDir.isDirectory()) { + throw new GradleException( + "logs/ directory not found — run the simulation first:\n" + + " .\\gradlew.bat simulateJava") + } + def latest = logsDir.listFiles() + .findAll { it.name.endsWith('_sim.wpilog') } + .max { it.lastModified() } + if (!latest) { + throw new GradleException( + "No _sim.wpilog found in logs/ — run replay first:\n" + + " .\\gradlew.bat simulateJava -Plog=") + } + args = [latest.absolutePath] + } +} + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { @@ -130,8 +182,22 @@ test { } // Simulation configuration (e.g. environment variables). -wpi.sim.addGui().defaultEnabled = true -wpi.sim.addDriverstation() +// REPLAY mode (-Plog=...) must run without HAL sim extensions — AdvantageKit +// enforces this and throws if the Glass GUI or DriverStation extensions are present. +if (!project.hasProperty('log')) { + wpi.sim.addGui().defaultEnabled = true + wpi.sim.addDriverstation() +} + +// Forward the replay log path to the JVM as a system property so Robot code can read them. +// Usage: +// .\gradlew.bat simulateJava # Glass simulation +// .\gradlew.bat simulateJava -Plog=logs/foo.wpilog # replay mode (headless) +tasks.withType(JavaExec).configureEach { + if (project.hasProperty('log')) { + systemProperty 'log', project.log + } +} // 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 diff --git a/simgui-ds.json b/simgui-ds.json index 01e79d2a..7e39eb7f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -97,7 +97,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "Keyboard0", + "name": "Driver" } ] } diff --git a/simgui.json b/simgui.json index 2cfd91f7..2f523cfd 100644 --- a/simgui.json +++ b/simgui.json @@ -10,6 +10,51 @@ } }, "Other Devices": { + "CANcoder (v6)[16]": { + "header": { + "open": true + } + }, + "SPARK MAX [10]": { + "header": { + "open": true + } + }, + "SPARK MAX [11]": { + "header": { + "open": true + } + }, + "SPARK MAX [11] RELATIVE ENCODER": { + "header": { + "open": true + } + }, + "SPARK MAX [12]": { + "header": { + "open": true + } + }, + "SPARK MAX [13]": { + "header": { + "open": true + } + }, + "SPARK MAX [13] RELATIVE ENCODER": { + "header": { + "open": true + } + }, + "SPARK MAX [16]": { + "header": { + "open": true + } + }, + "SPARK MAX [16] RELATIVE ENCODER": { + "header": { + "open": true + } + }, "SPARK MAX [5]": { "header": { "open": true @@ -20,6 +65,11 @@ "open": true } }, + "SPARK MAX [9]": { + "header": { + "open": true + } + }, "Talon FX (v6)[12]": { "header": { "open": true @@ -64,6 +114,9 @@ "header": { "open": true } + }, + "window": { + "visible": false } } }, @@ -86,8 +139,10 @@ "/Shuffleboard/BabySwerve/Auto Modes": "String Chooser", "/Shuffleboard/ExampleRobot/Auto Modes": "String Chooser", "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/Alpha/Auto Modes": "String Chooser", "/SmartDashboard/Arm/mechanism": "Mechanism2d", "/SmartDashboard/Auto Modes": "String Chooser", + "/SmartDashboard/Climb/mechanism": "Mechanism2d", "/SmartDashboard/Drive Visual": "Mechanism2d", "/SmartDashboard/DrivePoseEstimator/Pose Field": "Field2d", "/SmartDashboard/DrivePoseEstimator/values/Pose Field": "Field2d", @@ -98,6 +153,31 @@ "/SmartDashboard/Hood/mechanism": "Mechanism2d", "/SmartDashboard/IMU": "Alerts", "/SmartDashboard/JSON": "Alerts", + "/SmartDashboard/Mechanisms/Commands/Climb/Live Tuning": "Command", + "/SmartDashboard/Mechanisms/Commands/ElevatorMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/ElevatorMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/ElevatorMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/Intake/Live Tuning": "Command", + "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning": "Command", + "/SmartDashboard/Mechanisms/Commands/climb.json/Live Tuning": "Command", + "/SmartDashboard/Mechanisms/Commands/flywheelMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/flywheelMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/flywheelMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/hoodMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/hoodMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/hoodMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/hopperMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/hopperMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/hopperMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/lifterMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/lifterMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/lifterMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/spintakeMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/spintakeMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/spintakeMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/turretMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/turretMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/turretMotor/ZeroEncoder": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/Down": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/LiveTuning": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/Up": "Command", @@ -108,311 +188,270 @@ "/SmartDashboard/PhotonAlerts": "Alerts", "/SmartDashboard/Pigeon 2 (v6) [13]": "Gyro", "/SmartDashboard/Pigeon 2 [13]": "Gyro", + "/SmartDashboard/Rebuilt/Auto Modes": "String Chooser", "/SmartDashboard/Robot Visual": "Mechanism2d", "/SmartDashboard/SendableChooser[0]": "String Chooser", "/SmartDashboard/Shooter/mechanism": "Mechanism2d", "/SmartDashboard/Swerve Drive": "Alerts", + "/SmartDashboard/TigerShark/Auto Modes": "String Chooser", "/SmartDashboard/Turret/mechanism": "Mechanism2d", "/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d", + "/SmartDashboard/flywheel/mechanism": "Mechanism2d", + "/SmartDashboard/hood/mechanism": "Mechanism2d", "/SmartDashboard/hoodmotor/mechanism": "Mechanism2d", + "/SmartDashboard/hopper/mechanism": "Mechanism2d", + "/SmartDashboard/lifter/mechanism": "Mechanism2d", + "/SmartDashboard/lowershootermotor/mechanism": "Mechanism2d", "/SmartDashboard/navX-Sensor[1]": "Gyro", - "/SmartDashboard/navX-Sensor[4]": "Gyro" + "/SmartDashboard/navX-Sensor[4]": "Gyro", + "/SmartDashboard/pinion/mechanism": "Mechanism2d", + "/SmartDashboard/spintake/mechanism": "Mechanism2d", + "/SmartDashboard/spintake_inner/mechanism": "Mechanism2d", + "/SmartDashboard/spintake_outer/mechanism": "Mechanism2d", + "/SmartDashboard/transfer/mechanism": "Mechanism2d", + "/SmartDashboard/turret/mechanism": "Mechanism2d", + "/SmartDashboard/turretmotor/mechanism": "Mechanism2d", + "/SmartDashboard/uppershootermotor/mechanism": "Mechanism2d", + "/SmartDashboard/winch_motor/mechanism": "Mechanism2d" }, "windows": { - "/FMSInfo": { + "/AdvantageKit/RealOutputs/PathPlanner": { "window": { "visible": true } }, - "/SmartDashboard/Drive Visual": { + "/SmartDashboard/Auto Modes": { "window": { "visible": true } }, "/SmartDashboard/DrivePoseEstimator/Pose Field": { "CARPET0": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET1": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET10": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET11": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET12": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET13": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET14": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET15": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET16": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET17": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET18": { - "image": ".\\pictures\\carpet.png", "length": 0.4000000059604645, "width": 0.4000000059604645 }, "CARPET19": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET2": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET20": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET21": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET22": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET23": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET24": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET3": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET4": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET5": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET6": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET7": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET8": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "CARPET9": { - "image": ".\\pictures\\carpet.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 1": { - "image": ".\\pictures\\AT1.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 10": { - "image": ".\\pictures\\AT10.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 11": { - "image": ".\\pictures\\AT11.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 12": { - "image": ".\\pictures\\AT12.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 13": { - "image": ".\\pictures\\AT13.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 14": { - "image": ".\\pictures\\AT14.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 15": { - "image": ".\\pictures\\AT15.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 16": { - "image": ".\\pictures\\AT16.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 17": { - "image": ".\\pictures\\AT17.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 18": { - "image": ".\\pictures\\AT18.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 19": { - "image": ".\\pictures\\AT19.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 2": { - "image": ".\\pictures\\AT2.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 20": { - "image": ".\\pictures\\AT20.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 21": { - "image": ".\\pictures\\AT21.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 22": { - "image": ".\\pictures\\AT22.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 23": { - "image": ".\\pictures\\AT23.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 24": { - "image": ".\\pictures\\AT24.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 25": { - "image": ".\\pictures\\AT25.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 26": { - "image": ".\\pictures\\AT26.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 27": { - "image": ".\\pictures\\AT27.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 28": { - "image": ".\\pictures\\AT28.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 29": { - "image": ".\\pictures\\AT29.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 3": { - "image": ".\\pictures\\AT3.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 30": { - "image": ".\\pictures\\AT30.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 31": { - "image": ".\\pictures\\AT31.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 32": { - "image": ".\\pictures\\AT32.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 4": { - "image": ".\\pictures\\AT4.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 5": { - "image": ".\\pictures\\AT5.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 6": { - "image": ".\\pictures\\AT6.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 7": { - "image": ".\\pictures\\AT7.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 8": { - "image": ".\\pictures\\AT8.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, "Field Tag 9": { - "image": ".\\pictures\\AT9.png", "length": 0.44999998807907104, "width": 0.44999998807907104 }, @@ -651,26 +690,271 @@ "length": 0.15000000596046448, "width": 0.15000000596046448 }, + "GPA51": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA52": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA53": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA54": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA55": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA56": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA57": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA58": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA59": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, "GPA6": { "image": ".\\pictures\\gpa.png", "length": 0.15000000596046448, "width": 0.15000000596046448 }, + "GPA60": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA61": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA62": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA63": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA64": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA65": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA66": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA67": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA68": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA69": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, "GPA7": { "image": ".\\pictures\\gpa.png", "length": 0.15000000596046448, "width": 0.15000000596046448 }, + "GPA70": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA71": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA72": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA73": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA74": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA75": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA76": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA77": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA78": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA79": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, "GPA8": { "image": ".\\pictures\\gpa.png", "length": 0.15000000596046448, "width": 0.15000000596046448 }, + "GPA80": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA81": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA82": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA83": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA84": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA85": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA86": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA87": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA88": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA89": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, "GPA9": { "image": ".\\pictures\\gpa.png", "length": 0.15000000596046448, "width": 0.15000000596046448 }, + "GPA90": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA91": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA92": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA93": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA94": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA95": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA96": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA97": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA98": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, + "GPA99": { + "image": ".\\pictures\\gpa.png", + "length": 0.15000000596046448, + "width": 0.15000000596046448 + }, "GPB0": { "length": 0.4000000059604645, "width": 0.4000000059604645 @@ -786,7 +1070,6 @@ ] }, "Robot": { - "image": ".\\pictures\\robot.png", "length": 0.8669999837875366, "width": 0.8669999837875366 }, @@ -811,11 +1094,6 @@ "visible": true } }, - "/SmartDashboard/ExampleRobot/Auto Modes": { - "window": { - "visible": true - } - }, "/SmartDashboard/Field": { "OdometryPose": { "arrowColor": [ @@ -841,14 +1119,7 @@ "left": 245, "right": 3942, "top": 118, - "width": 16.54119300842285 - }, - "/SmartDashboard/Robot Visual": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Shooter/mechanism": { + "width": 16.54119300842285, "window": { "visible": true } @@ -864,7 +1135,22 @@ "visible": true } }, - "/SmartDashboard/hoodmotor/mechanism": { + "/SmartDashboard/flywheel/mechanism": { + "window": { + "visible": true + } + }, + "/SmartDashboard/hood/mechanism": { + "window": { + "visible": true + } + }, + "/SmartDashboard/hopper/mechanism": { + "window": { + "visible": true + } + }, + "/SmartDashboard/turret/mechanism": { "window": { "visible": true } @@ -893,9 +1179,6 @@ }, "transitory": { "AdvantageKit": { - "Drive": { - "open": true - }, "Vision": { "Camera left": { "open": true @@ -932,9 +1215,21 @@ } } }, + "flywheel": { + "open": true + }, + "hopper": { + "hopperMotor": { + "open": true + }, + "open": true + }, "open": true }, "Shuffleboard": { + ".metadata": { + "open": true + }, "ExampleSubsystem": { "percent_motor": { "open": true @@ -953,6 +1248,9 @@ }, "open": true }, + "ClimbCommands": { + "open": true + }, "Drive Feedforward": { "open": true }, @@ -969,9 +1267,33 @@ "open": true } }, + "Indexer": { + "open": true, + "spindexer": { + "open": true + }, + "transfer_back": { + "open": true + }, + "transfer_front": { + "open": true + } + }, "Wheel Radius Characterization": { "open": true }, + "hood": { + "open": true + }, + "hopper": { + "mechanism": { + "hopperRoot": { + "open": true + }, + "open": true + }, + "open": true + }, "open": true }, "Tuning": { @@ -999,5 +1321,16 @@ } } } + }, + "NetworkTables Info": { + "Connections": { + "open": true + }, + "Server": { + "Subscribers": { + "open": true + } + }, + "visible": true } } diff --git a/src/main/deploy/baby_swerve/cameras.json b/src/main/deploy/baby_swerve/cameras.json deleted file mode 100644 index bfdc48b6..00000000 --- a/src/main/deploy/baby_swerve/cameras.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "cameras": [] -} diff --git a/src/main/deploy/baby_swerve/cameras/localization.json b/src/main/deploy/baby_swerve/cameras/localization.json deleted file mode 100644 index 50c5f575..00000000 --- a/src/main/deploy/baby_swerve/cameras/localization.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "name": "two", - "use": "apriltag", - "type": "limelight", - "strategy": "LOWEST_AMBIGUITY", - "column": 0, - "x": 0.16, - "y": -0.1, - "z": 0.335, - "roll": -2, - "pitch": 0, - "yaw": 0 -} diff --git a/src/main/deploy/baby_swerve/controllers.json b/src/main/deploy/baby_swerve/controllers.json deleted file mode 100644 index 901f9478..00000000 --- a/src/main/deploy/baby_swerve/controllers.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "controllers": [ - "driver.json" - ] -} diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json b/src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json deleted file mode 100644 index d77f0e91..00000000 --- a/src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "channel": 0, - "deadband": 0.07, - "invert": true, - "scale": 1.0, - "curvePower": 3, - "limit": 1.0 -} diff --git a/src/main/deploy/baby_swerve/demo_mode.json b/src/main/deploy/baby_swerve/demo_mode.json deleted file mode 100644 index 967ff73b..00000000 --- a/src/main/deploy/baby_swerve/demo_mode.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "maxSpeed": 0.5, - "maxAngularSpeed": 1.5, - "maxAccelleration": 0.5, - "maxAngularAccelleration": 1.5 -} diff --git a/src/main/deploy/baby_swerve/robot.json b/src/main/deploy/baby_swerve/robot.json deleted file mode 100644 index 67726cd4..00000000 --- a/src/main/deploy/baby_swerve/robot.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "userConfig": "demo_mode.json", - "driveType": "YAGSL_SWERVE_DRIVE", - "trackWidth": 7.5, - "trackWidthUom": "in", - "wheelBase": 7.5, - "wheelBaseUom": "in", - "wheelDiameter": 3.1242, - "wheelDiameterUom": "in", - "physicalMaxSpeed": 5.93, - "physicalMaxSpeedUom": "m/s", - "driveMotorGearRatio": 1.0 -} diff --git a/src/main/deploy/baby_swerve/yagsl_drivetrain.json b/src/main/deploy/baby_swerve/yagsl_drivetrain.json deleted file mode 100644 index c75b4305..00000000 --- a/src/main/deploy/baby_swerve/yagsl_drivetrain.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - "directory": "baby_swerve/yagsl_swerve", - "turningMotorGearRatio": 1.0, - "driveModules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ] -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json b/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json deleted file mode 100644 index c5ab6446..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "angleJoystickRadiusDeadband": 0.5, - "heading": { - "p": 0.4, - "i": 0, - "d": 0.01 - } -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json deleted file mode 100644 index 929f3fc9..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "drive": { - "type": "neo", - "id": 3, - "canbus": null - }, - "angle": { - "type": "neo550", - "id": 4, - "canbus": null - }, - "encoder": { - "type": "thrifty", - "id": 1, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": -120.6, - "absoluteEncoderInverted": true, - "location": { - "front": -3.75, - "left": 3.75 - } -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json deleted file mode 100644 index a985764b..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "drive": { - "type": "neo", - "id": 5, - "canbus": null - }, - "angle": { - "type": "neo550", - "id": 6, - "canbus": null - }, - "encoder": { - "type": "thrifty", - "id": 2, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": -166.15, - "absoluteEncoderInverted": true, - "location": { - "front": -3.75, - "left": -3.75 - } -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json deleted file mode 100644 index 517bdf23..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "drive": { - "type": "neo", - "id": 1, - "canbus": null - }, - "angle": { - "type": "neo550", - "id": 2, - "canbus": null - }, - "encoder": { - "type": "thrifty", - "id": 0, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": -165.75, - "absoluteEncoderInverted": true, - "location": { - "front": 3.75, - "left": 3.75 - } -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json deleted file mode 100644 index 7234a3ef..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "drive": { - "type": "neo", - "id": 7, - "canbus": null - }, - "angle": { - "type": "neo550", - "id": 8, - "canbus": null - }, - "encoder": { - "type": "thrifty", - "id": 3, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": true - }, - "absoluteEncoderOffset": -358.5, - "absoluteEncoderInverted": true, - "location": { - "front": 3.75, - "left": -3.75 - } -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json deleted file mode 100644 index 2f0bd03d..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json +++ /dev/null @@ -1,24 +0,0 @@ -{ - "conversionFactors": { - "angle": { - "gearRatio": 55.965, - "factor": 0 - }, - "drive": { - "gearRatio": 5.25, - "diameter": 3.1242, - "factor": 0 - } - }, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "rampRate": { - "drive": 0.1, - "angle": 0.1 - }, - "wheelGripCoefficientOfFriction": 1.19, - "optimalVoltage": 12, - "robotMass": 47.8 -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json b/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json deleted file mode 100644 index a0d528dc..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "drive": { - "p": 0.00057373, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - }, - "angle": { - "p": 0.01125, - "i": 0.000002, - "d": 0, - "f": 0, - "iz": 5 - } -} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json b/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json deleted file mode 100644 index 5fb01c6b..00000000 --- a/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "imu": { - "type": "navx", - "id": 0, - "canbus": null - }, - "invertedIMU": false, - "modules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ] -} diff --git a/src/main/deploy/basic_robot/cameras.json b/src/main/deploy/basic_robot/cameras.json deleted file mode 100644 index ac73ba18..00000000 --- a/src/main/deploy/basic_robot/cameras.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "cameras": [ - "left.json", - "right.json", - "quest.json" - ], - "viewGamePieces": false, - "aprilTagLayout": "2026-rebuilt-andymark.json" -} diff --git a/src/main/deploy/basic_robot/cameras/intake.json b/src/main/deploy/basic_robot/cameras/intake.json deleted file mode 100644 index 33938add..00000000 --- a/src/main/deploy/basic_robot/cameras/intake.json +++ /dev/null @@ -1,13 +0,0 @@ -{ - "name": "intake", - "use": "target", - "type": "limelight", - "column": 0, - "x": 0.15, - "y": 0.05, - "z": 0.2, - "roll": 0, - "pitch": 10, - "yaw": 0, - "targetHeight": 0.02 -} diff --git a/src/main/deploy/basic_robot/cameras/left.json b/src/main/deploy/basic_robot/cameras/left.json deleted file mode 100644 index 0eaef281..00000000 --- a/src/main/deploy/basic_robot/cameras/left.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "name": "left", - "use": "apriltag", - "type": "photonvision", - "column": 0, - "x": -0.33, - "y": 0.33, - "z": 0.5, - "roll": 0, - "pitch": 0, - "yaw": 90 -} diff --git a/src/main/deploy/basic_robot/cameras/localization.json b/src/main/deploy/basic_robot/cameras/localization.json deleted file mode 100644 index 481de59d..00000000 --- a/src/main/deploy/basic_robot/cameras/localization.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "name": "localization", - "use": "apriltag", - "type": "limelight", - "column": 0, - "x": -0.35, - "y": -0.25, - "z": 0.5, - "roll": 0, - "pitch": -20, - "yaw": 180 -} diff --git a/src/main/deploy/basic_robot/cameras/right.json b/src/main/deploy/basic_robot/cameras/right.json deleted file mode 100644 index aed9a771..00000000 --- a/src/main/deploy/basic_robot/cameras/right.json +++ /dev/null @@ -1,12 +0,0 @@ -{ - "name": "right", - "use": "apriltag", - "type": "photonvision", - "column": 0, - "x": -0.33, - "y": -0.33, - "z": 0.5, - "roll": 0, - "pitch": 0, - "yaw": -90 -} diff --git a/src/main/deploy/basic_robot/cameras/shooter.json b/src/main/deploy/basic_robot/cameras/shooter.json deleted file mode 100644 index 9cf3bb35..00000000 --- a/src/main/deploy/basic_robot/cameras/shooter.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "name": "shooter", - "use": "target", - "type": "photonvision", - "column": 0, - "x": 0.25, - "y": 0, - "z": 0.25, - "roll": 0, - "pitch": 10, - "yaw": 0, - "targetFiducialIds": [ - 17, - 18, - 19, - 20, - 21, - 22 - ] -} diff --git a/src/main/deploy/basic_robot/competition_mode.json b/src/main/deploy/basic_robot/competition_mode.json deleted file mode 100644 index f67b949b..00000000 --- a/src/main/deploy/basic_robot/competition_mode.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "maxSpeed": 6.0, - "maxAngularSpeed": 6.0, - "maxAccelleration": 3.0, - "maxAngularAccelleration": 18.75 -} diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json b/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json deleted file mode 100644 index 41b79133..00000000 --- a/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "channel": 2, - "deadband": 0.07, - "invert": true, - "scale": 1.0, - "curvePower": 3, - "limit": 1.0 -} diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_y.json b/src/main/deploy/basic_robot/controllers/axis/driver_left_y.json deleted file mode 100644 index d826731e..00000000 --- a/src/main/deploy/basic_robot/controllers/axis/driver_left_y.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "channel": 1, - "deadband": 0.07, - "invert": true, - "scale": 1.0, - "curvePower": 3.0, - "limit": 1.0 -} diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json b/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json deleted file mode 100644 index 47b46917..00000000 --- a/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "channel": 3, - "deadband": 0.07, - "invert": true, - "scale": 1.0, - "curvePower": 3, - "limit": 1.0 -} diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json b/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json deleted file mode 100644 index 8f981e23..00000000 --- a/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "channel": 4, - "deadband": 0.07, - "invert": true, - "scale": 1.0, - "curvePower": 3, - "limit": 1.0 -} diff --git a/src/main/deploy/basic_robot/controllers/axis/operator_left_y.json b/src/main/deploy/basic_robot/controllers/axis/operator_left_y.json deleted file mode 100644 index c13bd0e0..00000000 --- a/src/main/deploy/basic_robot/controllers/axis/operator_left_y.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "channel": 1, - "deadband": 0.07, - "invert": "true", - "scale": 1.0, - "curvePower": 3.0, - "limit": 1.0, - "rate": 1.0 -} diff --git a/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json b/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json deleted file mode 100644 index 2668956f..00000000 --- a/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "channel": 5, - "deadband": 0.07, - "invert": "true", - "scale": 1.0, - "curvePower": 3.0, - "limit": 1.0, - "rate": 1.0 -} diff --git a/src/main/deploy/basic_robot/controllers/driver.json b/src/main/deploy/basic_robot/controllers/driver.json deleted file mode 100644 index e2dbc76a..00000000 --- a/src/main/deploy/basic_robot/controllers/driver.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "name": "driver", - "port": 0, - "axis": [ - "driver_left_y.json", - "driver_left_x.json", - "driver_right_x.json", - "driver_left_trigger.json", - "driver_right_trigger.json" - ] -} diff --git a/src/main/deploy/basic_robot/controllers/operator.json b/src/main/deploy/basic_robot/controllers/operator.json deleted file mode 100644 index 858cfea8..00000000 --- a/src/main/deploy/basic_robot/controllers/operator.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "name": "operator", - "port": 1, - "axis": [ - "operator_left_y.json", - "operator_right_y.json" - ] -} diff --git a/src/main/deploy/basic_robot/drive_modules/backleft.json b/src/main/deploy/basic_robot/drive_modules/backleft.json deleted file mode 100644 index c62ce932..00000000 --- a/src/main/deploy/basic_robot/drive_modules/backleft.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "s": 0.19542, - "v": 2.2924, - "a": 0.35934 -} diff --git a/src/main/deploy/basic_robot/drive_modules/backright.json b/src/main/deploy/basic_robot/drive_modules/backright.json deleted file mode 100644 index aee7fd95..00000000 --- a/src/main/deploy/basic_robot/drive_modules/backright.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "s": 0.18096, - "v": 2.2915, - "a": 0.37156 -} diff --git a/src/main/deploy/basic_robot/drive_modules/frontleft.json b/src/main/deploy/basic_robot/drive_modules/frontleft.json deleted file mode 100644 index bdf7d3fa..00000000 --- a/src/main/deploy/basic_robot/drive_modules/frontleft.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "s": 0.21848, - "v": 2.3118, - "a": 0.20314 -} diff --git a/src/main/deploy/basic_robot/drive_modules/frontright.json b/src/main/deploy/basic_robot/drive_modules/frontright.json deleted file mode 100644 index 36a1c209..00000000 --- a/src/main/deploy/basic_robot/drive_modules/frontright.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "s": 0.18434, - "v": 2.3018, - "a": 0.30992 -} diff --git a/src/main/deploy/basic_robot/subsystems/example.json b/src/main/deploy/basic_robot/subsystems/example.json deleted file mode 100644 index 91fb54bd..00000000 --- a/src/main/deploy/basic_robot/subsystems/example.json +++ /dev/null @@ -1,24 +0,0 @@ -{ - "devices": [ - { - "device": "percent_motor", - "file": "example/percent_motor.json" - }, - { - "device": "velocity_motor", - "file": "example/velocity_motor.json" - }, - { - "device": "yams_shooter", - "file": "example/yams_shooter.json" - }, - { - "device": "yams_arm", - "file": "example/yams_arm.json" - }, - { - "device": "yams_turret", - "file": "example/yams_pivot.json" - } - ] -} diff --git a/src/main/deploy/basic_robot/subsystems/example/velocity_motor.json b/src/main/deploy/basic_robot/subsystems/example/velocity_motor.json deleted file mode 100644 index 185de2e0..00000000 --- a/src/main/deploy/basic_robot/subsystems/example/velocity_motor.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "name": "velocity_motor", - "controller": "spark", - "type": "Neo", - "id": 14, - "gearing": 1.0, - "momentOfInertiaKgMSq": 1.0, - "x": -0.25, - "y": 0.0, - "z": 0.25, - "kP": 0.1, - "kI": 0.0, - "kD": 0.01, - "iZone": 0.0, - "kS": 0.1, - "kV": 0.1, - "kA": 0.0 -} diff --git a/src/main/deploy/music/mariachi.chrp b/src/main/deploy/music/mariachi.chrp new file mode 100644 index 00000000..a7c16056 Binary files /dev/null and b/src/main/deploy/music/mariachi.chrp differ diff --git a/src/main/deploy/music/raiders.chrp b/src/main/deploy/music/raiders.chrp new file mode 100644 index 00000000..7103ee9c Binary files /dev/null and b/src/main/deploy/music/raiders.chrp differ diff --git a/src/main/deploy/music/sea2.chrp b/src/main/deploy/music/sea2.chrp new file mode 100644 index 00000000..e0f1a421 Binary files /dev/null and b/src/main/deploy/music/sea2.chrp differ diff --git a/src/main/deploy/pathplanner/autos/2X-TL-CTR-BUMP-DEP.auto b/src/main/deploy/pathplanner/autos/2X-TL-CTR-BUMP-DEP.auto new file mode 100644 index 00000000..fa784b01 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2X-TL-CTR-BUMP-DEP.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-CTLD" + } + }, + { + "type": "path", + "data": { + "pathName": "CTLD-BLA" + } + }, + { + "type": "path", + "data": { + "pathName": "BLA-TLS" + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TL-QTRH" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRLD-BLA" + } + }, + { + "type": "path", + "data": { + "pathName": "BLA-DEP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Left", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2X-TR-CTR-BUMP-DEP.auto b/src/main/deploy/pathplanner/autos/2X-TR-CTR-BUMP-DEP.auto new file mode 100644 index 00000000..ab2e3ba9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2X-TR-CTR-BUMP-DEP.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTRD" + } + }, + { + "type": "path", + "data": { + "pathName": "CTRD-BA" + } + }, + { + "type": "path", + "data": { + "pathName": "BA-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTRLong" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-BUMP" + } + }, + { + "type": "path", + "data": { + "pathName": "BA-DEP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-BL-MS-DEP-MS.auto b/src/main/deploy/pathplanner/autos/A-BL-MS-DEP-MS.auto new file mode 100644 index 00000000..27591f90 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-BL-MS-DEP-MS.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "BL-DEP" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "DEP-CL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-BL-SS-DEP-SS.auto b/src/main/deploy/pathplanner/autos/A-BL-SS-DEP-SS.auto new file mode 100644 index 00000000..9499cafc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-BL-SS-DEP-SS.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "BL-DEP" + } + }, + { + "type": "path", + "data": { + "pathName": "DEP-CL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-BR-MS-HP-MS.auto b/src/main/deploy/pathplanner/autos/A-BR-MS-HP-MS.auto new file mode 100644 index 00000000..eb140426 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-BR-MS-HP-MS.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "path", + "data": { + "pathName": "BR-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HP-CR" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-BR-SS-HP-SS-Tested.auto b/src/main/deploy/pathplanner/autos/A-BR-SS-HP-SS-Tested.auto new file mode 100644 index 00000000..2e0f7aa9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-BR-SS-HP-SS-Tested.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "BR-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-CR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-C-CL-SS-Tested.auto b/src/main/deploy/pathplanner/autos/A-C-CL-SS-Tested.auto new file mode 100644 index 00000000..4e2b2c27 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-C-CL-SS-Tested.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "C-CL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Center", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-C-CR-SS-Tested.auto b/src/main/deploy/pathplanner/autos/A-C-CR-SS-Tested.auto new file mode 100644 index 00000000..ba1210c8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-C-CR-SS-Tested.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "C-CR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Center", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-C-MS-TWR-MS.auto b/src/main/deploy/pathplanner/autos/A-C-MS-TWR-MS.auto new file mode 100644 index 00000000..5364d3e3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-C-MS-TWR-MS.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C-TWR" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Center", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-HP-MS-DEP-MS.auto b/src/main/deploy/pathplanner/autos/A-HP-MS-DEP-MS.auto new file mode 100644 index 00000000..b306d7c7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-HP-MS-DEP-MS.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BR-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "path", + "data": { + "pathName": "HP-DEP" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-TL-MS-DEP-MS.auto b/src/main/deploy/pathplanner/autos/A-TL-MS-DEP-MS.auto new file mode 100644 index 00000000..2e7710f8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-TL-MS-DEP-MS.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "TL-Dep" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "DEP-CL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-TL-SS-DEP-SS.auto b/src/main/deploy/pathplanner/autos/A-TL-SS-DEP-SS.auto new file mode 100644 index 00000000..71c3c16f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-TL-SS-DEP-SS.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "TL-Dep" + } + }, + { + "type": "path", + "data": { + "pathName": "DEP-CL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-TR-CTRQ-HP.auto b/src/main/deploy/pathplanner/autos/A-TR-CTRQ-HP.auto new file mode 100644 index 00000000..1b448e61 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-TR-CTRQ-HP.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-HP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-TR-MS-HP-MS.auto b/src/main/deploy/pathplanner/autos/A-TR-MS-HP-MS.auto new file mode 100644 index 00000000..72fe7539 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-TR-MS-HP-MS.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-CR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A-TR-SS-HP-SS.auto b/src/main/deploy/pathplanner/autos/A-TR-SS-HP-SS.auto new file mode 100644 index 00000000..c7066dff --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-TR-SS-HP-SS.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "TR-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-CR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AllianceSide", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/BL-DEP-TWRFRONT-HP.auto similarity index 78% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/BL-DEP-TWRFRONT-HP.auto index 268147bb..ea5531f2 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/BL-DEP-TWRFRONT-HP.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "BL-DEP-TWRFRONT-HP" } } ] } }, "resetOdom": true, - "folder": null, + "folder": "Worlds", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BR-HP-DEP-Wall.auto b/src/main/deploy/pathplanner/autos/BR-HP-DEP-Wall.auto new file mode 100644 index 00000000..45d46c93 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BR-HP-DEP-Wall.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BRS-HP" + } + }, + { + "type": "path", + "data": { + "pathName": "HP-TWROUT-DEP-FAR" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BRS-HP-DEP-TWR-HP.auto b/src/main/deploy/pathplanner/autos/BRS-HP-DEP-TWR-HP.auto new file mode 100644 index 00000000..3e728d44 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BRS-HP-DEP-TWR-HP.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BRS-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-TWROUT-DEP-HP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BRS-HP-DEP-TWR.auto b/src/main/deploy/pathplanner/autos/BRS-HP-DEP-TWR.auto new file mode 100644 index 00000000..c1ed82ca --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BRS-HP-DEP-TWR.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BRS-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-TWROUT-DEP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BumpRight to wall.auto b/src/main/deploy/pathplanner/autos/BumpRight to wall.auto new file mode 100644 index 00000000..44edfe6e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BumpRight to wall.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BRS-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-TWROUT-DEP-FAR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Churn-Orbit Right 2 Swipe no HP.auto b/src/main/deploy/pathplanner/autos/Churn-Orbit Right 2 Swipe no HP.auto new file mode 100644 index 00000000..bc540b3b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Churn-Orbit Right 2 Swipe no HP.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTRAngled" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TRBack" + } + }, + { + "type": "named", + "data": { + "name": "indexerChurn" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TRBack-CTR-HALF" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRH-TRBack" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Orbit", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Delay Trench Neutral Bump HP.auto b/src/main/deploy/pathplanner/autos/Delay Trench Neutral Bump HP.auto new file mode 100644 index 00000000..b2a979d5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Delay Trench Neutral Bump HP.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "StartTR-CTR-HLF-BR-HP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Disrupt.auto b/src/main/deploy/pathplanner/autos/Disrupt.auto new file mode 100644 index 00000000..4df5fe7f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Disrupt.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RightDisrupt1" + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "WaitUntilIntaking" + } + }, + { + "type": "path", + "data": { + "pathName": "Disrupter2" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Left Bump Depot middle end.auto b/src/main/deploy/pathplanner/autos/Follow Left Bump Depot middle end.auto new file mode 100644 index 00000000..63440962 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Left Bump Depot middle end.auto @@ -0,0 +1,67 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.3 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TLBack-CTL-QTL-BL-L" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Depot end to middle left" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Left Bump Depot.auto b/src/main/deploy/pathplanner/autos/Follow Left Bump Depot.auto new file mode 100644 index 00000000..e2ebd041 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Left Bump Depot.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.3 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TLBack-CTL-QTL-BL-L" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Left Right Bump HP.auto b/src/main/deploy/pathplanner/autos/Follow Left Right Bump HP.auto new file mode 100644 index 00000000..2a7ac159 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Left Right Bump HP.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "Delay TL- Wait TL" + } + }, + { + "type": "path", + "data": { + "pathName": "Wait-TL-BR-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Left Trench .auto b/src/main/deploy/pathplanner/autos/Follow Left Trench .auto new file mode 100644 index 00000000..8e766831 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Left Trench .auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TLback-CTL-QTL" + } + }, + { + "type": "path", + "data": { + "pathName": "QTLong-TL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Right Bump HP.auto b/src/main/deploy/pathplanner/autos/Follow Right Bump HP.auto new file mode 100644 index 00000000..c80294d9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Right Bump HP.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.5 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "DelayTRS-CTR-QTR-BR-HP Longer" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Right Left Bumpt Depot.auto b/src/main/deploy/pathplanner/autos/Follow Right Left Bumpt Depot.auto new file mode 100644 index 00000000..f1f7c577 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Right Left Bumpt Depot.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Delay TR- Wait TR" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Wait-TR-BL-Depot" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Follow Right Trench HP.auto b/src/main/deploy/pathplanner/autos/Follow Right Trench HP.auto new file mode 100644 index 00000000..b0378a25 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Follow Right Trench HP.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "DelayTR-QTRL" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRLeft-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2056 double HP.auto b/src/main/deploy/pathplanner/autos/Left 2056 double HP.auto new file mode 100644 index 00000000..ccd5acb9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2056 double HP.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-CTR-QTL-BL-TL" + } + }, + { + "type": "path", + "data": { + "pathName": "TL-CTR-HLF-BL-HP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 5010 Double.auto b/src/main/deploy/pathplanner/autos/Left 5010 Double.auto new file mode 100644 index 00000000..6743ebef --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 5010 Double.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-CTL-QTL" + } + }, + { + "type": "path", + "data": { + "pathName": "QTL-TL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TL-CTL-QTL-BL-L" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left-3Shuttle-HPC.auto b/src/main/deploy/pathplanner/autos/Left-3Shuttle-HPC.auto new file mode 100644 index 00000000..359c70f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left-3Shuttle-HPC.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-QTLCTLSHORT" + } + }, + { + "type": "path", + "data": { + "pathName": "CTL-NWALL" + } + }, + { + "type": "path", + "data": { + "pathName": "Left3rdSwipe" + } + }, + { + "type": "path", + "data": { + "pathName": "LeftNwall-Tower" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Orbit Left 1Swipe.auto b/src/main/deploy/pathplanner/autos/Orbit Left 1Swipe.auto new file mode 100644 index 00000000..81d0109a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Orbit Left 1Swipe.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-QTRHLong" + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRLong-TL" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Left", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Orbit Left.auto b/src/main/deploy/pathplanner/autos/Orbit Left.auto new file mode 100644 index 00000000..7790de27 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Orbit Left.auto @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-QTRH" + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRLong-TL" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TL-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRL-MID" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Left", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Orbit Right 1Swipe.auto b/src/main/deploy/pathplanner/autos/Orbit Right 1Swipe.auto new file mode 100644 index 00000000..2880825c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Orbit Right 1Swipe.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTRLong" + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRLong-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Orbit", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Orbit Right 2 Swipe no HP.auto b/src/main/deploy/pathplanner/autos/Orbit Right 2 Swipe no HP.auto new file mode 100644 index 00000000..a4ca4b2a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Orbit Right 2 Swipe no HP.auto @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTRAngled" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TRBack" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TRBack-CTR-HALF" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRH-TRBack" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TRBack-CTR-HALF" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Orbit Right.auto b/src/main/deploy/pathplanner/autos/Orbit Right.auto new file mode 100644 index 00000000..c85140cf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Orbit Right.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-HALF" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRH-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Quals 110.auto b/src/main/deploy/pathplanner/autos/Quals 110.auto new file mode 100644 index 00000000..cae3ec6d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Quals 110.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TLback-CTL-QTL" + } + }, + { + "type": "path", + "data": { + "pathName": "QTLong-TLHP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Quals 73.auto b/src/main/deploy/pathplanner/autos/Quals 73.auto new file mode 100644 index 00000000..177a247b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Quals 73.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "DelayTR-QTRL" + } + }, + { + "type": "path", + "data": { + "pathName": "QBRight-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2056 double HP.auto b/src/main/deploy/pathplanner/autos/Right 2056 double HP.auto new file mode 100644 index 00000000..85462f72 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2056 double HP.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR-BR-TR" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-HLF-BR-HP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 5010 Double Old.auto b/src/main/deploy/pathplanner/autos/Right 5010 Double Old.auto new file mode 100644 index 00000000..b9a3bfcc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 5010 Double Old.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TRSide-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.5 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR-BR-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 5010 Double Optimized.auto b/src/main/deploy/pathplanner/autos/Right 5010 Double Optimized.auto new file mode 100644 index 00000000..1192d11e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 5010 Double Optimized.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TRSide-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR-BR-HP Longer" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 5010 Double Short.auto b/src/main/deploy/pathplanner/autos/Right 5010 Double Short.auto new file mode 100644 index 00000000..883fb42c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 5010 Double Short.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTRShort" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRShort-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR-BR-HP" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 5010 Double.auto b/src/main/deploy/pathplanner/autos/Right 5010 Double.auto new file mode 100644 index 00000000..2e5dc557 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 5010 Double.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TRSide-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR-BR-HP Longer" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right-3Shuttle-HPC.auto b/src/main/deploy/pathplanner/autos/Right-3Shuttle-HPC.auto new file mode 100644 index 00000000..8bb06208 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right-3Shuttle-HPC.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TR-QTRCTRSHORT" + } + }, + { + "type": "path", + "data": { + "pathName": "CTR-NWALL" + } + }, + { + "type": "path", + "data": { + "pathName": "3RD-SWIPE" + } + }, + { + "type": "path", + "data": { + "pathName": "NWALL-CLIMB" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Worlds", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TL-DIS-NWALL-DEP.auto b/src/main/deploy/pathplanner/autos/TL-DIS-NWALL-DEP.auto new file mode 100644 index 00000000..ac3634ba --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TL-DIS-NWALL-DEP.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TL-DIS" + } + }, + { + "type": "path", + "data": { + "pathName": "NWALL-DEP" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Left", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TL-QTR-SS-QTRH-DEP-SS.auto b/src/main/deploy/pathplanner/autos/TL-QTR-SS-QTRH-DEP-SS.auto new file mode 100644 index 00000000..ebf76766 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TL-QTR-SS-QTRH-DEP-SS.auto @@ -0,0 +1,74 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "TL-QTRH" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRLong-TL" + } + }, + { + "type": "path", + "data": { + "pathName": "TL-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRL-TL" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Left", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TR-CTR-SS-CTRH-MS-HP-SS.auto b/src/main/deploy/pathplanner/autos/TR-CTR-SS-CTRH-MS-HP-SS.auto new file mode 100644 index 00000000..aa6de1b3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TR-CTR-SS-CTRH-MS-HP-SS.auto @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-QTR" + } + }, + { + "type": "path", + "data": { + "pathName": "QTR-TR" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "launcherLow" + } + }, + { + "type": "path", + "data": { + "pathName": "TR-CTR-HALF" + } + }, + { + "type": "path", + "data": { + "pathName": "QTRH-HP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HP-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 6e83d807..660ca524 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,9 +1,9 @@ { "field_size": { - "x": 15.24, - "y": 8.25 + "x": 16.54, + "y": 8.07 }, - "nodeSizeMeters": 0.25, + "nodeSizeMeters": 0.3, "grid": [ [ true, @@ -28,47 +28,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ true, true, true, @@ -91,45 +50,18 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true ], [ true, @@ -154,47 +86,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ true, true, true, @@ -215,6 +106,22 @@ true, true, true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ true, true, false, @@ -255,15 +162,6 @@ false, false, false, - false - ], - [ - true, - false, - false, - false, - false, - false, false, false, false, @@ -279,7 +177,12 @@ false, false, true, + true + ], + [ true, + true, + false, false, false, false, @@ -289,6 +192,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -307,6 +217,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -318,9 +234,11 @@ false, false, false, - false + true, + true ], [ + true, true, false, false, @@ -332,6 +250,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -341,9 +266,6 @@ false, false, false, - true, - true, - false, false, false, false, @@ -353,6 +275,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -363,6 +291,13 @@ false, false, false, + true, + true, + true + ], + [ + true, + true, false, false, false, @@ -373,6 +308,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -381,10 +323,6 @@ false, false, false, - false - ], - [ - true, false, false, false, @@ -395,6 +333,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -404,6 +348,12 @@ false, false, false, + true, + true, + true, + true + ], + [ true, true, false, @@ -444,12 +394,6 @@ false, false, false, - false - ], - [ - true, - false, - false, false, false, false, @@ -462,13 +406,19 @@ false, false, false, + true, + true, + true, + true + ], + [ + true, + true, false, false, false, false, false, - true, - true, false, false, false, @@ -507,10 +457,6 @@ false, false, false, - false - ], - [ - true, false, false, false, @@ -518,6 +464,14 @@ false, false, false, + true, + true, + true, + true + ], + [ + true, + true, false, false, false, @@ -530,8 +484,6 @@ false, false, false, - true, - true, false, false, false, @@ -570,9 +522,16 @@ false, false, false, - false + true, + true, + true, + true ], [ + true, + true, + true, + true, true, false, false, @@ -581,597 +540,13 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1190,6 +565,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1200,15 +581,32 @@ false, false, false, - false + true, + true, + true ], [ + true, + true, + true, + true, + true, true, false, false, false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, + false, false, false, false, @@ -1223,6 +621,13 @@ false, false, false, + false, + true, + true, + true, + true, + true, + true, true, true, false, @@ -1235,11 +640,30 @@ false, false, false, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, false, false, false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1256,6 +680,14 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1263,15 +695,33 @@ false, false, false, - false + true, + true, + true, + true, + true ], [ + true, + true, + true, + true, + true, true, false, false, false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1286,24 +736,50 @@ false, false, false, - true, - true, - false, - false, - false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, false, false, false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, false, false, false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1320,21 +796,48 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, false, false, false, - false + true, + true, + true, + true, + true, + true ], [ + true, + true, + true, + true, + true, true, false, false, false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1349,24 +852,50 @@ false, false, false, - true, - true, - false, - false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, false, false, false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, false, false, false, false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1383,15 +912,29 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, false, false, false, - false + true, + true, + true, + true, + true, + true ], [ + true, true, false, false, @@ -1402,18 +945,16 @@ false, false, false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, true, true, + true, + true, + true, + true, + true, + true, + true, + false, false, false, false, @@ -1429,12 +970,30 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + true, false, false, false, false, false, false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, false, false, false, @@ -1445,6 +1004,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1452,10 +1018,6 @@ false, false, false, - false - ], - [ - true, false, false, false, @@ -1467,6 +1029,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1477,6 +1045,15 @@ false, true, true, + true, + true, + true + ], + [ + true, + true, + true, + false, false, false, false, @@ -1515,10 +1092,6 @@ false, false, false, - false - ], - [ - true, false, false, false, @@ -1531,6 +1104,13 @@ false, false, false, + true, + true + ], + [ + true, + true, + true, false, false, false, @@ -1538,8 +1118,6 @@ false, false, false, - true, - true, false, false, false, @@ -1578,16 +1156,19 @@ false, false, false, - false - ], - [ - true, false, false, false, false, false, false, + true, + true + ], + [ + true, + true, + true, false, false, false, @@ -1601,8 +1182,6 @@ false, false, false, - true, - true, false, false, false, @@ -1641,9 +1220,12 @@ false, false, false, - false + true, + true ], [ + true, + true, true, false, false, @@ -1654,6 +1236,14 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + false, false, false, false, @@ -1664,8 +1254,6 @@ false, false, false, - true, - true, false, false, false, @@ -1673,6 +1261,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1684,6 +1278,12 @@ false, false, false, + true, + true + ], + [ + true, + true, false, false, false, @@ -1694,6 +1294,13 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1704,10 +1311,6 @@ false, false, false, - false - ], - [ - true, false, false, false, @@ -1716,6 +1319,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1727,6 +1336,10 @@ false, false, false, + true, + true + ], + [ true, true, false, @@ -1739,6 +1352,14 @@ false, false, false, + true, + true, + true, + true, + true, + true, + true, + false, false, false, false, @@ -1756,6 +1377,12 @@ false, false, false, + true, + true, + true, + true, + true, + true, false, false, false, @@ -1767,9 +1394,11 @@ false, false, false, - false + true, + true ], [ + true, true, false, false, @@ -1790,13 +1419,6 @@ false, false, false, - true, - true, - false, - false, - false, - false, - false, false, false, false, @@ -1830,7 +1452,8 @@ false, false, false, - false + true, + true ], [ true, @@ -1855,47 +1478,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ true, true, true, @@ -1918,45 +1500,18 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true ], [ true, @@ -1981,47 +1536,6 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false - ], - [ true, true, true, @@ -2044,45 +1558,18 @@ true, true, true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true ] ] } diff --git a/src/main/deploy/pathplanner/paths/3RD-SWIPE.path b/src/main/deploy/pathplanner/paths/3RD-SWIPE.path new file mode 100644 index 00000000..97ecd29d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3RD-SWIPE.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.168877777777778, + "y": 1.2968111111111114 + }, + "prevControl": null, + "nextControl": { + "x": 6.779100000000001, + "y": 0.6829111111111106 + }, + "isLocked": false, + "linkedName": "CTR/NWALL" + }, + { + "anchor": { + "x": 5.941077777777777, + "y": 1.8619888888888885 + }, + "prevControl": { + "x": 6.111548415940352, + "y": 1.1173013642839542 + }, + "nextControl": { + "x": 5.385644444444446, + "y": 4.2883555555555555 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7535444444444455, + "y": 1.2968111111111114 + }, + "prevControl": { + "x": 8.143322222222222, + "y": 5.292077777777777 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "YHYRight" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0307053941908686, + "rotationDegrees": 89.1641830798336 + }, + { + "waypointRelativePos": 1.8207468879668083, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8764568764568764, + "maxWaypointRelativePos": 1.9020979020979023, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.78099970237523 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BA-DEP.path b/src/main/deploy/pathplanner/paths/BA-DEP.path new file mode 100644 index 00000000..900a2d80 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BA-DEP.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.493756708407871, + "y": 2.517924865831842 + }, + "prevControl": null, + "nextControl": { + "x": 3.493756708407871, + "y": 2.5179248658318425 + }, + "isLocked": false, + "linkedName": "BUMP-AFTER" + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 2.6789570230833886, + "y": 0.6682289803220032 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BA-TR.path b/src/main/deploy/pathplanner/paths/BA-TR.path new file mode 100644 index 00000000..b4f4d5ca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BA-TR.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.493756708407871, + "y": 2.517924865831842 + }, + "prevControl": null, + "nextControl": { + "x": 1.8771914132379246, + "y": 1.6093023255813943 + }, + "isLocked": false, + "linkedName": "BUMP-AFTER" + }, + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": { + "x": 2.5342000000000002, + "y": 0.6147000000000004 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "trsj" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0.010169491525423728, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BL-DEP-TWRFRONT-HP.path b/src/main/deploy/pathplanner/paths/BL-DEP-TWRFRONT-HP.path new file mode 100644 index 00000000..91464918 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BL-DEP-TWRFRONT-HP.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5700909090909088, + "y": 6.034522727272727 + }, + "prevControl": null, + "nextControl": { + "x": 2.9199926137123215, + "y": 7.293725422963357 + }, + "isLocked": false, + "linkedName": "BLStart" + }, + { + "anchor": { + "x": 0.9624659090909089, + "y": 7.013670454545454 + }, + "prevControl": { + "x": 1.292284090909091, + "y": 7.559931818181818 + }, + "nextControl": { + "x": 0.6769219369520476, + "y": 6.540738250690465 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5293409090909087, + "y": 5.158443181818182 + }, + "prevControl": { + "x": 0.08016767676767644, + "y": 5.5002249999999995 + }, + "nextControl": { + "x": 2.478268565479457, + "y": 4.934642319029992 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 1.774433333333333, + "y": 1.927666481304656 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.05, + "rotationDegrees": -117.24145121408237 + }, + { + "waypointRelativePos": 2.021220159151191, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.03, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 0.348387096774195, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Center", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BL-DEP-TWRIN-HP.path b/src/main/deploy/pathplanner/paths/BL-DEP-TWRIN-HP.path new file mode 100644 index 00000000..1c1c9c1e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BL-DEP-TWRIN-HP.path @@ -0,0 +1,132 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5700909090909088, + "y": 6.034522727272727 + }, + "prevControl": null, + "nextControl": { + "x": 2.9199926137123215, + "y": 7.293725422963357 + }, + "isLocked": false, + "linkedName": "BLStart" + }, + { + "anchor": { + "x": 0.8593977272691555, + "y": 7.0033636363607625 + }, + "prevControl": { + "x": 1.061755878976238, + "y": 7.704871895624426 + }, + "nextControl": { + "x": 0.7047954545454542, + "y": 6.467409090909091 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.51, + "y": 4.201011184841528 + }, + "prevControl": { + "x": 0.51, + "y": 4.451011184841528 + }, + "nextControl": { + "x": 0.51, + "y": 3.746407914778758 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.51, + "y": 0.7249755535012343 + }, + "prevControl": { + "x": 0.5089659090909088, + "y": 1.7056590909090916 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.05, + "rotationDegrees": -117.24145121408237 + }, + { + "waypointRelativePos": 2.021220159151191, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 2.402214022140218, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Center", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BL-DEP.path b/src/main/deploy/pathplanner/paths/BL-DEP.path new file mode 100644 index 00000000..4e4d7fa8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BL-DEP.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5880826799714898, + "y": 6.5478581610833935 + }, + "prevControl": null, + "nextControl": { + "x": 2.275751960085531, + "y": 6.983342836778333 + }, + "isLocked": false, + "linkedName": "BLS" + }, + { + "anchor": { + "x": 0.8719949494949504, + "y": 7.321729797979798 + }, + "prevControl": { + "x": 1.7814694250807417, + "y": 7.7374895582475895 + }, + "nextControl": { + "x": 0.4344325831371458, + "y": 7.121701287644801 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7231186868686872, + "y": 5.523762626262627 + }, + "prevControl": { + "x": 0.6773106060606064, + "y": 6.1536237373737395 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DEPOT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3038596491228112, + "rotationDegrees": -104.27045396689256 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -114.56717132151371 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.88820165166617 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BL-NW.path b/src/main/deploy/pathplanner/paths/BL-NW.path new file mode 100644 index 00000000..3030c39d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BL-NW.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6090277777777784, + "y": 6.073459595959596 + }, + "prevControl": null, + "nextControl": { + "x": 3.1464618476280997, + "y": 7.023595020050829 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.5757234497505355, + "y": 7.535498930862438 + }, + "prevControl": { + "x": 3.5757234497505355, + "y": 7.535498930862438 + }, + "nextControl": { + "x": 5.5757234497505355, + "y": 7.535498930862438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.200955096222382, + "y": 7.06043121881682 + }, + "prevControl": { + "x": 5.88433620109375, + "y": 7.44740986841848 + }, + "nextControl": { + "x": 6.538503207412688, + "y": 6.647872416250891 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3259729151817545, + "y": 1.6096543121881677 + }, + "prevControl": { + "x": 6.346060606060607, + "y": 2.660757575757576 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.593684210526313, + "rotationDegrees": 179.14780805275348 + }, + { + "waypointRelativePos": 2.153684210526314, + "rotationDegrees": -89.05422605441628 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.07082445478696 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.49123935164835 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BLA-DEP.path b/src/main/deploy/pathplanner/paths/BLA-DEP.path new file mode 100644 index 00000000..fd5270d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BLA-DEP.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6560107334525935, + "y": 5.649427549194991 + }, + "prevControl": null, + "nextControl": { + "x": 2.428855098389982, + "y": 6.57427549194991 + }, + "isLocked": false, + "linkedName": "BLA" + }, + { + "anchor": { + "x": 2.0069946332737025, + "y": 6.9312343470483 + }, + "prevControl": { + "x": 2.211802613403463, + "y": 6.787868760957469 + }, + "nextControl": { + "x": 1.6723013677124379, + "y": 7.165519632941184 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9847942754919494, + "y": 7.077262969588551 + }, + "prevControl": { + "x": 1.1632737030411446, + "y": 7.385545617173523 + }, + "nextControl": { + "x": 0.8548002027622305, + "y": 6.852727753055401 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7231186868686872, + "y": 5.523762626262627 + }, + "prevControl": { + "x": 0.6765116279069764, + "y": 6.606726296958855 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DEPOT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.013368983957223, + "rotationDegrees": -110.4131318098436 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.5254237288135593, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -114.56717132151371 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BLA-TLS.path b/src/main/deploy/pathplanner/paths/BLA-TLS.path new file mode 100644 index 00000000..a62da52c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BLA-TLS.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.6560107334525935, + "y": 5.649427549194991 + }, + "prevControl": null, + "nextControl": { + "x": 1.6500357781753126, + "y": 7.207066189624329 + }, + "isLocked": false, + "linkedName": "BLA" + }, + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 3.3578795180722896, + "y": 7.411662650602409 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TLS" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/BR-HP.path similarity index 55% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/BR-HP.path index 38908444..68d43feb 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/BR-HP.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 8.031762295081966, - "y": 6.728227459016393 + "x": 3.6113181818181816, + "y": 1.6438181818181818 }, "prevControl": null, "nextControl": { - "x": 6.677151639344262, - "y": 6.728227459016393 + "x": 3.0612397783969407, + "y": 1.4937967990669336 }, "isLocked": false, - "linkedName": null + "linkedName": "BR" }, { "anchor": { - "x": 4.207684426229508, - "y": 6.88406762295082 + "x": 2.3003991446899503, + "y": 1.0595759087669274 }, "prevControl": { - "x": 5.3096147111058976, - "y": 7.435032765389015 + "x": 3.2582254299110964, + "y": 1.346923794333272 }, "nextControl": { - "x": 3.8960040983606556, - "y": 6.728227459016393 + "x": 1.3002565930149685, + "y": 0.7595331432644322 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.812090163934426, - "y": 5.217776639344262 + "x": 0.475, + "y": 0.8003975903614469 }, "prevControl": { - "x": 3.57233606557377, - "y": 6.728227459016393 + "x": 1.6751710620099787, + "y": 0.8254011541533226 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "HP" } ], "rotationTargets": [], @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 9.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -63.43494882292201 + "rotation": -90.0 }, "reversed": false, - "folder": null, + "folder": "Right", "idealStartingState": { "velocity": 0, - "rotation": -179.06080905426444 + "rotation": -89.48708452392692 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BR-NW.path b/src/main/deploy/pathplanner/paths/BR-NW.path new file mode 100644 index 00000000..1e7b60c9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BR-NW.path @@ -0,0 +1,95 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.613086243763365, + "y": 1.3971240199572332 + }, + "prevControl": null, + "nextControl": { + "x": 3.2505345687811835, + "y": 0.7095260156806842 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.350691375623665, + "y": 0.609511760513185 + }, + "prevControl": { + "x": 3.350691375623665, + "y": 0.6095117605131851 + }, + "nextControl": { + "x": 5.350691375623665, + "y": 0.609511760513185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.254444444444443, + "y": 0.9200505050505046 + }, + "prevControl": { + "x": 5.891898264931644, + "y": 0.5848663013499928 + }, + "nextControl": { + "x": 6.91703888492912, + "y": 1.5326378179514304 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.388481824661441, + "y": 6.2228118317890235 + }, + "prevControl": { + "x": 6.138481824661441, + "y": 6.2228118317890235 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.669473684210526, + "rotationDegrees": -179.96309845746404 + }, + { + "waypointRelativePos": 2.120000000000005, + "rotationDegrees": 90.31195699936117 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.12212255271466 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.70508837257486 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BRS-HP.path b/src/main/deploy/pathplanner/paths/BRS-HP.path new file mode 100644 index 00000000..923ac883 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BRS-HP.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6024111111111115, + "y": 2.056877777777778 + }, + "prevControl": null, + "nextControl": { + "x": 2.880933838380322, + "y": 1.76828686868184 + }, + "isLocked": false, + "linkedName": "BRSide" + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 1.3025909090873924, + "y": 1.1697045454495163 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.11414392059553939, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.20296296296296304, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 0.4162962962962971, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C-CL.path b/src/main/deploy/pathplanner/paths/C-CL.path new file mode 100644 index 00000000..cbf840e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C-CL.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5505773342836786, + "y": 3.997494654312188 + }, + "prevControl": null, + "nextControl": { + "x": 3.250534568774933, + "y": 4.12251247327156 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.337904490371512, + "y": 4.9601318602993585 + }, + "prevControl": { + "x": 3.1005131860236856, + "y": 4.1850213827512475 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CL" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -16.521034797124635 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 92.78713602149966 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C-CR.path b/src/main/deploy/pathplanner/paths/C-CR.path new file mode 100644 index 00000000..1b765f5a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C-CR.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.574671717171718, + "y": 3.977739898989899 + }, + "prevControl": null, + "nextControl": { + "x": 2.9690222222222222, + "y": 3.4308444444444435 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2381888888888892, + "y": 2.8754111111111103 + }, + "prevControl": { + "x": 3.553688888888889, + "y": 3.9765333333333333 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 26.5650511770781 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C-TWR.path b/src/main/deploy/pathplanner/paths/C-TWR.path new file mode 100644 index 00000000..55909093 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C-TWR.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5975757575757576, + "y": 4.035 + }, + "prevControl": null, + "nextControl": { + "x": 2.622436769692649, + "y": 3.997494654312188 + }, + "isLocked": false, + "linkedName": "C" + }, + { + "anchor": { + "x": 2.062865288667142, + "y": 4.035 + }, + "prevControl": { + "x": 3.0880114041339994, + "y": 3.997494654312188 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.2318158048334027 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CR-DEP.path b/src/main/deploy/pathplanner/paths/CR-DEP.path new file mode 100644 index 00000000..971ccb92 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CR-DEP.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.038, + "y": 2.81 + }, + "prevControl": null, + "nextControl": { + "x": 2.2881096445968256, + "y": 3.285199080049246 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8376388888966055, + "y": 7.367537878793638 + }, + "prevControl": { + "x": 1.7661155797818637, + "y": 7.73892855514774 + }, + "nextControl": { + "x": 0.6055197161752905, + "y": 7.2746902097051125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7231186868686872, + "y": 5.523762626262627 + }, + "prevControl": { + "x": 0.7917853535430703, + "y": 6.26785101010677 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DEPOT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.4217543859649109, + "rotationDegrees": -115.20021424448595 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -114.56717132151371 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CTL-NWALL.path b/src/main/deploy/pathplanner/paths/CTL-NWALL.path new file mode 100644 index 00000000..6383fba2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CTL-NWALL.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.752453637660485, + "y": 4.882482168330956 + }, + "prevControl": null, + "nextControl": { + "x": 7.091639351946198, + "y": 4.7529107397595265 + }, + "isLocked": false, + "linkedName": "CTL-QTLShort" + }, + { + "anchor": { + "x": 6.642677777777777, + "y": 4.882482168330956 + }, + "prevControl": { + "x": 6.778924865426631, + "y": 4.672871264255797 + }, + "nextControl": { + "x": 6.4742349206349195, + "y": 5.141625025473813 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.168877777777778, + "y": 6.773188888888889 + }, + "prevControl": { + "x": 6.915522222222223, + "y": 6.217755555555555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTL/NWALL" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7047244094488185, + "rotationDegrees": -90.6513805827204 + } + ], + "constraintZones": [ + { + "name": "Constlaints Zone", + "minWaypointRelativePos": 0.3682983682983686, + "maxWaypointRelativePos": 1.934731934731934, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0.07925407925407942, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CTLD-BLA.path b/src/main/deploy/pathplanner/paths/CTLD-BLA.path new file mode 100644 index 00000000..e103a065 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CTLD-BLA.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.578282647584974, + "y": 4.464973166368515 + }, + "prevControl": null, + "nextControl": { + "x": 7.604758497316638, + "y": 5.811681574239714 + }, + "isLocked": false, + "linkedName": "CTLD" + }, + { + "anchor": { + "x": 6.1444722719141325, + "y": 5.649427549194991 + }, + "prevControl": { + "x": 7.539856887298749, + "y": 5.454722719141323 + }, + "nextControl": { + "x": 5.154067376548645, + "y": 5.787623581106455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6560107334525935, + "y": 5.649427549194991 + }, + "prevControl": { + "x": 3.1560107334525935, + "y": 5.649427549194991 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BLA" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0101694915254242, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -89.70002483769608 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CTR-NWALL.path b/src/main/deploy/pathplanner/paths/CTR-NWALL.path new file mode 100644 index 00000000..205513a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CTR-NWALL.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.752453637660485, + "y": 3.187517831669045 + }, + "prevControl": null, + "nextControl": { + "x": 7.091639351946198, + "y": 3.3170892602404733 + }, + "isLocked": false, + "linkedName": "CTR-QTRShort" + }, + { + "anchor": { + "x": 6.642677777777777, + "y": 3.187517831669045 + }, + "prevControl": { + "x": 6.778924865426631, + "y": 3.397128735744204 + }, + "nextControl": { + "x": 6.4742349206349195, + "y": 2.928374974526188 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.168877777777778, + "y": 1.2968111111111114 + }, + "prevControl": { + "x": 6.915522222222223, + "y": 1.8522444444444455 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTR/NWALL" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7047244094488185, + "rotationDegrees": 90.6513805827204 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.3682983682983686, + "maxWaypointRelativePos": 1.934731934731934, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0.07925407925407942, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CTRD-BA.path b/src/main/deploy/pathplanner/paths/CTRD-BA.path new file mode 100644 index 00000000..e3854562 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CTRD-BA.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.480930232558139, + "y": 3.637477638640429 + }, + "prevControl": null, + "nextControl": { + "x": 7.620983899821109, + "y": 2.3881216457960637 + }, + "isLocked": false, + "linkedName": "CTRD" + }, + { + "anchor": { + "x": 5.901091234347048, + "y": 2.517924865831842 + }, + "prevControl": { + "x": 6.7448121645796055, + "y": 2.5016994633273693 + }, + "nextControl": { + "x": 4.901276094317208, + "y": 2.5371520800631857 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.493756708407871, + "y": 2.517924865831842 + }, + "prevControl": { + "x": 2.993756708407871, + "y": 2.517924865831842 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BUMP-AFTER" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0033898305084745, + "maxWaypointRelativePos": 1.667796610169492, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 88.36342295838341 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of QTRL-MID.path b/src/main/deploy/pathplanner/paths/Copy of QTRL-MID.path new file mode 100644 index 00000000..c3506a6d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of QTRL-MID.path @@ -0,0 +1,121 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.95867469879518, + "y": 5.368180722891566 + }, + "prevControl": null, + "nextControl": { + "x": 6.120451949980013, + "y": 7.485230485924739 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.854975903614458, + "y": 7.378879518072289 + }, + "prevControl": { + "x": 6.908561762200318, + "y": 7.333071437264206 + }, + "nextControl": { + "x": 4.855919745259401, + "y": 7.422316742348598 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 4.851791865998495, + "y": 7.392758427352289 + }, + "nextControl": { + "x": 3.7510031405241824, + "y": 7.434890509906926 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 2.0853209700427953, + "y": 6.616262482168331 + }, + "prevControl": { + "x": 1.8385663965588042, + "y": 6.656414449302378 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0124333925399558, + "rotationDegrees": 179.65469616554114 + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": -178.76956835980303 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.386489479512739, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 2.2, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -138.66842599895932 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 2.0, + "rotation": -90.1883893275273 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DEP-CL.path b/src/main/deploy/pathplanner/paths/DEP-CL.path new file mode 100644 index 00000000..bb774fd8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DEP-CL.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7231186868686872, + "y": 5.523762626262627 + }, + "prevControl": null, + "nextControl": { + "x": 0.7689651818787465, + "y": 4.893427716159044 + }, + "isLocked": false, + "linkedName": "DEPOT" + }, + { + "anchor": { + "x": 2.5439898989898997, + "y": 4.699217171717173 + }, + "prevControl": { + "x": 2.097361111111112, + "y": 4.974065656565657 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -114.56717132151371 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -114.56717132151371 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Delay TL- Wait TL.path b/src/main/deploy/pathplanner/paths/Delay TL- Wait TL.path new file mode 100644 index 00000000..126b1526 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Delay TL- Wait TL.path @@ -0,0 +1,140 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.582922222222222, + "y": 7.455299999999999 + }, + "prevControl": null, + "nextControl": { + "x": 4.582922222222221, + "y": 7.455299999999999 + }, + "isLocked": false, + "linkedName": "TLback" + }, + { + "anchor": { + "x": 5.798716119828815, + "y": 7.455 + }, + "prevControl": { + "x": 5.242203351126415, + "y": 7.455 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Wait TL" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "intake", + "minWaypointRelativePos": 1, + "maxWaypointRelativePos": 1, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 1, + "maxWaypointRelativePos": 1.5, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Delay TR- Wait TR.path b/src/main/deploy/pathplanner/paths/Delay TR- Wait TR.path new file mode 100644 index 00000000..2ecb8890 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Delay TR- Wait TR.path @@ -0,0 +1,133 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5732667617689007, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.573266761768901, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TRBack Delay" + }, + { + "anchor": { + "x": 5.798716119828815, + "y": 0.652 + }, + "prevControl": { + "x": 5.242203351126415, + "y": 0.6520000000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Wait TR" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "intake", + "minWaypointRelativePos": 1, + "maxWaypointRelativePos": 1, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 1, + "maxWaypointRelativePos": 1.5, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -1.3639275316027573 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DelayTR-QTRL.path b/src/main/deploy/pathplanner/paths/DelayTR-QTRL.path new file mode 100644 index 00000000..80c5ae8e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DelayTR-QTRL.path @@ -0,0 +1,129 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5732667617689007, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.5732667617689025, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TRBack Delay" + }, + { + "anchor": { + "x": 7.6360057061340925, + "y": 0.9232524964336677 + }, + "prevControl": { + "x": 7.30963770077934, + "y": 0.5494172889370136 + }, + "nextControl": { + "x": 8.171647388267164, + "y": 1.5367982977949755 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.909385542168675, + "y": 3.947578313253013 + }, + "prevControl": { + "x": 7.896245833887984, + "y": 3.6979238559198855 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "QTR-Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 89.06764595078421 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.6919104991394142, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherLow", + "waypointRelativePos": 1.600688468158345, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -1.3639275316027573 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DelayTRS-CTR-QTR-BR-HP Longer.path b/src/main/deploy/pathplanner/paths/DelayTRS-CTR-QTR-BR-HP Longer.path new file mode 100644 index 00000000..98dd41d6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DelayTRS-CTR-QTR-BR-HP Longer.path @@ -0,0 +1,231 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5732667617689007, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.573266761768901, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TRBack Delay" + }, + { + "anchor": { + "x": 7.7237386363636364, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 7.356348795194274, + "y": 0.46679072309271497 + }, + "nextControl": { + "x": 8.849401974452082, + "y": 1.8225516559819888 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7237386363636364, + "y": 3.510469879518072 + }, + "prevControl": { + "x": 8.566501134742575, + "y": 3.2890864824230137 + }, + "nextControl": { + "x": 6.171183772093508, + "y": 3.918307046797949 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.734771084337349, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 6.040739941643182, + "y": 2.499655745717445 + }, + "nextControl": { + "x": 5.486701615158932, + "y": 2.4304009549069154 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.07933734939759, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 3.560156626506024, + "y": 2.505120481927711 + }, + "nextControl": { + "x": 2.6093551789393854, + "y": 2.4186839866943797 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3636867469879512, + "y": 1.7073975903614462 + }, + "prevControl": { + "x": 2.063060240963855, + "y": 2.406771084337348 + }, + "nextControl": { + "x": 0.6605244289301885, + "y": 1.0042352723036858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.7960111111111112, + "y": 1.1311555555555548 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 77.60386375779406 + }, + { + "waypointRelativePos": 1.7014925373134253, + "rotationDegrees": 124.52672781089822 + }, + { + "waypointRelativePos": 2.897053726169844, + "rotationDegrees": -89.73393281353583 + }, + { + "waypointRelativePos": 4.1532062391681, + "rotationDegrees": -89.57446389529602 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 4.132343011478706, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 4.068846815834772, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -1.3639275316027573 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DelayTRS-CTR-QTR-BR-HP Short.path b/src/main/deploy/pathplanner/paths/DelayTRS-CTR-QTR-BR-HP Short.path new file mode 100644 index 00000000..5fddbb1c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DelayTRS-CTR-QTR-BR-HP Short.path @@ -0,0 +1,231 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5732667617689007, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.573266761768901, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TRBack Delay" + }, + { + "anchor": { + "x": 7.7237386363636364, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 7.356348795194274, + "y": 0.46679072309271497 + }, + "nextControl": { + "x": 8.849401974452082, + "y": 1.8225516559819888 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.7237386363636364, + "y": 3.510469879518072 + }, + "prevControl": { + "x": 8.566501134742575, + "y": 3.2890864824230137 + }, + "nextControl": { + "x": 6.171183772093508, + "y": 3.918307046797949 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.734771084337349, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 6.040739941643182, + "y": 2.499655745717445 + }, + "nextControl": { + "x": 5.486701615158932, + "y": 2.4304009549069154 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.07933734939759, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 3.560156626506024, + "y": 2.505120481927711 + }, + "nextControl": { + "x": 2.6093551789393854, + "y": 2.4186839866943797 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3636867469879512, + "y": 1.7073975903614462 + }, + "prevControl": { + "x": 2.063060240963855, + "y": 2.406771084337348 + }, + "nextControl": { + "x": 0.6605244289301885, + "y": 1.0042352723036858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.7960111111111112, + "y": 1.1311555555555548 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 77.60386375779406 + }, + { + "waypointRelativePos": 1.7014925373134253, + "rotationDegrees": 124.52672781089822 + }, + { + "waypointRelativePos": 2.897053726169844, + "rotationDegrees": -89.73393281353583 + }, + { + "waypointRelativePos": 4.1532062391681, + "rotationDegrees": -89.57446389529602 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 4.132343011478706, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.5266781411359722, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 4.068846815834772, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -1.3639275316027573 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot end to middle left.path b/src/main/deploy/pathplanner/paths/Depot end to middle left.path new file mode 100644 index 00000000..dd3d2d32 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot end to middle left.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5331807228915662, + "y": 7.309133333333333 + }, + "prevControl": null, + "nextControl": { + "x": 1.8663614457831328, + "y": 6.821566265060242 + }, + "isLocked": false, + "linkedName": "Depot End" + }, + { + "anchor": { + "x": 2.654621968616262, + "y": 7.309133333333333 + }, + "prevControl": { + "x": 1.944648741958622, + "y": 7.275315320424938 + }, + "nextControl": { + "x": 3.8894532939174664, + "y": 7.367951807228916 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.9640240963855415, + "y": 7.309133333333333 + }, + "prevControl": { + "x": 6.9640240963855415, + "y": 7.309133333333333 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 1.1343686698176911, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.0252571194393 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Disrupter2.path b/src/main/deploy/pathplanner/paths/Disrupter2.path new file mode 100644 index 00000000..ec43fc6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Disrupter2.path @@ -0,0 +1,116 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.663626506024096, + "y": 3.8383012048192775 + }, + "prevControl": null, + "nextControl": { + "x": 6.445072289156626, + "y": 3.051506024096385 + }, + "isLocked": false, + "linkedName": "Disrupt1End" + }, + { + "anchor": { + "x": 6.445072289156626, + "y": 1.6090481927710845 + }, + "prevControl": { + "x": 6.484803196985326, + "y": 2.2617702499568617 + }, + "nextControl": { + "x": 6.368578313253011, + "y": 0.35236144578313167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0465542168674697, + "y": 0.6474096385542167 + }, + "prevControl": { + "x": 5.734771084337349, + "y": 0.669265060240964 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Disruptor2End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.4058091286307017, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.8004962779156348, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 1.3756823821339927, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 1.8719602977667555, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.9583733239901 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -117.69947280805494 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP-CR.path b/src/main/deploy/pathplanner/paths/HP-CR.path new file mode 100644 index 00000000..ecc494da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP-CR.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": null, + "nextControl": { + "x": 1.3043484416201951, + "y": 0.8563997661853033 + }, + "isLocked": false, + "linkedName": "HP" + }, + { + "anchor": { + "x": 2.4864194008559197, + "y": 1.45373751783167 + }, + "prevControl": { + "x": 1.8912410841654772, + "y": 1.1043937232524967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 30.06858282186252 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP-DEP.path b/src/main/deploy/pathplanner/paths/HP-DEP.path new file mode 100644 index 00000000..e180f040 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP-DEP.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": null, + "nextControl": { + "x": 1.6545580808080804, + "y": 2.3922283984422537 + }, + "isLocked": false, + "linkedName": "HP" + }, + { + "anchor": { + "x": 1.925580808080809, + "y": 2.752373737373738 + }, + "prevControl": { + "x": 1.9141287878787883, + "y": 2.1454166666666676 + }, + "nextControl": { + "x": 1.9914810690227644, + "y": 6.245087567297212 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8605429292929296, + "y": 7.3675378787878785 + }, + "prevControl": { + "x": 1.6524176020005827, + "y": 7.123884133339367 + }, + "nextControl": { + "x": 0.6215981771124424, + "y": 7.44105934099726 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7231186868686872, + "y": 5.523762626262627 + }, + "prevControl": { + "x": 0.6658585858585865, + "y": 7.046881313131314 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DEPOT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.507368421052627, + "rotationDegrees": -113.28528920977637 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -114.56717132151371 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP-SHOOT.path b/src/main/deploy/pathplanner/paths/HP-SHOOT.path new file mode 100644 index 00000000..29e9301b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP-SHOOT.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": null, + "nextControl": { + "x": 1.265189393939394, + "y": 1.2470263782402344 + }, + "isLocked": false, + "linkedName": "HP" + }, + { + "anchor": { + "x": 1.719444444444445, + "y": 1.5728156565656572 + }, + "prevControl": { + "x": 1.0208712121212127, + "y": 1.13763888888889 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 31.865977693603632 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP-FAR.path b/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP-FAR.path new file mode 100644 index 00000000..24f47623 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP-FAR.path @@ -0,0 +1,150 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": null, + "nextControl": { + "x": 1.4620361445783137, + "y": 1.7948192771084335 + }, + "isLocked": false, + "linkedName": "HP" + }, + { + "anchor": { + "x": 2.2119875781873777, + "y": 2.9108085660264544 + }, + "prevControl": { + "x": 2.2323155614396493, + "y": 2.2399851187015054 + }, + "nextControl": { + "x": 2.182754244854044, + "y": 3.875508566026455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2119875781873777, + "y": 4.409032273923896 + }, + "prevControl": { + "x": 2.180894273052745, + "y": 3.600606340423459 + }, + "nextControl": { + "x": 2.233842999874125, + "y": 4.977273237779317 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0058780898306343, + "y": 5.216407440931898 + }, + "prevControl": { + "x": 1.2244087904543446, + "y": 4.951048733031677 + }, + "nextControl": { + "x": 0.7500837982383879, + "y": 5.527014795008199 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5838306893366783, + "y": 7.402396540926235 + }, + "prevControl": { + "x": 0.7028696997324098, + "y": 7.1102098790458035 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5742738589211462, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.1037344398339943, + "rotationDegrees": 146.5744600455709 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.6709677419354926, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.2590570719603225, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0.019851116625301098, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP-HP.path b/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP-HP.path new file mode 100644 index 00000000..13dcc7c0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP-HP.path @@ -0,0 +1,195 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": null, + "nextControl": { + "x": 1.7022444444444447, + "y": 1.7742888888888877 + }, + "isLocked": false, + "linkedName": "HP" + }, + { + "anchor": { + "x": 2.2381888888888892, + "y": 3.021577777777777 + }, + "prevControl": { + "x": 2.258516872141161, + "y": 2.350754330452828 + }, + "nextControl": { + "x": 2.2089555555555553, + "y": 3.9862777777777776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2381888888888892, + "y": 4.785322222222223 + }, + "prevControl": { + "x": 2.2576777777777783, + "y": 3.976533333333334 + }, + "nextControl": { + "x": 2.2312324768230467, + "y": 5.074013322954682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9418522727272725, + "y": 7.199193181818182 + }, + "prevControl": { + "x": 2.257352272727273, + "y": 7.511015404040405 + }, + "nextControl": { + "x": 0.58204659500551, + "y": 7.113905910061912 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.55, + "y": 4.785322222222223 + }, + "prevControl": { + "x": 0.5834222222222223, + "y": 5.155611111111111 + }, + "nextControl": { + "x": 0.5192909932183902, + "y": 4.445093379135811 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.55, + "y": 2.056877777777778 + }, + "prevControl": { + "x": 0.546653435380747, + "y": 2.6613859992403963 + }, + "nextControl": { + "x": 0.5533465646192531, + "y": 1.4523695563151593 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.47499999999999987, + "y": 2.3225385132311627 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.05, + "rotationDegrees": -126.57067858255719 + }, + { + "waypointRelativePos": 3.95, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 3.5, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.6, + "maxWaypointRelativePos": 4.651116625310213, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.65, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0.019851116625301098, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP.path b/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP.path new file mode 100644 index 00000000..66ba1ff5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP-TWROUT-DEP.path @@ -0,0 +1,133 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": null, + "nextControl": { + "x": 1.7022444444444447, + "y": 1.7742888888888877 + }, + "isLocked": false, + "linkedName": "HP" + }, + { + "anchor": { + "x": 2.2381888888888892, + "y": 3.021577777777777 + }, + "prevControl": { + "x": 2.258516872141161, + "y": 2.350754330452828 + }, + "nextControl": { + "x": 2.2089555555555553, + "y": 3.9862777777777776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2381888888888892, + "y": 4.785322222222223 + }, + "prevControl": { + "x": 2.2576777777777783, + "y": 3.976533333333334 + }, + "nextControl": { + "x": 2.2312324768230467, + "y": 5.074013322954682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9418522727272725, + "y": 7.199193181818182 + }, + "prevControl": { + "x": 2.257352272727273, + "y": 7.511015404040405 + }, + "nextControl": { + "x": 0.58204659500551, + "y": 7.113905910061912 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8181704545454542, + "y": 5.529488636363636 + }, + "prevControl": { + "x": 0.9827966065125846, + "y": 5.717632810040355 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.1037344398339943, + "rotationDegrees": -126.57067858255719 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 3.966253101736982, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 0.019851116625301098, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -128.7531608894681 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left3rdSwipe.path b/src/main/deploy/pathplanner/paths/Left3rdSwipe.path new file mode 100644 index 00000000..adcd12c8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left3rdSwipe.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.168877777777778, + "y": 6.773188888888889 + }, + "prevControl": null, + "nextControl": { + "x": 6.779100000000001, + "y": 7.38708888888889 + }, + "isLocked": false, + "linkedName": "CTL/NWALL" + }, + { + "anchor": { + "x": 5.804655555555557, + "y": 6.1787777777777775 + }, + "prevControl": { + "x": 5.975126193718132, + "y": 6.923465302382711 + }, + "nextControl": { + "x": 5.249222222222225, + "y": 3.75241111111111 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.734055555555555, + "y": 6.734211111111112 + }, + "prevControl": { + "x": 8.104344444444445, + "y": 3.557522222222221 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "YHY" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0307053941908686, + "rotationDegrees": -89.1641830798336 + }, + { + "waypointRelativePos": 1.8207468879668083, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constlaints Zone", + "minWaypointRelativePos": 0.8764568764568764, + "maxWaypointRelativePos": 1.9020979020979023, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftNwall-Tower.path b/src/main/deploy/pathplanner/paths/LeftNwall-Tower.path new file mode 100644 index 00000000..b2ea1b3e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftNwall-Tower.path @@ -0,0 +1,165 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.734055555555555, + "y": 6.734211111111112 + }, + "prevControl": null, + "nextControl": { + "x": 7.081177777777778, + "y": 7.0655222222222225 + }, + "isLocked": false, + "linkedName": "YHY" + }, + { + "anchor": { + "x": 5.609766666666667, + "y": 7.416322222222221 + }, + "prevControl": { + "x": 6.109766666666667, + "y": 7.416322222222221 + }, + "nextControl": { + "x": 5.109766666666667, + "y": 7.416322222222221 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8813222222222223, + "y": 7.416322222222221 + }, + "prevControl": { + "x": 3.1308041515507417, + "y": 7.432408456664117 + }, + "nextControl": { + "x": 2.3239571428571426, + "y": 7.380384126984124 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9129444444444446, + "y": 6.9291 + }, + "prevControl": { + "x": 1.2498301587301588, + "y": 7.123457142857143 + }, + "nextControl": { + "x": 0.6963980479322944, + "y": 6.804169386627605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7278000000000002, + "y": 5.389477777777777 + }, + "prevControl": { + "x": 0.7018857142857144, + "y": 5.868892063492063 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": -178.67153523769426 + }, + { + "waypointRelativePos": 2.061410788381753, + "rotationDegrees": 179.9898157564805 + }, + { + "waypointRelativePos": 3.1830708661417297, + "rotationDegrees": -129.31976232947846 + } + ], + "constraintZones": [ + { + "name": "Constlaints Zone", + "minWaypointRelativePos": 2.1002331002331003, + "maxWaypointRelativePos": 3.813519813519817, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constlaints Zone", + "minWaypointRelativePos": 0.42481389578166007, + "maxWaypointRelativePos": 1.5682382133995136, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.45, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 1.584119106699762, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.42745640318978 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NWALL-CLIMB.path b/src/main/deploy/pathplanner/paths/NWALL-CLIMB.path new file mode 100644 index 00000000..0c222854 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NWALL-CLIMB.path @@ -0,0 +1,165 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.7535444444444455, + "y": 1.2968111111111114 + }, + "prevControl": null, + "nextControl": { + "x": 7.100666666666668, + "y": 0.9655000000000002 + }, + "isLocked": false, + "linkedName": "YHYRight" + }, + { + "anchor": { + "x": 5.587871428571429, + "y": 0.5495285714285705 + }, + "prevControl": { + "x": 6.087871428571429, + "y": 0.5495285714285705 + }, + "nextControl": { + "x": 5.087871428571429, + "y": 0.5495285714285706 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8150428571428576, + "y": 0.5495285714285705 + }, + "prevControl": { + "x": 2.5650428571428576, + "y": 0.5495285714285705 + }, + "nextControl": { + "x": 3.0650428571428576, + "y": 0.5495285714285705 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8714714285714285, + "y": 0.8475428571428563 + }, + "prevControl": { + "x": 1.2083571428571427, + "y": 0.6531857142857133 + }, + "nextControl": { + "x": 0.6549250320592783, + "y": 0.9724734705152506 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.6123285714285713, + "y": 2.3764857142857134 + }, + "prevControl": { + "x": 0.5864142857142856, + "y": 1.897071428571428 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 178.67153523769426 + }, + { + "waypointRelativePos": 2.484251968503936, + "rotationDegrees": -179.56362140522202 + }, + { + "waypointRelativePos": 3.1830708661417297, + "rotationDegrees": 89.76480910221724 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.1002331002331003, + "maxWaypointRelativePos": 3.813519813519817, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.426063470627953, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.45, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 1.584119106699762, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.97696981133217 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.78099970237523 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NWALL-DEP.path b/src/main/deploy/pathplanner/paths/NWALL-DEP.path new file mode 100644 index 00000000..61589496 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NWALL-DEP.path @@ -0,0 +1,188 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.905781990521327, + "y": 1.3913744075829368 + }, + "prevControl": null, + "nextControl": { + "x": 6.02540404040404, + "y": 3.496755050505052 + }, + "isLocked": false, + "linkedName": "HSE" + }, + { + "anchor": { + "x": 6.277348484848485, + "y": 4.733573232323233 + }, + "prevControl": { + "x": 6.162828282828284, + "y": 3.8861237373737376 + }, + "nextControl": { + "x": 6.385039644017192, + "y": 5.530487810171672 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.071212121212121, + "y": 7.344905213270142 + }, + "prevControl": { + "x": 6.54405572310792, + "y": 7.151469194312795 + }, + "nextControl": { + "x": 5.747382754909533, + "y": 7.4773808631211836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9906186868686877, + "y": 7.344905213270142 + }, + "prevControl": { + "x": 3.3774907247834443, + "y": 7.32341232227463 + }, + "nextControl": { + "x": 2.013667352697342, + "y": 7.399180287391396 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1021208530806337, + "y": 7.344905213270142 + }, + "prevControl": { + "x": 1.6609360189574756, + "y": 7.5275947867296065 + }, + "nextControl": { + "x": 0.6371110642226974, + "y": 7.192882782297581 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8979383886256569, + "y": 5.3997985781988 + }, + "prevControl": { + "x": 0.6762921770843731, + "y": 5.5154400798727075 + }, + "nextControl": { + "x": 1.1451066350712193, + "y": 5.270841232226985 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7254146919431275, + "y": 5.528755924170615 + }, + "prevControl": { + "x": 1.733651975156703, + "y": 5.278891666692161 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.065719360568384, + "rotationDegrees": 177.81982432474064 + }, + { + "waypointRelativePos": 2.838365896980463, + "rotationDegrees": -178.4194815244811 + }, + { + "waypointRelativePos": 4.152753108348143, + "rotationDegrees": -123.38150300596608 + }, + { + "waypointRelativePos": 4.872113676731794, + "rotationDegrees": -114.87019297935495 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.2292358803986843, + "maxWaypointRelativePos": 5.767441860465112, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.03986710963455847, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 2.631229235880409, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -25.88702626632668 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.56170533256645 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NWALL-HP.path b/src/main/deploy/pathplanner/paths/NWALL-HP.path new file mode 100644 index 00000000..c4b8f644 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NWALL-HP.path @@ -0,0 +1,158 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.132575757575758, + "y": 7.069785353535353 + }, + "prevControl": null, + "nextControl": { + "x": 7.033181818181819, + "y": 7.069785353535353 + }, + "isLocked": false, + "linkedName": "SEH" + }, + { + "anchor": { + "x": 6.048308080808081, + "y": 7.069785353535353 + }, + "prevControl": { + "x": 6.323156565656564, + "y": 7.539318181818182 + }, + "nextControl": { + "x": 5.682961419895421, + "y": 6.445651474476225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.899431818181819, + "y": 3.8288636363636366 + }, + "prevControl": { + "x": 5.8765277777777785, + "y": 4.561792929292929 + }, + "nextControl": { + "x": 5.911536972297631, + "y": 3.4414987046576666 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.899431818181819, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 6.250953816026736, + "y": 1.0039103259558735 + }, + "nextControl": { + "x": 5.464255050505051, + "y": 0.5484531459170012 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.505959595959596, + "y": 0.6795580808080808 + }, + "prevControl": { + "x": 3.755959595959596, + "y": 0.6795580808080808 + }, + "nextControl": { + "x": 2.8646464646464653, + "y": 0.6795580808080809 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 1.432223317512475, + "y": 1.2636111111111108 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1491228070175437, + "rotationDegrees": -91.30498426887868 + }, + { + "waypointRelativePos": 2.5614035087719005, + "rotationDegrees": -89.52336750739953 + }, + { + "waypointRelativePos": 3.1578947368420867, + "rotationDegrees": -179.710783645745 + }, + { + "waypointRelativePos": 3.9561403508771993, + "rotationDegrees": 179.31442234054276 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.25470653377631153, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 3.6877076411960146, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Misc", + "idealStartingState": { + "velocity": 0, + "rotation": 89.60211903816543 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 00000000..89c0cb56 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.379734939759036, + "y": 0.6036987951807202 + }, + "prevControl": null, + "nextControl": { + "x": 5.379734939759036, + "y": 0.60369879518072 + }, + "isLocked": false, + "linkedName": "TRS" + }, + { + "anchor": { + "x": 7.754659090909092, + "y": 0.9315025252525266 + }, + "prevControl": { + "x": 6.774782542201074, + "y": 0.7318980431083034 + }, + "nextControl": { + "x": 8.373068181818182, + "y": 1.057474747474747 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.235643939393938, + "y": 4.01209595959596 + }, + "prevControl": { + "x": 8.304356060606061, + "y": 2.637853535353536 + }, + "nextControl": { + "x": 8.210675130921992, + "y": 4.511472129034879 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.132575757575758, + "y": 7.069785353535353 + }, + "prevControl": { + "x": 8.132575757575758, + "y": 6.073459595959596 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SEH" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1000000000000099, + "rotationDegrees": 89.64085041980647 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 89.60211903816543 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.6213872948567607 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QBRight-HP.path b/src/main/deploy/pathplanner/paths/QBRight-HP.path new file mode 100644 index 00000000..23c5f38c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QBRight-HP.path @@ -0,0 +1,144 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.909385542168675, + "y": 3.947578313253013 + }, + "prevControl": null, + "nextControl": { + "x": 7.829221400754534, + "y": 3.031416697091397 + }, + "isLocked": false, + "linkedName": "QTR-Left" + }, + { + "anchor": { + "x": 7.515987951807228, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 8.17292968834893, + "y": 2.56676821894298 + }, + "nextControl": { + "x": 6.4234652245345, + "y": 2.2861937294633083 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.543650602409638, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 5.407087706842395, + "y": 2.4775486498520247 + }, + "nextControl": { + "x": 3.3743855421686746, + "y": 2.439554216867471 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2488313253012047, + "y": 2.1445060240963856 + }, + "prevControl": { + "x": 2.805333729805586, + "y": 2.477886577560241 + }, + "nextControl": { + "x": 1.6923289207968235, + "y": 1.81112547063253 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.9830795454545451, + "y": 1.200625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.7, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.9, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 1.2753872633390742, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 3.3548387096774173, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTL-TL.path b/src/main/deploy/pathplanner/paths/QTL-TL.path new file mode 100644 index 00000000..1001c14d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTL-TL.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.01866265060241, + "y": 4.483036144578313 + }, + "prevControl": null, + "nextControl": { + "x": 7.938498509188269, + "y": 5.399197760739929 + }, + "isLocked": false, + "linkedName": "CTR-QTR__LEFT" + }, + { + "anchor": { + "x": 7.122590361445783, + "y": 7.214963855421686 + }, + "prevControl": { + "x": 7.66962167676118, + "y": 7.098574213865219 + }, + "nextControl": { + "x": 6.046100462455883, + "y": 7.4440042594620905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.140798858773181, + "y": 7.214963855421686 + }, + "prevControl": { + "x": 2.0259434370864335, + "y": 7.2914578313253 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "trsj__LEFT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.176763485477179, + "rotationDegrees": -0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.55851851851852, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.6372208436724653, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.11550613618262 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTLong-TL.path b/src/main/deploy/pathplanner/paths/QTLong-TL.path new file mode 100644 index 00000000..31d7c92b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTLong-TL.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.958177777777777, + "y": 4.054488888888889 + }, + "prevControl": null, + "nextControl": { + "x": 7.878013636363637, + "y": 4.9706505050505045 + }, + "isLocked": false, + "linkedName": "QTLLong" + }, + { + "anchor": { + "x": 7.122590361445783, + "y": 7.214963855421686 + }, + "prevControl": { + "x": 7.66962167676118, + "y": 7.098574213865219 + }, + "nextControl": { + "x": 6.046100462455883, + "y": 7.4440042594620905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.140798858773181, + "y": 7.214963855421686 + }, + "prevControl": { + "x": 2.0259434370864358, + "y": 7.2914578313253 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "trsj__LEFT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.176763485477179, + "rotationDegrees": -0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1096774193548447, + "maxWaypointRelativePos": 1.5464019851116673, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.6372208436724653, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.11550613618262 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTLong-TLHP.path b/src/main/deploy/pathplanner/paths/QTLong-TLHP.path new file mode 100644 index 00000000..b7969608 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTLong-TLHP.path @@ -0,0 +1,144 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.958177777777777, + "y": 4.054488888888889 + }, + "prevControl": null, + "nextControl": { + "x": 7.878013636363637, + "y": 4.9706505050505045 + }, + "isLocked": false, + "linkedName": "QTLLong" + }, + { + "anchor": { + "x": 7.122590361445783, + "y": 7.214963855421686 + }, + "prevControl": { + "x": 7.66962167676118, + "y": 7.098574213865219 + }, + "nextControl": { + "x": 6.046100462455883, + "y": 7.4440042594620905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.140798858773181, + "y": 7.214963855421686 + }, + "prevControl": { + "x": 1.7618544935805986, + "y": 7.366704707560627 + }, + "nextControl": { + "x": 0.8979425469936021, + "y": 7.155627426233372 + }, + "isLocked": false, + "linkedName": "trsj__LEFT" + }, + { + "anchor": { + "x": 0.5585592011412266, + "y": 5.257703281027104 + }, + "prevControl": { + "x": 0.5714978601997143, + "y": 5.82700427960057 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.176763485477179, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 1.5820895522388005, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.379530916844367, + "rotationDegrees": -114.04950742372787 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1096774193548447, + "maxWaypointRelativePos": 1.5464019851116673, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.7825793382849493, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.6372208436724653, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 1.6448345712356738, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.11550613618262 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTR-BUMP.path b/src/main/deploy/pathplanner/paths/QTR-BUMP.path new file mode 100644 index 00000000..fc920e6e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTR-BUMP.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.592481927710843, + "y": 3.7399518072289153 + }, + "prevControl": null, + "nextControl": { + "x": 6.598783542039356, + "y": 2.6477280858676204 + }, + "isLocked": false, + "linkedName": "QTRLong" + }, + { + "anchor": { + "x": 5.641484794275492, + "y": 2.517924865831842 + }, + "prevControl": { + "x": 6.177249425116623, + "y": 2.544447867358631 + }, + "nextControl": { + "x": 4.002719141323793, + "y": 2.4367978533094794 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.493756708407871, + "y": 2.517924865831842 + }, + "prevControl": { + "x": 4.164973166368515, + "y": 2.5016994633273706 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BUMP-AFTER" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8203389830508477, + "maxWaypointRelativePos": 1.7627118644067767, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTR-TR.path b/src/main/deploy/pathplanner/paths/QTR-TR.path new file mode 100644 index 00000000..bdec2a3e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTR-TR.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.821755555555557, + "y": 3.5965 + }, + "prevControl": null, + "nextControl": { + "x": 7.741591414141416, + "y": 2.680338383838384 + }, + "isLocked": false, + "linkedName": "CTR-QTR" + }, + { + "anchor": { + "x": 7.122590361445783, + "y": 0.8550361445783141 + }, + "prevControl": { + "x": 7.66962167676118, + "y": 0.9714257861347814 + }, + "nextControl": { + "x": 6.046100462455883, + "y": 0.6259957405379102 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": { + "x": 4.419344578313253, + "y": 0.5382060240963868 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "trsj" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.176763485477179, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6877076411960215, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.6372208436724653, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTR-TRBack.path b/src/main/deploy/pathplanner/paths/QTR-TRBack.path new file mode 100644 index 00000000..517ae9f6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTR-TRBack.path @@ -0,0 +1,112 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.98587951807229, + "y": 3.1717108433734937 + }, + "prevControl": null, + "nextControl": { + "x": 7.905715376658149, + "y": 2.255549227211878 + }, + "isLocked": false, + "linkedName": "QTR" + }, + { + "anchor": { + "x": 7.122590361445783, + "y": 0.8550361445783141 + }, + "prevControl": { + "x": 7.66962167676118, + "y": 0.9714257861347814 + }, + "nextControl": { + "x": 6.046100462455883, + "y": 0.6259957405379102 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9919156626506025, + "y": 0.680192771084337 + }, + "prevControl": { + "x": 3.8770602409638535, + "y": 0.6036987951807236 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TRBack" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.176763485477179, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.3598014888337497, + "maxWaypointRelativePos": 1.689330024813893, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.6372208436724653, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 1.6734491315136455, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRH-HP.path b/src/main/deploy/pathplanner/paths/QTRH-HP.path new file mode 100644 index 00000000..d8f1f854 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRH-HP.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.215590361445784, + "y": 3.2809879518072296 + }, + "prevControl": null, + "nextControl": { + "x": 6.303290361445786, + "y": 0.3576546184738967 + }, + "isLocked": false, + "linkedName": "nWall" + }, + { + "anchor": { + "x": 4.662613636363636, + "y": 0.6108459595959597 + }, + "prevControl": { + "x": 6.925893890115048, + "y": 0.6362759383412605 + }, + "nextControl": { + "x": 2.4038444444444447, + "y": 0.5854666666666654 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.718341499330656, + "y": 1.2968111111111114 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7087136929460608, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2962655601659734, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5049833887043119, + "maxWaypointRelativePos": 1.4440753045404362, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 1.046153846153844, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRH-TRBack.path b/src/main/deploy/pathplanner/paths/QTRH-TRBack.path new file mode 100644 index 00000000..d4e3cfc6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRH-TRBack.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.313939759036145, + "y": 3.3684096385542173 + }, + "prevControl": null, + "nextControl": { + "x": 6.401639759036146, + "y": 0.44507630522088437 + }, + "isLocked": false, + "linkedName": "nWallNew" + }, + { + "anchor": { + "x": 4.662613636363636, + "y": 0.6108459595959597 + }, + "prevControl": { + "x": 6.9227336217661595, + "y": 0.4886091303499399 + }, + "nextControl": { + "x": 4.390662650602409, + "y": 0.6255542168674697 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9919156626506025, + "y": 0.680192771084337 + }, + "prevControl": { + "x": 4.379734939759036, + "y": 0.669265060240964 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TRBack" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7087136929460608, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2962655601659734, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.22431761786600546, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 1.046153846153844, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRL-MID.path b/src/main/deploy/pathplanner/paths/QTRL-MID.path new file mode 100644 index 00000000..c6455943 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRL-MID.path @@ -0,0 +1,121 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.346722891566266, + "y": 4.668807228915662 + }, + "prevControl": null, + "nextControl": { + "x": 6.390433734939759, + "y": 5.3463253012048195 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.346722891566266, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 7.122590361445783, + "y": 7.3898072289156636 + }, + "nextControl": { + "x": 5.347119402287654, + "y": 7.439820495370818 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 4.851791865998495, + "y": 7.392758427352289 + }, + "nextControl": { + "x": 3.7510031405241824, + "y": 7.434890509906926 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 2.4673855421686746, + "y": 7.16032530120482 + }, + "prevControl": { + "x": 2.2206309686846835, + "y": 7.200477268338867 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.49543568464731724, + "rotationDegrees": 1.0871828014246014 + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": 0.10378265392839682 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.386489479512739, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 2.2, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 18.824710018240136 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 2.0, + "rotation": -90.1883893275273 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRL-TL.path b/src/main/deploy/pathplanner/paths/QTRL-TL.path new file mode 100644 index 00000000..e45b4d57 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRL-TL.path @@ -0,0 +1,141 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.346722891566266, + "y": 4.668807228915662 + }, + "prevControl": null, + "nextControl": { + "x": 6.399066666666666, + "y": 5.408966666666666 + }, + "isLocked": false, + "linkedName": "CTR-QTRL" + }, + { + "anchor": { + "x": 5.854975903614458, + "y": 7.378879518072289 + }, + "prevControl": { + "x": 6.908561762200318, + "y": 7.333071437264206 + }, + "nextControl": { + "x": 4.855919745259401, + "y": 7.422316742348598 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 4.85215350797128, + "y": 7.411662650602409 + }, + "nextControl": { + "x": 3.8636055281732995, + "y": 7.411662650602409 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 1.010388888888889, + "y": 7.211688888888889 + }, + "prevControl": { + "x": 1.2357967512092107, + "y": 7.339618034845836 + }, + "nextControl": { + "x": 0.6665026329647186, + "y": 7.016517901622288 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8252444444444447, + "y": 5.243311111111111 + }, + "prevControl": { + "x": 0.598159499952956, + "y": 5.397728873365324 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0124333925399558, + "rotationDegrees": 179.65469616554114 + }, + { + "waypointRelativePos": 2.380106571936063, + "rotationDegrees": -178.76956835980303 + }, + { + "waypointRelativePos": 3.339253996447626, + "rotationDegrees": -113.45729127312542 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.386489479512739, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 2.3622047244094206, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -114.62356478616367 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 2.0, + "rotation": -90.1883893275273 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRLD-BLA.path b/src/main/deploy/pathplanner/paths/QTRLD-BLA.path new file mode 100644 index 00000000..513b4108 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRLD-BLA.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.614337349397591, + "y": 4.8982891566265065 + }, + "prevControl": null, + "nextControl": { + "x": 7.4769492424079145, + "y": 5.29876336832123 + }, + "isLocked": false, + "linkedName": "QTRLH" + }, + { + "anchor": { + "x": 6.1444722719141325, + "y": 5.649427549194991 + }, + "prevControl": { + "x": 6.598783542039356, + "y": 5.633202146690519 + }, + "nextControl": { + "x": 5.14510941756659, + "y": 5.685119079707403 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6560107334525935, + "y": 5.649427549194991 + }, + "prevControl": { + "x": 3.1560107334525935, + "y": 5.649427549194991 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BLA" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0101694915254242, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": -118.17859010995926 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRLeft-HP.path b/src/main/deploy/pathplanner/paths/QTRLeft-HP.path new file mode 100644 index 00000000..bc50dafd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRLeft-HP.path @@ -0,0 +1,128 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.909385542168675, + "y": 3.947578313253013 + }, + "prevControl": null, + "nextControl": { + "x": 7.829221400754534, + "y": 3.031416697091397 + }, + "isLocked": false, + "linkedName": "QTR-Left" + }, + { + "anchor": { + "x": 7.569136363636364, + "y": 0.9532613636363636 + }, + "prevControl": { + "x": 8.226078100178066, + "y": 1.0586199440251265 + }, + "nextControl": { + "x": 6.476613636363636, + "y": 0.778045454545455 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.631693181818182, + "y": 0.5822159090909098 + }, + "prevControl": { + "x": 5.492129545454545, + "y": 0.5085045454545467 + }, + "nextControl": { + "x": 3.4554893620553235, + "y": 0.6829782561198184 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.9830795454545451, + "y": 1.200625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.05, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.3786574870912236, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 1.2753872633390742, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 2.3287435456110144, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRLong-HP.path b/src/main/deploy/pathplanner/paths/QTRLong-HP.path new file mode 100644 index 00000000..32f1affe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRLong-HP.path @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.592481927710843, + "y": 3.7399518072289153 + }, + "prevControl": null, + "nextControl": { + "x": 7.680181927710844, + "y": 0.8166184738955824 + }, + "isLocked": false, + "linkedName": "QTRLong" + }, + { + "anchor": { + "x": 4.662613636363636, + "y": 0.6108459595959597 + }, + "prevControl": { + "x": 6.925893890115048, + "y": 0.6362759383412605 + }, + "nextControl": { + "x": 2.4038444444444447, + "y": 0.5854666666666654 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.718341499330656, + "y": 1.2968111111111114 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7087136929460608, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2962655601659734, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5049833887043119, + "maxWaypointRelativePos": 1.137468982630273, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.129528535980151, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 100.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 1.0660049627791606, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRLong-TL.path b/src/main/deploy/pathplanner/paths/QTRLong-TL.path new file mode 100644 index 00000000..4293879e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRLong-TL.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.614337349397591, + "y": 4.8982891566265065 + }, + "prevControl": null, + "nextControl": { + "x": 8.158335446482184, + "y": 7.75592098438274 + }, + "isLocked": false, + "linkedName": "QTRLH" + }, + { + "anchor": { + "x": 6.182807228915664, + "y": 7.324240963855422 + }, + "prevControl": { + "x": 6.707337349397591, + "y": 7.313313253012049 + }, + "nextControl": { + "x": 5.66708121700441, + "y": 7.334985255770239 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7187228915662653, + "y": 7.324240963855422 + }, + "prevControl": { + "x": 7.3599702792211446, + "y": 7.408477559527209 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TL-Long" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7452282157676345, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.2, + "maxWaypointRelativePos": 0.3, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 1.8878411910669979, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": -118.17859010995926 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/QTRShort-TR.path b/src/main/deploy/pathplanner/paths/QTRShort-TR.path new file mode 100644 index 00000000..4a0b3aa8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/QTRShort-TR.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.752453637660485, + "y": 3.187517831669045 + }, + "prevControl": null, + "nextControl": { + "x": 7.672289496246345, + "y": 2.271356215507429 + }, + "isLocked": false, + "linkedName": "CTR-QTRShort" + }, + { + "anchor": { + "x": 7.122590361445783, + "y": 0.8550361445783141 + }, + "prevControl": { + "x": 7.66962167676118, + "y": 0.9714257861347814 + }, + "nextControl": { + "x": 6.046100462455883, + "y": 0.6259957405379102 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": { + "x": 4.419344578313253, + "y": 0.5382060240963868 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "trsj" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.176763485477179, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6877076411960215, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherLow", + "waypointRelativePos": 0.6372208436724653, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightDisrupt1.path b/src/main/deploy/pathplanner/paths/RightDisrupt1.path new file mode 100644 index 00000000..3344f0f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightDisrupt1.path @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.314168674698795, + "y": 0.5709156626506025 + }, + "prevControl": null, + "nextControl": { + "x": 5.559927710843374, + "y": 0.691120481927711 + }, + "isLocked": false, + "linkedName": "Disrupt1Start" + }, + { + "anchor": { + "x": 8.401132530120481, + "y": 1.0845180722891563 + }, + "prevControl": { + "x": 8.207112700990953, + "y": 0.6277630578800578 + }, + "nextControl": { + "x": 8.92566265060241, + "y": 2.3193493975903605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.401132530120481, + "y": 2.6253253012048186 + }, + "prevControl": { + "x": 8.38799282183979, + "y": 2.375670843871691 + }, + "nextControl": { + "x": 8.414272238401173, + "y": 2.874979758537946 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.663626506024096, + "y": 3.8383012048192775 + }, + "prevControl": { + "x": 7.670081902333116, + "y": 3.6790079515581304 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Disrupt1End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.08347505865411542 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 59.99999999999999 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.3076923076923077, + "constraints": { + "maxVelocity": 6.0, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": true + }, + "goalEndState": { + "velocity": 0, + "rotation": -117.69947280805494 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 1.548157698977892 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StartTR-CTR-HLF-BR-HP.path b/src/main/deploy/pathplanner/paths/StartTR-CTR-HLF-BR-HP.path new file mode 100644 index 00000000..a4b2e54f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StartTR-CTR-HLF-BR-HP.path @@ -0,0 +1,234 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.528863636363636, + "y": 0.5269999999999992 + }, + "prevControl": null, + "nextControl": { + "x": 4.528863636363638, + "y": 0.5269999999999992 + }, + "isLocked": false, + "linkedName": "DELAYTRS" + }, + { + "anchor": { + "x": 8.27, + "y": 1.2109318181818185 + }, + "prevControl": { + "x": 8.26707684069612, + "y": 0.5288207070707074 + }, + "nextControl": { + "x": 8.27401314782924, + "y": 2.147388726713465 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27, + "y": 4.168988636363637 + }, + "prevControl": { + "x": 8.630544444444444, + "y": 4.052055303030303 + }, + "nextControl": { + "x": 7.702750918359643, + "y": 4.3529613114902395 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.09698888888889, + "y": 3.7329222222222214 + }, + "prevControl": { + "x": 6.445693181818182, + "y": 4.168988636363637 + }, + "nextControl": { + "x": 5.709247550368681, + "y": 3.248038673234238 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.827284090909091, + "y": 2.5405113636363637 + }, + "prevControl": { + "x": 6.076730380373243, + "y": 2.557141116267308 + }, + "nextControl": { + "x": 5.577837801444936, + "y": 2.523881611005419 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.528863636363636, + "y": 2.5405113636363637 + }, + "prevControl": { + "x": 4.425556818181818, + "y": 2.5817386363636365 + }, + "nextControl": { + "x": 1.7212293386068067, + "y": 2.4574017407509916 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8252444444444447, + "y": 2.154322222222222 + }, + "prevControl": { + "x": 1.0623012626262627, + "y": 2.6593563131313123 + }, + "nextControl": { + "x": 0.6646317814276703, + "y": 1.8121474184038764 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.5334666666666664, + "y": 1.4435309236947798 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.44730290456431443, + "rotationDegrees": 90.94294723905875 + }, + { + "waypointRelativePos": 1.0291777188328912, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.5875331564986732, + "rotationDegrees": 90.08083451403282 + }, + { + "waypointRelativePos": 2.4403183023872708, + "rotationDegrees": -162.56886722416561 + }, + { + "waypointRelativePos": 3.9469496021220216, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.47703703703703715, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9851851851851856, + "maxWaypointRelativePos": 2.551111111111109, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.019259259259263, + "maxWaypointRelativePos": 6.875555555555566, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.4348413234301141, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 5.040000000000003, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTL-QTL-BL-L.path b/src/main/deploy/pathplanner/paths/TL-CTL-QTL-BL-L.path new file mode 100644 index 00000000..06a5be81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTL-QTL-BL-L.path @@ -0,0 +1,204 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.140798858773181, + "y": 7.214963855421686 + }, + "prevControl": null, + "nextControl": { + "x": 2.1407988587731825, + "y": 7.214963855421686 + }, + "isLocked": false, + "linkedName": "trsj__LEFT" + }, + { + "anchor": { + "x": 8.063863636363637, + "y": 7.157965909090909 + }, + "prevControl": { + "x": 7.775775413999792, + "y": 7.562037701497341 + }, + "nextControl": { + "x": 8.905297371303396, + "y": 5.97777313800657 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0022613636363635, + "y": 4.220522727272727 + }, + "prevControl": { + "x": 7.984959267394379, + "y": 4.07829013593933 + }, + "nextControl": { + "x": 6.218943181818181, + "y": 4.333897727272728 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.755136363636364, + "y": 5.455602409638555 + }, + "prevControl": { + "x": 6.1364886363636355, + "y": 5.405806818181818 + }, + "nextControl": { + "x": 5.363698115753111, + "y": 5.506714991013086 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.188738636363636, + "y": 5.455602409638555 + }, + "prevControl": { + "x": 3.6695579134720697, + "y": 5.411891566265061 + }, + "nextControl": { + "x": 2.707919359255202, + "y": 5.4993132530120485 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5499545454545451, + "y": 5.0450681818181815 + }, + "prevControl": { + "x": 1.3010354393457102, + "y": 5.068290554952265 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": -111.59192207613846 + }, + { + "waypointRelativePos": 1.6741767764298092, + "rotationDegrees": -147.78271195448391 + }, + { + "waypointRelativePos": 2.897053726169844, + "rotationDegrees": 89.73393281353583 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 4.155555555555562, + "maxWaypointRelativePos": 4.75, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 3.8000000000000007, + "endWaypointRelativePos": 4.918518518518525, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 75.66492912023901 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": -90.11550613618262 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTL-QTL.path b/src/main/deploy/pathplanner/paths/TL-CTL-QTL.path new file mode 100644 index 00000000..4f5bfac5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTL-QTL.path @@ -0,0 +1,122 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 7.44444578313253 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289159, + "y": 7.44444578313253 + }, + "isLocked": false, + "linkedName": "TL" + }, + { + "anchor": { + "x": 7.826806818181819, + "y": 7.096125 + }, + "prevControl": { + "x": 7.500438812827066, + "y": 7.469960207496654 + }, + "nextControl": { + "x": 8.362448500314889, + "y": 6.482579198638692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.01866265060241, + "y": 4.483036144578313 + }, + "prevControl": { + "x": 8.005522942321718, + "y": 4.7326906019114405 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTL-QTL" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.23227548953409913, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTLD.path b/src/main/deploy/pathplanner/paths/TL-CTLD.path new file mode 100644 index 00000000..50c6b239 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTLD.path @@ -0,0 +1,112 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": null, + "nextControl": { + "x": 5.35787951807229, + "y": 7.411662650602409 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 7.247799642218247, + "y": 7.239516994633274 + }, + "prevControl": { + "x": 6.248082968474112, + "y": 7.215714216686984 + }, + "nextControl": { + "x": 7.9292665474060815, + "y": 7.255742397137746 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.253774597495529, + "y": 7.1259391771019684 + }, + "prevControl": { + "x": 7.915766001965363, + "y": 7.333944466658994 + }, + "nextControl": { + "x": 8.466689176671665, + "y": 6.994914820685885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.578282647584974, + "y": 4.464973166368515 + }, + "prevControl": { + "x": 8.643184257602861, + "y": 6.330894454382826 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTLD" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.7, + "rotationDegrees": -89.33092242292855 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.871186440677964, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.3661016949152543, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -89.70002483769608 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTR-HLF-BL-HP.path b/src/main/deploy/pathplanner/paths/TL-CTR-HLF-BL-HP.path new file mode 100644 index 00000000..fc278301 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTR-HLF-BL-HP.path @@ -0,0 +1,198 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 7.44444578313253 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289159, + "y": 7.44444578313253 + }, + "isLocked": false, + "linkedName": "TL" + }, + { + "anchor": { + "x": 8.11701204819277, + "y": 6.958333333333334 + }, + "prevControl": { + "x": 8.11408888888889, + "y": 7.640444444444445 + }, + "nextControl": { + "x": 8.12102519602201, + "y": 6.021876424801688 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.987411111111111, + "y": 4.112955555555556 + }, + "prevControl": { + "x": 8.347955555555556, + "y": 4.2298888888888895 + }, + "nextControl": { + "x": 7.420162029470755, + "y": 3.9289828804289533 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.09698888888889, + "y": 4.337077777777779 + }, + "prevControl": { + "x": 6.390501479394842, + "y": 4.049369467295772 + }, + "nextControl": { + "x": 5.653619277108436, + "y": 4.7716796519411 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.827284090909091, + "y": 5.529488636363636 + }, + "prevControl": { + "x": 6.077284090909091, + "y": 5.529488636363636 + }, + "nextControl": { + "x": 5.577284090909091, + "y": 5.529488636363636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.528863636363636, + "y": 5.529488636363636 + }, + "prevControl": { + "x": 4.426424029386704, + "y": 5.541475763293156 + }, + "nextControl": { + "x": 2.631303243340568, + "y": 5.517501509434116 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8302710413694718, + "y": 7.094992867332382 + }, + "prevControl": { + "x": 0.37222791760270146, + "y": 7.108443245974541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.44730290456431443, + "rotationDegrees": 0.7868689853072587 + }, + { + "waypointRelativePos": 1.0291777188328912, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.5875331564986732, + "rotationDegrees": -102.7057803125483 + }, + { + "waypointRelativePos": 2.4403183023872708, + "rotationDegrees": 174.87589666021177 + }, + { + "waypointRelativePos": 3.9469496021220216, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 5, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9851851851851856, + "maxWaypointRelativePos": 2.551111111111109, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.218095881161351, + "maxWaypointRelativePos": 5.75, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 5.250506414584755, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 2.0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTR-QTL-BL-TL.path b/src/main/deploy/pathplanner/paths/TL-CTR-QTL-BL-TL.path new file mode 100644 index 00000000..1376b263 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTR-QTL-BL-TL.path @@ -0,0 +1,265 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 7.44444578313253 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289159, + "y": 7.44444578313253 + }, + "isLocked": false, + "linkedName": "TL" + }, + { + "anchor": { + "x": 7.607377777777778, + "y": 7.211688888888889 + }, + "prevControl": { + "x": 7.258988780482645, + "y": 7.549637017168658 + }, + "nextControl": { + "x": 7.967868057074687, + "y": 6.862002141898319 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.607377777777778, + "y": 4.795066666666667 + }, + "prevControl": { + "x": 7.72745841852047, + "y": 5.190078577945538 + }, + "nextControl": { + "x": 7.464961351026875, + "y": 4.326579951568878 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.09698888888889, + "y": 4.561200000000001 + }, + "prevControl": { + "x": 6.336076781651726, + "y": 4.488145366100246 + }, + "nextControl": { + "x": 5.746188888888888, + "y": 4.66838888888889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.631454545454545, + "y": 5.529488636363636 + }, + "prevControl": { + "x": 5.881454545454545, + "y": 5.529488636363636 + }, + "nextControl": { + "x": 5.381454545454545, + "y": 5.529488636363636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.528863636363636, + "y": 5.529488636363636 + }, + "prevControl": { + "x": 4.425556818181818, + "y": 5.488261363636363 + }, + "nextControl": { + "x": 1.7212293386068067, + "y": 5.612598259249008 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3644318181818178, + "y": 6.312806818181819 + }, + "prevControl": { + "x": 1.341831325301205, + "y": 5.269831325301205 + }, + "nextControl": { + "x": 1.385208456206181, + "y": 7.271614415800682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.017744444444445, + "y": 7.44444578313253 + }, + "prevControl": { + "x": 1.6138225810909899, + "y": 7.392758698156791 + }, + "nextControl": { + "x": 3.456244444444445, + "y": 7.460589692101741 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.412518072289157, + "y": 7.44444578313253 + }, + "prevControl": { + "x": 3.8152874001248183, + "y": 7.44444578313253 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TL" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4580912863070636, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.6620450606585724, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.4403183023872708, + "rotationDegrees": 178.83529957430528 + }, + { + "waypointRelativePos": 3.9469496021220216, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 5.078838174273852, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 5.875518672199152, + "rotationDegrees": -0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.8210668467251732, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8858879135719221, + "maxWaypointRelativePos": 3.5017369727047347, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.135972461273671, + "maxWaypointRelativePos": 6.979068197164106, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.1944632005401583, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 5.4233250620347535, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + }, + { + "name": "launcherLow", + "waypointRelativePos": 7.281389578163752, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 0.13923629139309768 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTR-QTR.path b/src/main/deploy/pathplanner/paths/TL-CTR-QTR.path new file mode 100644 index 00000000..4521a9bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTR-QTR.path @@ -0,0 +1,100 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.7187228915662653, + "y": 7.324240963855422 + }, + "prevControl": null, + "nextControl": { + "x": 3.7187228915662653, + "y": 7.324240963855422 + }, + "isLocked": false, + "linkedName": "TL-Long" + }, + { + "anchor": { + "x": 6.204662650602408, + "y": 7.324240963855422 + }, + "prevControl": { + "x": 5.61694243289291, + "y": 7.411960399334451 + }, + "nextControl": { + "x": 6.936819277108433, + "y": 7.214963855421687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.346722891566266, + "y": 4.668807228915662 + }, + "prevControl": { + "x": 6.324867469879518, + "y": 5.739722891566264 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTR-QTRL" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6190871369294697, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4655601659750637, + "rotationDegrees": -85.29264819406745 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.41638981173864464, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.33945409429278844, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.1883893275273 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-CTR.path b/src/main/deploy/pathplanner/paths/TL-CTR.path new file mode 100644 index 00000000..2132c29b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-CTR.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 7.44444578313253 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289158, + "y": 7.44444578313253 + }, + "isLocked": false, + "linkedName": "TL" + }, + { + "anchor": { + "x": 7.901197434069852, + "y": 7.104141414141413 + }, + "prevControl": { + "x": 7.751060379006985, + "y": 7.392525458486395 + }, + "nextControl": { + "x": 8.1554797979798, + "y": 6.615714513272424 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.901197434069852, + "y": 1.3971240199572332 + }, + "prevControl": { + "x": 7.838688524590165, + "y": 6.01028153955809 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.39438596491227657, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8940350877192969, + "rotationDegrees": -90.47983053172646 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.36724565756823013, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.21887523513125 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-DIS.path b/src/main/deploy/pathplanner/paths/TL-DIS.path new file mode 100644 index 00000000..1bfcd517 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-DIS.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": null, + "nextControl": { + "x": 5.35787951807229, + "y": 7.411662650602409 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 6.658033175355451, + "y": 7.355651658767772 + }, + "prevControl": { + "x": 5.658033175355451, + "y": 7.355651658767772 + }, + "nextControl": { + "x": 7.658033175355451, + "y": 7.355651658767772 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.065817535545024, + "y": 7.172962085308057 + }, + "prevControl": { + "x": 7.726873969729322, + "y": 7.547583921209622 + }, + "nextControl": { + "x": 8.474182464454977, + "y": 6.7216113744075825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.22701421800948, + "y": 1.3913744075829368 + }, + "prevControl": { + "x": 8.495675355450237, + "y": 1.5740639810426522 + }, + "nextControl": { + "x": 7.768819494561197, + "y": 1.0798019956381042 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.905781990521327, + "y": 1.3913744075829368 + }, + "prevControl": { + "x": 5.60219490521327, + "y": 1.4048074644549753 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HSE" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.152753108348143, + "rotationDegrees": -89.92333279619605 + }, + { + "waypointRelativePos": 2.678507992895191, + "rotationDegrees": -89.64712703383984 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.56170533256645 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-Dep.path b/src/main/deploy/pathplanner/paths/TL-Dep.path new file mode 100644 index 00000000..d2251556 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-Dep.path @@ -0,0 +1,98 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": null, + "nextControl": { + "x": 3.2978352393552344, + "y": 7.411662650602409 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 2.0859090909090914, + "y": 7.226694312796208 + }, + "prevControl": { + "x": 3.12251698647997, + "y": 6.968500234193897 + }, + "nextControl": { + "x": 0.9484337349397589, + "y": 7.51001204819277 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7002146464646473, + "y": 7.104141414141413 + }, + "prevControl": { + "x": 0.7835506113356967, + "y": 7.339842744131323 + }, + "nextControl": { + "x": 0.445759036144578, + "y": 6.384457831325301 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7231186868686872, + "y": 5.523762626262627 + }, + "prevControl": { + "x": 0.6772721918586281, + "y": 6.15409753636621 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DEPOT" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.073684210526315, + "rotationDegrees": -109.12578116282143 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.5210918114144006, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.2, + "rotation": -114.56717132151371 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-QTLCTLSHORT.path b/src/main/deploy/pathplanner/paths/TL-QTLCTLSHORT.path new file mode 100644 index 00000000..cefed2ff --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-QTLCTLSHORT.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 7.44444578313253 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289157, + "y": 7.444445783132531 + }, + "isLocked": false, + "linkedName": "TL" + }, + { + "anchor": { + "x": 6.944755555555556, + "y": 7.338366666666666 + }, + "prevControl": { + "x": 6.681655555555556, + "y": 7.348111111111108 + }, + "nextControl": { + "x": 7.579309221699307, + "y": 7.314864679031716 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.816500000000001, + "y": 5.849000000000001 + }, + "prevControl": { + "x": 7.777628571428572, + "y": 6.548685714285716 + }, + "nextControl": { + "x": 7.844376993942099, + "y": 5.347214109042251 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.752453637660485, + "y": 4.882482168330956 + }, + "prevControl": { + "x": 7.752453637660485, + "y": 5.674565357246909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTL-QTLShort" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6673228346456703, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4586614173228343, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constlaints Zone", + "minWaypointRelativePos": 1.6223776223776203, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constlaints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.7354838709677294, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.6044665012406814, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 0.7412587412587424, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-QTRH.path b/src/main/deploy/pathplanner/paths/TL-QTRH.path new file mode 100644 index 00000000..66fc0d77 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-QTRH.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": null, + "nextControl": { + "x": 5.35787951807229, + "y": 7.411662650602409 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 6.947746987951808, + "y": 7.25867469879518 + }, + "prevControl": { + "x": 6.49665530561821, + "y": 7.429094936741865 + }, + "nextControl": { + "x": 7.548743489107949, + "y": 7.031621151271753 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.614337349397591, + "y": 4.8982891566265065 + }, + "prevControl": { + "x": 7.71268674698795, + "y": 6.373530120481928 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "QTRLH" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.28381742738588983, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.0970954356846392, + "rotationDegrees": -106.63717271459142 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.9151364764267971, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.43076923076921525, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -118.17859010995926 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TL-QTRHLong.path b/src/main/deploy/pathplanner/paths/TL-QTRHLong.path new file mode 100644 index 00000000..5bb71104 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TL-QTRHLong.path @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.35787951807229, + "y": 7.411662650602409 + }, + "prevControl": null, + "nextControl": { + "x": 5.35787951807229, + "y": 7.411662650602409 + }, + "isLocked": false, + "linkedName": "TLS" + }, + { + "anchor": { + "x": 6.947746987951808, + "y": 7.25867469879518 + }, + "prevControl": { + "x": 6.482007633724585, + "y": 7.38362915968541 + }, + "nextControl": { + "x": 7.395783132530121, + "y": 7.138469879518072 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.58155421686747, + "y": 4.1442771084337355 + }, + "prevControl": { + "x": 7.450421686746989, + "y": 5.608590361445783 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "QTRHLong" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.28381742738588983, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.0970954356846392, + "rotationDegrees": -51.64768397768135 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.9151364764267971, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.2803970223324992, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.43076923076921525, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.22216863363612 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TLBack-CTL-QTL-BL-L.path b/src/main/deploy/pathplanner/paths/TLBack-CTL-QTL-BL-L.path new file mode 100644 index 00000000..32f5d196 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TLBack-CTL-QTL-BL-L.path @@ -0,0 +1,237 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.582922222222222, + "y": 7.455299999999999 + }, + "prevControl": null, + "nextControl": { + "x": 4.582922222222224, + "y": 7.455299999999999 + }, + "isLocked": false, + "linkedName": "TLback" + }, + { + "anchor": { + "x": 8.063863636363637, + "y": 7.157965909090909 + }, + "prevControl": { + "x": 7.775775413999792, + "y": 7.562037701497341 + }, + "nextControl": { + "x": 8.905297371303396, + "y": 5.97777313800657 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.183152639087017, + "y": 4.546077032810271 + }, + "prevControl": { + "x": 8.175550776987984, + "y": 4.513347596846524 + }, + "nextControl": { + "x": 6.448765081629416, + "y": 4.57029724203519 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.755136363636364, + "y": 5.455602409638555 + }, + "prevControl": { + "x": 6.1364886363636355, + "y": 5.405806818181818 + }, + "nextControl": { + "x": 5.363698115753111, + "y": 5.506714991013086 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.188738636363636, + "y": 5.455602409638555 + }, + "prevControl": { + "x": 3.6695579134720697, + "y": 5.411891566265061 + }, + "nextControl": { + "x": 2.707919359255202, + "y": 5.4993132530120485 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8949643366619111, + "y": 5.296519258202568 + }, + "prevControl": { + "x": 0.9570581742288374, + "y": 5.054353291691557 + }, + "nextControl": { + "x": 0.7975198922174667, + "y": 5.6765525915359 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5331807228915662, + "y": 7.309133333333333 + }, + "prevControl": { + "x": 0.49419007952331323, + "y": 7.0621925920010655 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": -111.59192207613846 + }, + { + "waypointRelativePos": 1.6741767764298092, + "rotationDegrees": -147.78271195448391 + }, + { + "waypointRelativePos": 2.897053726169844, + "rotationDegrees": 89.73393281353583 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.854771784232385, + "rotationDegrees": 130.53996629715718 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.6158001350438886, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 5.079900744416839, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.115136476426812, + "maxWaypointRelativePos": 5.08, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 4.278190411883858, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TLback-CTL-QTL.path b/src/main/deploy/pathplanner/paths/TLback-CTL-QTL.path new file mode 100644 index 00000000..fabc5f65 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TLback-CTL-QTL.path @@ -0,0 +1,122 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.582922222222222, + "y": 7.455299999999999 + }, + "prevControl": null, + "nextControl": { + "x": 4.582922222222224, + "y": 7.455299999999999 + }, + "isLocked": false, + "linkedName": "TLback" + }, + { + "anchor": { + "x": 7.826806818181819, + "y": 7.096125 + }, + "prevControl": { + "x": 7.500438812827066, + "y": 7.469960207496654 + }, + "nextControl": { + "x": 8.362448500314889, + "y": 6.482579198638692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.958177777777777, + "y": 4.054488888888889 + }, + "prevControl": { + "x": 7.945038069497086, + "y": 4.304143346222016 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "QTLLong" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.23227548953409913, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.13923629139309768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-HALF.path b/src/main/deploy/pathplanner/paths/TR-CTR-HALF.path new file mode 100644 index 00000000..f0cb14c3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-HALF.path @@ -0,0 +1,113 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": null, + "nextControl": { + "x": 4.627189616703165, + "y": 0.6147000000000002 + }, + "isLocked": false, + "linkedName": "trsj" + }, + { + "anchor": { + "x": 5.48308888888889, + "y": 0.6364819277108438 + }, + "prevControl": { + "x": 4.456655922164885, + "y": 0.6091103819315393 + }, + "nextControl": { + "x": 6.213922222222224, + "y": 0.6559708165997311 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.215590361445784, + "y": 3.2809879518072296 + }, + "prevControl": { + "x": 6.226518072289158, + "y": 2.40677108433735 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "nWall" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9477178423236537, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4622406639004049, + "rotationDegrees": 89.93611487106567 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.035437430786267494, + "maxWaypointRelativePos": 0.26578073089701393, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.3439205955334774, + "maxWaypointRelativePos": 1.891811414392056, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.39900744416872924, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-HLF-BR-HP.path b/src/main/deploy/pathplanner/paths/TR-CTR-HLF-BR-HP.path new file mode 100644 index 00000000..cab1d1f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-HLF-BR-HP.path @@ -0,0 +1,223 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.52151212553495, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.521512125534952, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TR" + }, + { + "anchor": { + "x": 8.11701204819277, + "y": 1.1116666666666664 + }, + "prevControl": { + "x": 8.11408888888889, + "y": 0.42955555555555525 + }, + "nextControl": { + "x": 8.12102519602201, + "y": 2.048123575198313 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.11701204819277, + "y": 4.220522727272727 + }, + "prevControl": { + "x": 8.477556492637214, + "y": 4.1035893939393935 + }, + "nextControl": { + "x": 7.5497629665524135, + "y": 4.40449540239933 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.09698888888889, + "y": 3.7329222222222214 + }, + "prevControl": { + "x": 6.390501479394842, + "y": 4.020630532704228 + }, + "nextControl": { + "x": 5.653619277108436, + "y": 3.2983203480589003 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.806670454545455, + "y": 2.5405113636363637 + }, + "prevControl": { + "x": 6.056670454545455, + "y": 2.5405113636363637 + }, + "nextControl": { + "x": 5.556670454545455, + "y": 2.5405113636363637 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.528863636363636, + "y": 2.5405113636363637 + }, + "prevControl": { + "x": 4.425556818181818, + "y": 2.5817386363636365 + }, + "nextControl": { + "x": 1.7212293386068067, + "y": 2.4574017407509916 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8252444444444447, + "y": 2.154322222222222 + }, + "prevControl": { + "x": 1.0623012626262627, + "y": 2.6593563131313123 + }, + "nextControl": { + "x": 0.6646317814276703, + "y": 1.8121474184038764 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.47, + "y": 0.7413777777777772 + }, + "prevControl": { + "x": 0.46999999999999986, + "y": 1.266220406915744 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "hp" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.44730290456431443, + "rotationDegrees": -0.7868689853072587 + }, + { + "waypointRelativePos": 1.0291777188328912, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.5875331564986732, + "rotationDegrees": 102.7057803125483 + }, + { + "waypointRelativePos": 2.4403183023872708, + "rotationDegrees": -174.87589666021177 + }, + { + "waypointRelativePos": 3.9469496021220216, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.47703703703703715, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9851851851851856, + "maxWaypointRelativePos": 2.260740740740742, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.398511166253143, + "maxWaypointRelativePos": 6.875555555555566, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 5.315136476426798, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.07937580765068 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 2.0, + "rotation": -0.08211332881449039 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP Longer.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP Longer.path new file mode 100644 index 00000000..4e88586d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP Longer.path @@ -0,0 +1,231 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": null, + "nextControl": { + "x": 4.5342, + "y": 0.6147000000000002 + }, + "isLocked": false, + "linkedName": "trsj" + }, + { + "anchor": { + "x": 7.623067047075606, + "y": 0.9232524964336677 + }, + "prevControl": { + "x": 7.2556772059062435, + "y": 0.5896456291649358 + }, + "nextControl": { + "x": 8.748730385164052, + "y": 1.9454065620542096 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.623067047075606, + "y": 4.287303851640513 + }, + "prevControl": { + "x": 8.465829545454545, + "y": 4.065920454545455 + }, + "nextControl": { + "x": 6.070512182805478, + "y": 4.69514101892039 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.765677777777778, + "y": 2.6902666666666666 + }, + "prevControl": { + "x": 5.904105840957745, + "y": 2.9657977496290453 + }, + "nextControl": { + "x": 5.588456984128626, + "y": 2.3375214495778387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.07933734939759, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 3.560156626506024, + "y": 2.505120481927711 + }, + "nextControl": { + "x": 2.6093551789393854, + "y": 2.4186839866943797 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3636867469879512, + "y": 1.7073975903614462 + }, + "prevControl": { + "x": 2.063060240963855, + "y": 2.406771084337348 + }, + "nextControl": { + "x": 0.6605244289301885, + "y": 1.0042352723036858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.7960111111111112, + "y": 1.1311555555555548 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 77.60386375779406 + }, + { + "waypointRelativePos": 1.7014925373134253, + "rotationDegrees": 124.52672781089822 + }, + { + "waypointRelativePos": 2.897053726169844, + "rotationDegrees": -89.73393281353583 + }, + { + "waypointRelativePos": 4.1532062391681, + "rotationDegrees": -89.57446389529602 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 4.132343011478706, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.4109181141439192, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 4.278190411883857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP.path new file mode 100644 index 00000000..24dcc73f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-HP.path @@ -0,0 +1,220 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": null, + "nextControl": { + "x": 4.534200000000002, + "y": 0.6147000000000002 + }, + "isLocked": false, + "linkedName": "trsj" + }, + { + "anchor": { + "x": 8.291855421686748, + "y": 1.0954457831325295 + }, + "prevControl": { + "x": 8.003767199322903, + "y": 0.6913739907260976 + }, + "nextControl": { + "x": 9.133289156626507, + "y": 2.2756385542168687 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.80793152639087, + "y": 3.756818830242511 + }, + "prevControl": { + "x": 7.698583535820807, + "y": 4.269618472035503 + }, + "nextControl": { + "x": 6.447317068559546, + "y": 3.5491923242184162 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.647349397590362, + "y": 2.6143975903614454 + }, + "prevControl": { + "x": 5.896485837151582, + "y": 2.5936362203980106 + }, + "nextControl": { + "x": 5.253951807228916, + "y": 2.647180722891566 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.07933734939759, + "y": 2.461409638554217 + }, + "prevControl": { + "x": 3.560156626506024, + "y": 2.505120481927711 + }, + "nextControl": { + "x": 2.6093551789393854, + "y": 2.4186839866943797 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3636867469879512, + "y": 1.7073975903614462 + }, + "prevControl": { + "x": 2.063060240963855, + "y": 2.406771084337348 + }, + "nextControl": { + "x": 0.6605244289301885, + "y": 1.0042352723036858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.4265952904665635, + "y": 0.5551283829688684 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 111.59192207613846 + }, + { + "waypointRelativePos": 1.6741767764298092, + "rotationDegrees": -176.88494593076837 + }, + { + "waypointRelativePos": 2.897053726169844, + "rotationDegrees": -89.73393281353583 + }, + { + "waypointRelativePos": 4.1532062391681, + "rotationDegrees": -73.04060721630577 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "moveShoot", + "minWaypointRelativePos": 4.132343011478706, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 4.278190411883857, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-TR.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-TR.path new file mode 100644 index 00000000..78081776 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTR-BR-TR.path @@ -0,0 +1,256 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.52151212553495, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.521512125534952, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TR" + }, + { + "anchor": { + "x": 7.607377777777778, + "y": 0.858311111111111 + }, + "prevControl": { + "x": 7.258988780482645, + "y": 0.5203629828313425 + }, + "nextControl": { + "x": 7.967868057074687, + "y": 1.2079978581016813 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.607377777777778, + "y": 3.2749333333333333 + }, + "prevControl": { + "x": 7.72745841852047, + "y": 2.8799214220544616 + }, + "nextControl": { + "x": 7.464961351026875, + "y": 3.7434200484311226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.09698888888889, + "y": 3.508799999999999 + }, + "prevControl": { + "x": 6.336076781651726, + "y": 3.581854633899754 + }, + "nextControl": { + "x": 5.746188888888888, + "y": 3.4016111111111105 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.755136363636364, + "y": 2.5405113636363637 + }, + "prevControl": { + "x": 6.005136363636364, + "y": 2.5405113636363637 + }, + "nextControl": { + "x": 5.505136363636364, + "y": 2.5405113636363637 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.528863636363636, + "y": 2.5405113636363637 + }, + "prevControl": { + "x": 4.425556818181818, + "y": 2.5817386363636365 + }, + "nextControl": { + "x": 1.7212293386068067, + "y": 2.4574017407509916 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3644318181818178, + "y": 1.7571931818181814 + }, + "prevControl": { + "x": 1.341831325301205, + "y": 2.8001686746987957 + }, + "nextControl": { + "x": 1.385208456206181, + "y": 0.7983855841993184 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.017744444444445, + "y": 0.6255542168674697 + }, + "prevControl": { + "x": 1.6138225810909899, + "y": 0.6772413018432097 + }, + "nextControl": { + "x": 3.456244444444445, + "y": 0.6094103078982596 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.52151212553495, + "y": 0.6515406562054205 + }, + "prevControl": { + "x": 2.9257829421346813, + "y": 0.693863413902876 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TR" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4448132780083091, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.1618257261410645, + "rotationDegrees": 89.8246005599076 + }, + { + "waypointRelativePos": 1.6620450606585724, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.4403183023872708, + "rotationDegrees": -178.83529957430528 + }, + { + "waypointRelativePos": 3.9469496021220216, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 5.078838174273852, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 5.875518672199152, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 2.1359801488833874, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 5.264516129032287, + "maxWaypointRelativePos": 8.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.19446320054017677, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 5.248635235732003, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + }, + { + "name": "launcherLow", + "waypointRelativePos": 7.281389578163752, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherLow" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -0.08211332881449039 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -0.08211332881449039 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTR.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTR.path new file mode 100644 index 00000000..24a90351 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTR.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 0.49442168674698794 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289159, + "y": 0.49442168674698794 + }, + "isLocked": false, + "linkedName": "TRSide" + }, + { + "anchor": { + "x": 7.402744444444444, + "y": 0.8290777777777771 + }, + "prevControl": { + "x": 7.0763764390896915, + "y": 0.455242570281123 + }, + "nextControl": { + "x": 7.938386126577512, + "y": 1.442623579139085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.821755555555557, + "y": 3.5965 + }, + "prevControl": { + "x": 7.808615847274865, + "y": 3.3468455426668724 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTR-QTR" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 90.81594493331889 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 89.06764595078421 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.1728561782579309, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTRAngled.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTRAngled.path new file mode 100644 index 00000000..b224a5bf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTRAngled.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 0.49442168674698794 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289159, + "y": 0.49442168674698794 + }, + "isLocked": false, + "linkedName": "TRSide" + }, + { + "anchor": { + "x": 7.461349397590361, + "y": 0.8987469879518073 + }, + "prevControl": { + "x": 7.134981392235608, + "y": 0.5249117804551532 + }, + "nextControl": { + "x": 7.996991079723431, + "y": 1.512292789313115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.98587951807229, + "y": 3.1717108433734937 + }, + "prevControl": { + "x": 7.972739809791598, + "y": 2.9220563860403663 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "QTR" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.0223325062034787, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 0.9548387096774164, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.3366555924695484, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTRLong.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTRLong.path new file mode 100644 index 00000000..d3796cb6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTRLong.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": null, + "nextControl": { + "x": 4.534200000000002, + "y": 0.6147000000000002 + }, + "isLocked": false, + "linkedName": "trsj" + }, + { + "anchor": { + "x": 7.1444457831325305, + "y": 1.008024096385542 + }, + "prevControl": { + "x": 6.818077777777778, + "y": 0.6341888888888879 + }, + "nextControl": { + "x": 7.680087465265601, + "y": 1.6215698977468498 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.592481927710843, + "y": 3.7399518072289153 + }, + "prevControl": { + "x": 7.579342219430152, + "y": 3.490297349895788 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "QTRLong" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 89.06764595078421 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.3366555924695484, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR-QTRShort.path b/src/main/deploy/pathplanner/paths/TR-CTR-QTRShort.path new file mode 100644 index 00000000..bcdfea74 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR-QTRShort.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412518072289157, + "y": 0.49442168674698794 + }, + "prevControl": null, + "nextControl": { + "x": 5.412518072289159, + "y": 0.49442168674698794 + }, + "isLocked": false, + "linkedName": "TRSide" + }, + { + "anchor": { + "x": 7.509933333333333, + "y": 0.8777999999999986 + }, + "prevControl": { + "x": 7.271317836812465, + "y": 0.44267762399135663 + }, + "nextControl": { + "x": 7.841244444444444, + "y": 1.4819555555555552 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.752453637660485, + "y": 3.187517831669045 + }, + "prevControl": { + "x": 7.739313929379794, + "y": 2.9378633743359175 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTR-QTRShort" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.2464730290456414, + "rotationDegrees": 89.06764595078421 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.23227548953409913, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTR.path b/src/main/deploy/pathplanner/paths/TR-CTR.path new file mode 100644 index 00000000..28e41976 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTR.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.52151212553495, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.521512125534951, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TR" + }, + { + "anchor": { + "x": 7.614337349397591, + "y": 0.8768915662650603 + }, + "prevControl": { + "x": 6.97674647270479, + "y": 0.6393577102422535 + }, + "nextControl": { + "x": 8.107072430228255, + "y": 1.0604595375549144 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.756397590361445, + "y": 6.898060240963855 + }, + "prevControl": { + "x": 7.778253012048193, + "y": 4.909216867469879 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5291228070175418, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.090526315789475, + "rotationDegrees": 89.1329986792534 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.3942414174972331, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 1.0365448504983563, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 89.42127443439227 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -0.08211332881449039 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-CTRD.path b/src/main/deploy/pathplanner/paths/TR-CTRD.path new file mode 100644 index 00000000..5635daf7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-CTRD.path @@ -0,0 +1,101 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.379734939759036, + "y": 0.6036987951807202 + }, + "prevControl": null, + "nextControl": { + "x": 5.3169767441860465, + "y": 0.7169051878354196 + }, + "isLocked": false, + "linkedName": "TRS" + }, + { + "anchor": { + "x": 7.426279069767442, + "y": 0.702399999999999 + }, + "prevControl": { + "x": 6.426279069767442, + "y": 0.7023999999999991 + }, + "nextControl": { + "x": 8.426279069767439, + "y": 0.702399999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.480930232558139, + "y": 3.637477638640429 + }, + "prevControl": { + "x": 8.659409660107332, + "y": 1.5930769230769224 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTRD" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.272727272727273, + "rotationDegrees": 88.54844195970549 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.2542372881355925, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.37966101694915244, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.36342295838341 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.6213872948567607 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-HP.path b/src/main/deploy/pathplanner/paths/TR-HP.path new file mode 100644 index 00000000..6d585744 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-HP.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5342000000000002, + "y": 0.6147000000000002 + }, + "prevControl": null, + "nextControl": { + "x": 2.33714797979798, + "y": 1.1301030303030326 + }, + "isLocked": false, + "linkedName": "trsj" + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 1.8903041255932833, + "y": 1.18344696969697 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.20376522702104335, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TR-QTRCTRSHORT.path b/src/main/deploy/pathplanner/paths/TR-QTRCTRSHORT.path new file mode 100644 index 00000000..c5e31813 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TR-QTRCTRSHORT.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.52151212553495, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.52151212553495, + "y": 0.6515406562054202 + }, + "isLocked": false, + "linkedName": "TR" + }, + { + "anchor": { + "x": 6.983733333333333, + "y": 0.7413777777777772 + }, + "prevControl": { + "x": 5.983873082246567, + "y": 0.7246601733829139 + }, + "nextControl": { + "x": 7.618633333333333, + "y": 0.7519932683113415 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.816500000000001, + "y": 2.220999999999999 + }, + "prevControl": { + "x": 7.777628571428572, + "y": 1.521314285714285 + }, + "nextControl": { + "x": 7.844376993942099, + "y": 2.7227858909577494 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.752453637660485, + "y": 3.187517831669045 + }, + "prevControl": { + "x": 7.752453637660485, + "y": 2.3954346427530915 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTR-QTRShort" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8489626556016647, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4586614173228343, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6223776223776203, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.7354838709677294, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.6044665012406814, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + }, + { + "name": "launcherPrep", + "waypointRelativePos": 0.7412587412587424, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -0.08211332881449039 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TRBack-CTR-HALF.path b/src/main/deploy/pathplanner/paths/TRBack-CTR-HALF.path new file mode 100644 index 00000000..eed045c0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TRBack-CTR-HALF.path @@ -0,0 +1,113 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9919156626506025, + "y": 0.680192771084337 + }, + "prevControl": null, + "nextControl": { + "x": 4.084905279353768, + "y": 0.680192771084337 + }, + "isLocked": false, + "linkedName": "TRBack" + }, + { + "anchor": { + "x": 5.48308888888889, + "y": 0.6364819277108438 + }, + "prevControl": { + "x": 4.456655922164885, + "y": 0.6091103819315393 + }, + "nextControl": { + "x": 6.213922222222224, + "y": 0.6559708165997311 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.313939759036145, + "y": 3.3684096385542173 + }, + "prevControl": { + "x": 6.324867469879519, + "y": 2.4941927710843377 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "nWallNew" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9477178423236537, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4622406639004049, + "rotationDegrees": 89.93611487106567 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.035437430786267494, + "maxWaypointRelativePos": 0.26578073089701393, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.3439205955334774, + "maxWaypointRelativePos": 1.891811414392056, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.39900744416872924, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TRSide-CTR-QTR.path b/src/main/deploy/pathplanner/paths/TRSide-CTR-QTR.path new file mode 100644 index 00000000..d844ca7d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TRSide-CTR-QTR.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.52151212553495, + "y": 0.6515406562054205 + }, + "prevControl": null, + "nextControl": { + "x": 4.521512125534952, + "y": 0.6515406562054205 + }, + "isLocked": false, + "linkedName": "TR" + }, + { + "anchor": { + "x": 7.539166666666667, + "y": 0.8485666666666658 + }, + "prevControl": { + "x": 7.212798661311914, + "y": 0.4747314591700117 + }, + "nextControl": { + "x": 8.074808348799735, + "y": 1.4621124680279736 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.821755555555557, + "y": 3.5965 + }, + "prevControl": { + "x": 7.808615847274865, + "y": 3.3468455426668724 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CTR-QTR" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6993603411513875, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "trench", + "minWaypointRelativePos": 0.06644518272425252, + "maxWaypointRelativePos": 0.47841191066996414, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "intake", + "minWaypointRelativePos": 1.2129032258064532, + "maxWaypointRelativePos": 1.9116625310173645, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "hopper", + "minWaypointRelativePos": 0.3831265508684809, + "maxWaypointRelativePos": 1.3439205955334927, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intakeIntake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "intakeIntake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 9.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -0.08211332881449039 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Wait-TL-BR-HP.path b/src/main/deploy/pathplanner/paths/Wait-TL-BR-HP.path new file mode 100644 index 00000000..3c977c0e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Wait-TL-BR-HP.path @@ -0,0 +1,161 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.798716119828815, + "y": 7.455 + }, + "prevControl": null, + "nextControl": { + "x": 6.798716119828814, + "y": 7.455 + }, + "isLocked": false, + "linkedName": "Wait TL" + }, + { + "anchor": { + "x": 8.040518072289156, + "y": 5.980132530120482 + }, + "prevControl": { + "x": 8.01866265060241, + "y": 6.668578313253013 + }, + "nextControl": { + "x": 8.072248119123929, + "y": 4.980636054825117 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.979857346647646, + "y": 2.516048192771084 + }, + "prevControl": { + "x": 7.998288159771755, + "y": 2.4772322155956203 + }, + "nextControl": { + "x": 5.4857400878531415, + "y": 2.525550447747902 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.978088445078458, + "y": 2.516048192771084 + }, + "prevControl": { + "x": 3.244340375662255, + "y": 2.516048192771084 + }, + "nextControl": { + "x": 2.641683309557773, + "y": 2.516048192771084 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.407397590361446, + "y": 1.7729638554216867 + }, + "prevControl": { + "x": 1.6065905848787443, + "y": 1.9324679029957212 + }, + "nextControl": { + "x": 1.2122519261459967, + "y": 1.6167007128131021 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.475, + "y": 0.8003975903614469 + }, + "prevControl": { + "x": 0.21273493975903635, + "y": 1.0408072289156645 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HP" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0554371002132203, + "rotationDegrees": -88.53233570020205 + }, + { + "waypointRelativePos": 1.5778251599147102, + "rotationDegrees": -115.0070378305669 + }, + { + "waypointRelativePos": 1.97, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.899786780383779, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6347062795408398, + "maxWaypointRelativePos": 1.7555705604321312, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 3.308575286968268, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Left", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Wait-TR-BL-Depot.path b/src/main/deploy/pathplanner/paths/Wait-TR-BL-Depot.path new file mode 100644 index 00000000..870716e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Wait-TR-BL-Depot.path @@ -0,0 +1,169 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.798716119828815, + "y": 0.652 + }, + "prevControl": null, + "nextControl": { + "x": 6.798716119828814, + "y": 0.6520000000000001 + }, + "isLocked": false, + "linkedName": "Wait TR" + }, + { + "anchor": { + "x": 8.101797432239657, + "y": 1.583124108416548 + }, + "prevControl": { + "x": 7.7265763195435095, + "y": 0.9879457917261072 + }, + "nextControl": { + "x": 8.635098355615177, + "y": 2.4290497110122007 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.682268188302425, + "y": 5.412967189728959 + }, + "prevControl": { + "x": 7.700699001426534, + "y": 5.374151212553495 + }, + "nextControl": { + "x": 5.188150929507921, + "y": 5.422469444705777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1592296718972896, + "y": 5.412967189728959 + }, + "prevControl": { + "x": 3.4254816024810864, + "y": 5.412967189728959 + }, + "nextControl": { + "x": 2.8228245363766047, + "y": 5.412967189728959 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8690870185449355, + "y": 5.154194008559203 + }, + "prevControl": { + "x": 1.107470710373314, + "y": 5.078873257963681 + }, + "nextControl": { + "x": 0.6307033267165572, + "y": 5.229514759154724 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.6491298145506419, + "y": 7.237318116975749 + }, + "prevControl": { + "x": 0.6313180520536639, + "y": 7.486682791933433 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0554371002132203, + "rotationDegrees": 90.70411766469275 + }, + { + "waypointRelativePos": 1.5138592750533022, + "rotationDegrees": 119.54102651288274 + }, + { + "waypointRelativePos": 1.6844349680170545, + "rotationDegrees": 126.27436497625904 + }, + { + "waypointRelativePos": 1.97, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.899786780383779, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.019189765458408, + "rotationDegrees": 126.18808384030064 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6347062795408398, + "maxWaypointRelativePos": 1.7555705604321312, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "launcherPrep", + "waypointRelativePos": 3.308575286968268, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "launcherPrep" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 93.36646066342978 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 7642c343..53dbed7c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,21 +1,33 @@ { - "robotWidth": 0.8, - "robotLength": 0.8, + "robotWidth": 0.89535, + "robotLength": 0.89535, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], - "defaultMaxVel": 5.25, - "defaultMaxAccel": 10.0, + "pathFolders": [ + "Center", + "Left", + "Misc", + "Right" + ], + "autoFolders": [ + "AllianceSide", + "Center", + "Left", + "Orbit", + "Right", + "Worlds" + ], + "defaultMaxVel": 4.0, + "defaultMaxAccel": 2.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 40.0, - "robotMOI": 6.883, + "robotMass": 54.5, + "robotMOI": 8.0968, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.1016, - "driveGearing": 5.143, + "driveWheelRadius": 0.051, + "driveGearing": 6.0, "maxDriveSpeed": 5.45, - "driveMotorType": "NEO", + "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, @@ -29,6 +41,8 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [ - "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.3,\"y\":0.0},\"size\":{\"width\":0.2,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.3,\"y\":0.0},\"size\":{\"width\":0.2,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", + "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.65386,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Robot Perimeter\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"size\":{\"width\":0.6858,\"length\":0.6858},\"borderRadius\":0.05,\"strokeWidth\":0.01,\"filled\":false}}" ] } diff --git a/src/main/deploy/rebuilt_robot/Programming b/src/main/deploy/rebuilt_robot/Programming new file mode 100644 index 00000000..5eff49a6 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/Programming @@ -0,0 +1,2925 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [ + { + "title": "HubStatus", + "x": 1024.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "AutoWinner", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/AutoWinner", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "CurrentShift", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/CurrentShift", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "MatchTime", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/MatchTime", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "NextShift", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/NextShift", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "TimeRemainingInCurrentShift", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/TimeRemainingInCurrentShift", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "IsActiveNext", + "x": 896.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/HubStatus/IsActiveNext", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "ActiveNow", + "x": 896.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/HubStatus/ActiveNow", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Auto Modes", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Rebuilt/Auto Modes", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "HopperAngleDouble", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/HopperAngleDouble", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StateCurrent", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 512.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "Launcher", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 768.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CoverageSatisfiesRange", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/CoverageSatisfiesRange", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "FlyWheelMotorOutput", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelMotorOutput", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedActual", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedCalculated", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedDesired", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedError", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleActual", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleCalculated", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleDesired", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleError", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodVelocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TargetDistance", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TargetDistance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleActual", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleCalculated", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleDesired", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleError", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretVelocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "UniqueCoverage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/UniqueCoverage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "translation", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "x", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Vision/Camera rear-prometheus/LatestTargetPose", + "period": 0.06, + "data_type": "double", + "struct_meta": { + "path": [ + "translation", + "x" + ], + "schema_name": "Pose3d", + "type": "double" + }, + "show_submit_button": false + } + }, + { + "title": "y", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Vision/Camera rear-prometheus/LatestTargetPose", + "period": 0.06, + "data_type": "double", + "struct_meta": { + "path": [ + "translation", + "y" + ], + "schema_name": "Pose3d", + "type": "double" + }, + "show_submit_button": false + } + }, + { + "title": "z", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Vision/Camera rear-prometheus/LatestTargetPose", + "period": 0.06, + "data_type": "double", + "struct_meta": { + "path": [ + "translation", + "z" + ], + "schema_name": "Pose3d", + "type": "double" + }, + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "Pose Field", + "x": 0.0, + "y": 0.0, + "width": 768.0, + "height": 384.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/DrivePoseEstimator/Pose Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "FlyWheelSpeedAtGoal", + "x": 1024.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "HoodAngleAtGoal", + "x": 1152.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "TurretAngleAtGoal", + "x": 1280.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "StateCurrent", + "x": 1024.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 1152.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "SimulatedGamepieces", + "x": 1280.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/SimulatedGamepieces", + "period": 0.06, + "data_type": "int", + "show_submit_button": false + } + }, + { + "title": "hasTarget", + "x": 512.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/photonvision/rear-prometheus/hasTarget", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "HasTarget", + "x": 640.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Vision/Camera rear-prometheus/HasTarget", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Connected", + "x": 640.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Vision/Camera rear-prometheus/Connected", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "left_Port_1184_Output_MJPEG_Server", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/left_Port_1184_Output_MJPEG_Server", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Auto Modes", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Rebuilt/Auto Modes", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Robot Pose3d", + "x": 128.0, + "y": 512.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Pose/Robot Pose3d", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Hood", + "grid_layout": { + "layouts": [ + { + "title": "feedback", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedback/kD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kI", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedback/kI", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedback/kP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "feedforward", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kA", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kA", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kG", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kG", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kS", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kS", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kV", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/feedforward/kV", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "setpoint", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/setpoint/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "motionprofile", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "maxAcceleration", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/motionprofile/maxAcceleration", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "maxVelocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/closedloop/motionprofile/maxVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "hoodMotor", + "x": 768.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Down", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/hoodMotor/Down", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Up", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/hoodMotor/Up", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "ZeroEncoder", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/hoodMotor/ZeroEncoder", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + } + ] + }, + { + "title": "setpoint", + "x": 128.0, + "y": 128.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/closedloop/setpoint/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "limit", + "x": 512.0, + "y": 512.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "stator", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/current/limit/stator", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "supply", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hood/hoodMotor/current/limit/supply", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "current", + "x": 256.0, + "y": 512.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "stator", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/current/stator", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "supply", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/current/supply", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "Live Tuning", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "position", + "x": 1024.0, + "y": 256.0, + "width": 384.0, + "height": 384.0, + "type": "Graph", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/mechanism/position", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "position", + "x": 896.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleAtGoal", + "x": 1536.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "HoodAngleActual", + "x": 1536.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleCalculated", + "x": 1536.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleError", + "x": 1536.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleDesired", + "x": 1792.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Turret", + "grid_layout": { + "layouts": [ + { + "title": "feedback", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kI", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedback/kI", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedback/kP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedback/kD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "feedforward", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kA", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kA", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kG", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kG", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kS", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kS", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kV", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/feedforward/kV", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "setpoint", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/setpoint/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Launcher", + "x": 1280.0, + "y": 128.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Live Tuning", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + } + ] + }, + { + "title": "turretMotor", + "x": 768.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Up", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/turretMotor/Up", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "ZeroEncoder", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/turretMotor/ZeroEncoder", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Down", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/turretMotor/Down", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + } + ] + }, + { + "title": "current", + "x": 1024.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "stator", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/current/stator", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "supply", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hood/hoodMotor/current/supply", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "limit", + "x": 1280.0, + "y": 384.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "stator", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/current/limit/stator", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "supply", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/current/limit/supply", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "motionprofile", + "x": 1024.0, + "y": 512.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "maxAcceleration", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/motionprofile/maxAcceleration", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "maxVelocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/turret/turretMotor/closedloop/motionprofile/maxVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + ], + "containers": [ + { + "title": "position", + "x": 384.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/turret/turretMotor/mechanism/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "position", + "x": 1408.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/turret/turretMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleActual", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleAtGoal", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "CRT Angle", + "x": 0.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/CRT Angle", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Coverage Satisfies Range", + "x": 768.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Coverage Satisfies Range", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Unique Coverage", + "x": 896.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Unique Coverage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Encoder 36", + "x": 1152.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Encoder 36", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Encoder 40", + "x": 1024.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Encoder 40", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleDesired", + "x": 0.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleError", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "CRT Status", + "x": 384.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/CRT Status", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateCurrent", + "x": 384.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Flywheel", + "grid_layout": { + "layouts": [ + { + "title": "feedback", + "x": 896.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedback/kD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kI", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedback/kI", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedback/kP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "feedforward", + "x": 1152.0, + "y": 256.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kA", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kA", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kG", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kG", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kS", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kS", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kV", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/feedforward/kV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "setpoint", + "x": 1408.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelMotor/closedloop/setpoint/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + ], + "containers": [ + { + "title": "FlyWheelSpeedActual", + "x": 640.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedAtGoal", + "x": 640.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "FlyWheelSpeedCalculated", + "x": 640.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedDesired", + "x": 640.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedError", + "x": 640.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Live Tuning", + "x": 1152.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/Launcher/Live Tuning", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "velocity", + "x": 1664.0, + "y": 128.0, + "width": 768.0, + "height": 640.0, + "type": "Graph", + "properties": { + "topic": "/Mechanisms/flywheel/flywheelMotor/mechanism/velocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 60.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "velocity", + "x": 1536.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/flywheel/flywheelMotor/rotor/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Hopper", + "grid_layout": { + "layouts": [ + { + "title": "feedback", + "x": 896.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedback/kD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kI", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedback/kI", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedback/kP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "feedforward", + "x": 1152.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "kA", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kA", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kG", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kG", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kS", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kS", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kV", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/feedforward/kV", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "setpoint", + "x": 128.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/setpoint/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "limit", + "x": 1664.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "stator", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/current/limit/stator", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "supply", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/hopper/hopperMotor/current/limit/supply", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "current", + "x": 1920.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "stator", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hopper/hopperMotor/current/stator", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "supply", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hopper/hopperMotor/current/supply", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "setpoint", + "x": 1152.0, + "y": 384.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hopper/hopperMotor/closedloop/setpoint/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "velocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hopper/hopperMotor/closedloop/setpoint/velocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "HopperAngle", + "x": 384.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/HopperAngle", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Speed", + "x": 640.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Live Tuning", + "x": 896.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Mechanisms/Commands/Intake/Live Tuning", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "position", + "x": 640.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hopper/hopperMotor/mechanism/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "position", + "x": 640.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Mechanisms/hopper/hopperMotor/rotor/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Speed", + "x": 512.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Graph", + "properties": { + "topic": "/AdvantageKit/Intake/Speed", + "period": 0.033, + "data_type": "double", + "time_displayed": 30.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "HopperAngleDouble", + "x": 384.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/HopperAngleDouble", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StateCurrent", + "x": 512.0, + "y": 256.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 512.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateCurrent", + "x": 896.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 1024.0, + "y": 640.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "HopperAmps", + "x": 0.0, + "y": 256.0, + "width": 512.0, + "height": 384.0, + "type": "Graph", + "properties": { + "topic": "/AdvantageKit/Intake/HopperAmps", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + } + ] + } + }, + { + "name": "Shot Tune", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "ApplyGuess", + "x": 896.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/SmartDashboard/ShotTuning/ApplyGuess", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "FireShot", + "x": 896.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/ShotTuning/FireShot", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "DistanceToTarget", + "x": 896.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ShotTuning/DistanceToTarget", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "UseGuessSource", + "x": 896.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/ShotTuning/UseGuessSource", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Tab 8", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pose Field", + "x": 0.0, + "y": 0.0, + "width": 1408.0, + "height": 768.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/DrivePoseEstimator/Pose Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + } + ] + } + }, + { + "name": "Tab 9", + "grid_layout": { + "layouts": [ + { + "title": "QuestNav", + "x": 1024.0, + "y": 128.0, + "width": 256.0, + "height": 512.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "streams", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/QuestNav/streams", + "period": 0.06, + "data_type": "string[]", + "show_submit_button": false + } + }, + { + "title": "version", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/QuestNav/version", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "DistanceToVirtualTarget", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/DistanceToVirtualTarget", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TargetDistance", + "x": 0.0, + "y": 128.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TargetDistance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Auto Modes", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Rebuilt/Auto Modes", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "QUEST POSE", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/QUEST POSE", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "QUEST POSE", + "x": 512.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/QUEST POSE", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "QUEST Active", + "x": 512.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/QUEST Active", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "QUEST Connected", + "x": 640.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/QUEST Connected", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "QUEST POSE", + "x": 640.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/QUEST POSE", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Quest Calculated Offset to Robot Center", + "x": 768.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/questnav/Quest Calculated Offset to Robot Center", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": false + } + }, + { + "title": "Quest POSE Update", + "x": 768.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/questnav/Quest POSE Update", + "period": 0.06, + "data_type": "double[]", + "show_submit_button": false + } + }, + { + "title": "Flywheel Multiplier", + "x": 256.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Flywheel Multiplier", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Tab 10", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Turret Offset Angle", + "x": 640.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Turret Offset Angle", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleActual", + "x": 640.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/basic_robot/akit_swerve_drivetrain.json b/src/main/deploy/rebuilt_robot/akit_swerve_drivetrain.json similarity index 81% rename from src/main/deploy/basic_robot/akit_swerve_drivetrain.json rename to src/main/deploy/rebuilt_robot/akit_swerve_drivetrain.json index f1a74ad6..534c085e 100644 --- a/src/main/deploy/basic_robot/akit_swerve_drivetrain.json +++ b/src/main/deploy/rebuilt_robot/akit_swerve_drivetrain.json @@ -10,50 +10,50 @@ "uom": "inches" }, "wheelDiameter": { - "val": 3.955, - "uom": "inches" + "val": 0.1002156588, + "uom": "meters" }, "maxDriveSpeed": { "val": 4.69, "uom": "m/sec" }, "bumperFrameLength": { - "val": 30, + "val": 33, "uom": "inches" }, "bumperFrameWidth": { - "val": 30, + "val": 33, "uom": "inches" }, "gyro": { "type": "pigeon2", "id": 50, "inverted": false, - "canbus": "" + "canbus": "canivore" }, - "driveGearRatio": "1:6.75", - "steerGearRatio": "1:25", + "driveGearRatio": "1:6", + "steerGearRatio": "1:24", "driveMotorControl": { "feedBack": { - "p": 0.00001, + "p": 50, "i": 0.0, "d": 0.0 }, "feedForward": { - "s": 0.0698163127444402, - "v": 0.11987949085405275, - "a": 0.0 + "s": 4.154401722318193, + "v": 0.12648375113784652, + "a": 0.004058324259193265 } }, "steerMotorControl": { "feedBack": { - "p": 100, + "p": 4000, "i": 0.0, - "d": 0.5 + "d": 50 }, "feedForward": { - "s": 0.06242725612073021, - "v": 0.4513057312290134, + "s": 3.6636, + "v": 0.63, "a": 0.0 } }, @@ -63,17 +63,17 @@ "name": "frontLeftDrive", "controllerType": "talonfx", "motorType": "KrakenX60", - "canId": 4 + "canId": 1 }, "steerMotorSetup": { "name": "frontLeftSteer", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 3 + "canId": 2 }, - "encoderId": 13, + "encoderId": 1, "absoluteOffset": { - "val": 0.15234375, + "val": -0.11811640625, "uom": "rotations" }, "encoderInverted": false @@ -83,17 +83,17 @@ "name": "frontRightDrive", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 2 + "canId": 7 }, "steerMotorSetup": { "name": "frontRightSteer", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 1 + "canId": 8 }, - "encoderId": 16, + "encoderId": 4, "absoluteOffset": { - "val": -0.4873046875, + "val": 0.055117578125, "uom": "rotations" }, "encoderInverted": false @@ -103,17 +103,17 @@ "name": "backLeftDrive", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 6 + "canId": 3 }, "steerMotorSetup": { "name": "backLeftSteer", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 5 + "canId": 4 }, - "encoderId": 14, + "encoderId": 2, "absoluteOffset": { - "val": -0.219482421875, + "val": -0.094970703125, "uom": "rotations" }, "encoderInverted": false @@ -123,23 +123,23 @@ "name": "backRightDrive", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 8 + "canId": 5 }, "steerMotorSetup": { "name": "backRightSteer", "controllerType": "talonfx", "motorType": "Krakenx60", - "canId": 7 + "canId": 6 }, - "encoderId": 15, + "encoderId": 3, "absoluteOffset": { - "val": 0.17236328125, + "val": 0.24560546875, "uom": "rotations" }, "encoderInverted": false } }, - "coupleRatio": 3.8181818181818183, + "coupleRatio": 3.0, "invertLeftSide": true, "invertRightSide": true, "steerInertia": { @@ -151,7 +151,7 @@ "uom": "kg*m^2" }, "robotMass": { - "val": 50, + "val": 68, "uom": "kg" }, "wheelCOF": 1.2, @@ -159,18 +159,18 @@ "val": 120, "uom": "amps" }, - "canbus": "", + "canbus": "canivore", "startingPose": { "x": { - "val": 3.42, + "val": 1.0, "uom": "m" }, "y": { - "val": 5.75, + "val": 1.0, "uom": "m" }, "rotation": { - "val": -45, + "val": 0.0, "uom": "deg" } } diff --git a/src/main/deploy/rebuilt_robot/cameras.json b/src/main/deploy/rebuilt_robot/cameras.json new file mode 100644 index 00000000..18b6f6b1 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/cameras.json @@ -0,0 +1,8 @@ +{ + "cameras": [ + "rear.json", + "right.json", + "left.json" + ], + "viewGamePieces": false +} diff --git a/src/main/deploy/rebuilt_robot/cameras/left.json b/src/main/deploy/rebuilt_robot/cameras/left.json new file mode 100644 index 00000000..e58bd7ca --- /dev/null +++ b/src/main/deploy/rebuilt_robot/cameras/left.json @@ -0,0 +1,13 @@ +{ + "name": "left-bagel", + "use": "apriltag", + "type": "photonvision", + "strategy": "LOWEST_AMBIGUITY", + "column": 0, + "x": -0.2778, + "y": 0.2975, + "z": 0.2368, + "roll": 0, + "pitch": -30, + "yaw": 70 +} diff --git a/src/main/deploy/basic_robot/cameras/quest.json b/src/main/deploy/rebuilt_robot/cameras/quest.json similarity index 55% rename from src/main/deploy/basic_robot/cameras/quest.json rename to src/main/deploy/rebuilt_robot/cameras/quest.json index df412311..21b84791 100644 --- a/src/main/deploy/basic_robot/cameras/quest.json +++ b/src/main/deploy/rebuilt_robot/cameras/quest.json @@ -2,10 +2,10 @@ "name": "quest", "use": "quest", "column": 0, - "x": 0, - "y": 0, - "z": 18, + "x": -0.171968, + "y": -0.252501, + "z": 0.225336, "roll": 0, "pitch": 0, - "yaw": 0 + "yaw": 180 } diff --git a/src/main/deploy/rebuilt_robot/cameras/rear.json b/src/main/deploy/rebuilt_robot/cameras/rear.json new file mode 100644 index 00000000..763d6499 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/cameras/rear.json @@ -0,0 +1,13 @@ +{ + "name": "rear-prometheus", + "use": "apriltag", + "type": "photonvision", + "strategy": "LOWEST_AMBIGUITY", + "column": 0, + "x": -0.2961, + "y": -0.2212, + "z": 0.2368, + "roll": 0, + "pitch": -30, + "yaw": 175 +} diff --git a/src/main/deploy/rebuilt_robot/cameras/right.json b/src/main/deploy/rebuilt_robot/cameras/right.json new file mode 100644 index 00000000..9001051e --- /dev/null +++ b/src/main/deploy/rebuilt_robot/cameras/right.json @@ -0,0 +1,13 @@ +{ + "name": "right-raikou", + "use": "apriltag", + "type": "photonvision", + "strategy": "LOWEST_AMBIGUITY", + "column": 0, + "x": -0.2775, + "y": -0.3025, + "z": 0.2432, + "roll": 0, + "pitch": -30, + "yaw": -81 +} diff --git a/src/main/deploy/baby_swerve/competition_mode.json b/src/main/deploy/rebuilt_robot/competition_mode.json similarity index 100% rename from src/main/deploy/baby_swerve/competition_mode.json rename to src/main/deploy/rebuilt_robot/competition_mode.json diff --git a/src/main/deploy/basic_robot/controllers.json b/src/main/deploy/rebuilt_robot/controllers.json similarity index 100% rename from src/main/deploy/basic_robot/controllers.json rename to src/main/deploy/rebuilt_robot/controllers.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_trigger.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_trigger.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_trigger.json rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_left_trigger.json diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_x.json similarity index 80% rename from src/main/deploy/basic_robot/controllers/axis/driver_left_x.json rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_left_x.json index d77f0e91..da5c70b0 100644 --- a/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json +++ b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_x.json @@ -3,6 +3,6 @@ "deadband": 0.07, "invert": true, "scale": 1.0, - "curvePower": 3, + "curvePower": 1.0, "limit": 1.0 } diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_y.json similarity index 80% rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_left_y.json index d826731e..a6038549 100644 --- a/src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json +++ b/src/main/deploy/rebuilt_robot/controllers/axis/driver_left_y.json @@ -3,6 +3,6 @@ "deadband": 0.07, "invert": true, "scale": 1.0, - "curvePower": 3.0, + "curvePower": 1.0, "limit": 1.0 } diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_right_trigger.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_right_trigger.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json b/src/main/deploy/rebuilt_robot/controllers/axis/driver_right_x.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json rename to src/main/deploy/rebuilt_robot/controllers/axis/driver_right_x.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/operator_left_y.json b/src/main/deploy/rebuilt_robot/controllers/axis/operator_left_y.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/operator_left_y.json rename to src/main/deploy/rebuilt_robot/controllers/axis/operator_left_y.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/operator_right_y.json b/src/main/deploy/rebuilt_robot/controllers/axis/operator_right_y.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/operator_right_y.json rename to src/main/deploy/rebuilt_robot/controllers/axis/operator_right_y.json diff --git a/src/main/deploy/baby_swerve/controllers/driver.json b/src/main/deploy/rebuilt_robot/controllers/driver.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/driver.json rename to src/main/deploy/rebuilt_robot/controllers/driver.json diff --git a/src/main/deploy/baby_swerve/controllers/operator.json b/src/main/deploy/rebuilt_robot/controllers/operator.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/operator.json rename to src/main/deploy/rebuilt_robot/controllers/operator.json diff --git a/src/main/deploy/basic_robot/demo_mode.json b/src/main/deploy/rebuilt_robot/demo_mode.json similarity index 100% rename from src/main/deploy/basic_robot/demo_mode.json rename to src/main/deploy/rebuilt_robot/demo_mode.json diff --git a/src/main/deploy/baby_swerve/drive_modules/backleft.json b/src/main/deploy/rebuilt_robot/drive_modules/backleft.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/backleft.json rename to src/main/deploy/rebuilt_robot/drive_modules/backleft.json diff --git a/src/main/deploy/baby_swerve/drive_modules/backright.json b/src/main/deploy/rebuilt_robot/drive_modules/backright.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/backright.json rename to src/main/deploy/rebuilt_robot/drive_modules/backright.json diff --git a/src/main/deploy/baby_swerve/drive_modules/frontleft.json b/src/main/deploy/rebuilt_robot/drive_modules/frontleft.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/frontleft.json rename to src/main/deploy/rebuilt_robot/drive_modules/frontleft.json diff --git a/src/main/deploy/baby_swerve/drive_modules/frontright.json b/src/main/deploy/rebuilt_robot/drive_modules/frontright.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/frontright.json rename to src/main/deploy/rebuilt_robot/drive_modules/frontright.json diff --git a/src/main/deploy/rebuilt_robot/elastic-layout.json b/src/main/deploy/rebuilt_robot/elastic-layout.json new file mode 100644 index 00000000..1c6e1d4f --- /dev/null +++ b/src/main/deploy/rebuilt_robot/elastic-layout.json @@ -0,0 +1,701 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [ + { + "title": "HubStatus", + "x": 768.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "AutoWinner", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/AutoWinner", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "CurrentShift", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/CurrentShift", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "MatchTime", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/MatchTime", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "NextShift", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/NextShift", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "TimeRemainingInCurrentShift", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/HubStatus/TimeRemainingInCurrentShift", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "IsActiveNext", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/HubStatus/IsActiveNext", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "ActiveNow", + "x": 640.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/HubStatus/ActiveNow", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "Indexer", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "SpindexerSpeed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/SpindexerSpeed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StateCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "TransferBackSpeed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/TransferBackSpeed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TransferFrontSpeed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Indexer/TransferFrontSpeed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + }, + { + "title": "Intake", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 384.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "HopperAngle", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/HopperAngle", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "StateCurrent", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + }, + { + "title": "Launcher", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 768.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CoverageSatisfiesRange", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/CoverageSatisfiesRange", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "FlyWheelMotorOutput", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelMotorOutput", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedActual", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedCalculated", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedDesired", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "FlyWheelSpeedError", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleActual", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleCalculated", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleDesired", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodAngleError", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HoodVelocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TargetDistance", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TargetDistance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleActual", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleActual", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleCalculated", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleCalculated", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleDesired", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleDesired", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretAngleError", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "TurretVelocity", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "UniqueCoverage", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/UniqueCoverage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + ], + "containers": [ + { + "title": "left-processed", + "x": 384.0, + "y": 384.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/left-processed", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "right-processed", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/right-processed", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Pose Field", + "x": 0.0, + "y": 0.0, + "width": 768.0, + "height": 384.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/DrivePoseEstimator/Pose Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "FlyWheelSpeedAtGoal", + "x": 1024.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/FlyWheelSpeedAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "HoodAngleAtGoal", + "x": 1152.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/HoodAngleAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "TurretAngleAtGoal", + "x": 1280.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Launcher/TurretAngleAtGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "StateCurrent", + "x": 1024.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/StateCurrent", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "StateRequested", + "x": 1152.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Launcher/StateRequested", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "SimulatedGamepieces", + "x": 1280.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/SimulatedGamepieces", + "period": 0.06, + "data_type": "int", + "show_submit_button": false + } + } + ] + } + } + ] +} diff --git a/src/main/deploy/basic_robot/field/game_pieces.json b/src/main/deploy/rebuilt_robot/field/game_pieces.json similarity index 78% rename from src/main/deploy/basic_robot/field/game_pieces.json rename to src/main/deploy/rebuilt_robot/field/game_pieces.json index a1e2a1fb..b13b37c1 100644 --- a/src/main/deploy/basic_robot/field/game_pieces.json +++ b/src/main/deploy/rebuilt_robot/field/game_pieces.json @@ -3,7 +3,7 @@ { "x": 2, "y": 7, - "type": "Fuel", + "type": "Coral", "rotation": 0 } ] diff --git a/src/main/deploy/basic_robot/robot.json b/src/main/deploy/rebuilt_robot/robot.json similarity index 80% rename from src/main/deploy/basic_robot/robot.json rename to src/main/deploy/rebuilt_robot/robot.json index 2475d0f1..9491e148 100644 --- a/src/main/deploy/basic_robot/robot.json +++ b/src/main/deploy/rebuilt_robot/robot.json @@ -1,11 +1,11 @@ { "userConfig": "competition_mode.json", "driveType": "AKIT_SWERVE_DRIVE", - "trackWidth": 16.8, + "trackWidth": 22, "trackWidthUom": "in", - "wheelBase": 18.5, + "wheelBase": 22, "wheelBaseUom": "in", - "wheelDiameter": 0.103386904, + "wheelDiameter": 0.1002156588, "wheelDiameterUom": "m", "physicalMaxSpeed": 5.93, "physicalMaxSpeedUom": "m/s", diff --git a/src/main/deploy/rebuilt_robot/subsystems/climb.json b/src/main/deploy/rebuilt_robot/subsystems/climb.json new file mode 100644 index 00000000..3dda928d --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/climb.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_elevator", + "file": "climb/lifter.json" + } + ] +} diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json b/src/main/deploy/rebuilt_robot/subsystems/climb/lifter.json similarity index 78% rename from src/main/deploy/basic_robot/subsystems/example/yams_elevator.json rename to src/main/deploy/rebuilt_robot/subsystems/climb/lifter.json index e8cbe550..ef3812e2 100644 --- a/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json +++ b/src/main/deploy/rebuilt_robot/subsystems/climb/lifter.json @@ -1,9 +1,10 @@ { "motorSetup": { - "name": "Elevator", - "controllerType": "spark", - "motorType": "Neo", - "canId": 10, + "name": "lifter", + "controllerType": "talonfx", + "motorType": "KrakenX60", + "canId": 20, + "logLevel": "LOW", "robotToMotor": { "x": { "val": -10.7, @@ -23,6 +24,7 @@ } } }, + "controlAlgorithm": "SIMPLE", "motorSystemId": { "feedBack": { "p": 2.7739E-10, @@ -64,35 +66,36 @@ "maxAcceleration": { "val": 20, "uom": "m/s^2" - } + }, + "controlMode": "CLOSED_LOOP" }, "sprocketTeeth": 22, "lowerSoftLimit": { - "val": 0.1, - "uom": "m" + "val": 0, + "uom": "in" }, "upperSoftLimit": { - "val": 2, - "uom": "m" + "val": 9, + "uom": "in" }, "lowerHardLimit": { - "val": 0.1, - "uom": "m" + "val": 0, + "uom": "in" }, "upperHardLimit": { - "val": 3, - "uom": "m" + "val": 26.5, + "uom": "in" }, "gearing": [ 3, 4 ], "startingPosition": { - "val": 0.5, + "val": 0, "uom": "m" }, "mass": { - "val": 16, + "val": 5, "uom": "lbs" } } diff --git a/src/main/deploy/rebuilt_robot/subsystems/indexer.json b/src/main/deploy/rebuilt_robot/subsystems/indexer.json new file mode 100644 index 00000000..58cfb171 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/indexer.json @@ -0,0 +1,12 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "indexer/transfer_flywheel.json" + }, + { + "device": "percent_motor", + "file": "indexer/spindexer.json" + } + ] +} diff --git a/src/main/deploy/basic_robot/subsystems/example/percent_motor.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/spindexer.json similarity index 52% rename from src/main/deploy/basic_robot/subsystems/example/percent_motor.json rename to src/main/deploy/rebuilt_robot/subsystems/indexer/spindexer.json index 93be5418..bacd343e 100644 --- a/src/main/deploy/basic_robot/subsystems/example/percent_motor.json +++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/spindexer.json @@ -1,8 +1,8 @@ { - "name": "percent_motor", - "controller": "spark", - "type": "Neo", - "id": 11, + "name": "spindexer", + "controller": "talonfx", + "type": "KrakenX44", + "id": 9, "gearing": 1.0, "momentOfInertiaKgMSq": 1.0, "x": 0.5, diff --git a/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_back.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_back.json new file mode 100644 index 00000000..75775998 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_back.json @@ -0,0 +1,10 @@ +{ + "name": "transfer_back", + "controller": "talonfx", + "type": "KrakenX60", + "id": 11, + "gearing": 1.0, + "x": -0.236, + "y": -0.001, + "z": 0.136 +} diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_flywheel.json similarity index 73% rename from src/main/deploy/basic_robot/subsystems/example/yams_pivot.json rename to src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_flywheel.json index d3d3b2ba..83ea7c16 100644 --- a/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json +++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_flywheel.json @@ -1,9 +1,17 @@ { "motorSetup": { - "name": "Turret", + "name": "transfer", "controllerType": "talonfx", "motorType": "KrakenX60", - "canId": 15, + "canId": 10, + "logLevel": "LOW", + "inverted": true, + "followers": [ + { + "canId": 11, + "inverted": false + } + ], "robotToMotor": { "x": { "val": -5.872, @@ -21,8 +29,7 @@ "val": 0.0, "uom": "deg" } - }, - "movementPlane": "XY" + } }, "motorSystemId": { "feedBack": { @@ -64,33 +71,21 @@ "uom": "deg/s^2" } }, - "lowerHardLimit": { - "val": -165, - "uom": "deg" - }, - "upperHardLimit": { - "val": 165, - "uom": "deg" - }, - "startingAngle": { - "val": 0, - "uom": "deg" - }, "lowerSoftLimit": { - "val": -160, - "uom": "deg" + "val": 0, + "uom": "rpm" }, "upperSoftLimit": { - "val": 160, - "uom": "deg" - }, - "gearStages": "12:36:40:12:108", - "radius": { - "val": 5, - "uom": "in" + "val": 5000, + "uom": "rpm" }, + "gearStages": "1:1", "mass": { - "val": 20, - "uom": "lb" + "val": 2, + "uom": "kg" + }, + "radius": { + "val": 0.05, + "uom": "m" } } diff --git a/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_front.json b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_front.json new file mode 100644 index 00000000..c76c9866 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/indexer/transfer_front.json @@ -0,0 +1,10 @@ +{ + "name": "transfer_front", + "controller": "talonfx", + "type": "KrakenX60", + "id": 10, + "gearing": 1.0, + "x": -0.236, + "y": -0.001, + "z": 0.136 +} diff --git a/src/main/deploy/rebuilt_robot/subsystems/intake.json b/src/main/deploy/rebuilt_robot/subsystems/intake.json new file mode 100644 index 00000000..342b728d --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/intake.json @@ -0,0 +1,16 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "intake/spintake_inner.json" + }, + { + "device": "yams_shooter", + "file": "intake/spintake_outer.json" + }, + { + "device": "yams_arm", + "file": "intake/hopper.json" + } + ] +} diff --git a/src/main/deploy/rebuilt_robot/subsystems/intake/hopper.json b/src/main/deploy/rebuilt_robot/subsystems/intake/hopper.json new file mode 100644 index 00000000..626776eb --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/intake/hopper.json @@ -0,0 +1,97 @@ +{ + "motorSetup": { + "name": "hopper", + "controllerType": "talonfx", + "motorType": "KrakenX44", + "canId": 15, + "logLevel": "LOW", + "inverted": true, + "currentLimit": { + "val": 60, + "uom": "amps" + }, + "followers": [ + { + "canId": 14, + "inverted": true + } + ] + }, + "controlAlgorithm": "PROFILED", + "useTorqueCurrentFOC": false, + "motorSystemId": { + "feedBack": { + "p": 40, + "i": 0.0, + "d": 5.0 + }, + "feedForward": { + "s": 0.5992224858009301, + "v": 0.010060789918368191, + "a": 0.0, + "g": 0.65 + }, + "maxVelocity": { + "val": 1080, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 5000, + "uom": "deg/s^2" + } + }, + "simSystemId": { + "feedBack": { + "p": 10.0, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.048722, + "v": 0.005486, + "a": 0.0, + "g": 0.04055 + }, + "maxVelocity": { + "val": 360, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 180, + "uom": "deg/s^2" + } + }, + "length": { + "val": 0.234, + "uom": "m" + }, + "lowerHardLimit": { + "val": 0, + "uom": "deg" + }, + "upperHardLimit": { + "val": 125, + "uom": "deg" + }, + "startingAngle": { + "val": 125, + "uom": "deg" + }, + "lowerSoftLimit": { + "val": 0, + "uom": "deg" + }, + "upperSoftLimit": { + "val": 120, + "uom": "deg" + }, + "gearStages": "24:1", + "mass": { + "val": 1, + "uom": "lbs" + }, + "horizontalZero": { + "val": 0, + "uom": "deg" + } +} diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_inner.json similarity index 63% rename from src/main/deploy/basic_robot/subsystems/example/yams_shooter.json rename to src/main/deploy/rebuilt_robot/subsystems/intake/spintake_inner.json index dc9a2c3b..e12c4798 100644 --- a/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json +++ b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_inner.json @@ -1,9 +1,11 @@ { "motorSetup": { - "name": "Shooter", - "controllerType": "spark", - "motorType": "KrakenX44", + "name": "spintake_inner", + "controllerType": "talonfx", + "motorType": "KrakenX60", "canId": 12, + "logLevel": "LOW", + "inverted": true, "robotToMotor": { "x": { "val": -5.872, @@ -14,7 +16,7 @@ "uom": "in" }, "z": { - "val": 18.72, + "val": 14.466, "uom": "in" }, "rotation": { @@ -24,6 +26,7 @@ } }, "motorSystemId": { + "controlMode": "CLOSED_LOOP", "feedBack": { "p": 4, "i": 0.0, @@ -33,6 +36,14 @@ "s": 0.0, "v": 0.0, "a": 0.0 + }, + "maxVelocity": { + "val": 180, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 90, + "uom": "deg/s^2" } }, "simSystemId": { @@ -45,6 +56,14 @@ "s": 0.0, "v": 0.0, "a": 0.0 + }, + "maxVelocity": { + "val": 180, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 90, + "uom": "deg/s^2" } }, "lowerSoftLimit": { @@ -55,10 +74,7 @@ "val": 5000, "uom": "rpm" }, - "gearing": [ - 3, - 4 - ], + "gearStages": "11:36", "mass": { "val": 2, "uom": "kg" diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_arm.json b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_outer.json similarity index 68% rename from src/main/deploy/basic_robot/subsystems/example/yams_arm.json rename to src/main/deploy/rebuilt_robot/subsystems/intake/spintake_outer.json index 2f588f40..6cc7ab7c 100644 --- a/src/main/deploy/basic_robot/subsystems/example/yams_arm.json +++ b/src/main/deploy/rebuilt_robot/subsystems/intake/spintake_outer.json @@ -1,20 +1,22 @@ { "motorSetup": { - "name": "Hood", + "name": "spintake_outer", "controllerType": "talonfx", "motorType": "KrakenX60", "canId": 13, + "logLevel": "LOW", + "inverted": true, "robotToMotor": { "x": { - "val": -8.2, + "val": -5.872, "uom": "in" }, "y": { - "val": 10.104, + "val": 4.8, "uom": "in" }, "z": { - "val": 15.786, + "val": 14.466, "uom": "in" }, "rotation": { @@ -24,6 +26,7 @@ } }, "motorSystemId": { + "controlMode": "CLOSED_LOOP", "feedBack": { "p": 4, "i": 0.0, @@ -63,40 +66,21 @@ "uom": "deg/s^2" } }, - "length": { - "val": 0.234, - "uom": "m" - }, - "lowerHardLimit": { - "val": 55, - "uom": "deg" - }, - "upperHardLimit": { - "val": 100, - "uom": "deg" - }, - "startingAngle": { - "val": 55, - "uom": "deg" - }, "lowerSoftLimit": { - "val": 55, - "uom": "deg" + "val": 0, + "uom": "rpm" }, "upperSoftLimit": { - "val": 85, - "uom": "deg" + "val": 5000, + "uom": "rpm" }, - "gearing": [ - 3, - 4 - ], + "gearStages": "11:36", "mass": { - "val": 1, - "uom": "lbs" + "val": 2, + "uom": "kg" }, - "horizontalZero": { - "val": 55, - "uom": "deg" + "radius": { + "val": 0.05, + "uom": "m" } } diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher.json b/src/main/deploy/rebuilt_robot/subsystems/launcher.json new file mode 100644 index 00000000..da7dc648 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/launcher.json @@ -0,0 +1,16 @@ +{ + "devices": [ + { + "device": "yams_arm", + "file": "launcher/hood.json" + }, + { + "device": "yams_shooter", + "file": "launcher/flywheel.json" + }, + { + "device": "yams_turret", + "file": "launcher/turret.json" + } + ] +} diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher/flywheel.json b/src/main/deploy/rebuilt_robot/subsystems/launcher/flywheel.json new file mode 100644 index 00000000..68c009b5 --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/launcher/flywheel.json @@ -0,0 +1,103 @@ +{ + "motorSetup": { + "name": "flywheel", + "controllerType": "talonfx", + "motorType": "KrakenX60", + "canId": 16, + "inverted": true, + "logLevel": "LOW", + "currentLimit": { + "val": 80, + "uom": "amps" + }, + "followers": [ + { + "canId": 17, + "inverted": true + } + ], + "robotToMotor": { + "x": { + "val": -5.872, + "uom": "in" + }, + "y": { + "val": 4.8, + "uom": "in" + }, + "z": { + "val": 14.466, + "uom": "in" + }, + "rotation": { + "val": 0.0, + "uom": "deg" + } + } + }, + "motorSystemId": { + "feedBack": { + "p": 5, + "i": 0.0, + "d": 0.0 + }, + "closedLoopRamp": { + "val": 0, + "uom": "s" + }, + "openLoopRamp": { + "val": 0, + "uom": "s" + }, + "feedForward": { + "s": 0.066854, + "v": 2.1, + "a": 0.38848 + }, + "maxVelocity": { + "val": 180, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 90, + "uom": "deg/s^2" + } + }, + "simSystemId": { + "feedBack": { + "p": 2, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.066854, + "v": 2.1962, + "a": 0.38848 + }, + "maxVelocity": { + "val": 180, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 90, + "uom": "deg/s^2" + } + }, + "lowerSoftLimit": { + "val": 0, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 5000, + "uom": "rpm" + }, + "gearStages": "18:1", + "mass": { + "val": 5.1, + "uom": "lbs" + }, + "radius": { + "val": 1.975, + "uom": "in" + } +} diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher/hood.json b/src/main/deploy/rebuilt_robot/subsystems/launcher/hood.json new file mode 100644 index 00000000..39749e6b --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/launcher/hood.json @@ -0,0 +1,88 @@ +{ + "motorSetup": { + "name": "hood", + "controllerType": "talonfx", + "motorType": "KrakenX44", + "canId": 19, + "logLevel": "LOW", + "currentLimit": { + "val": 60, + "uom": "amps" + } + }, + "controlAlgorithm": "PROFILED", + "motorSystemId": { + "feedBack": { + "p": 1100, + "i": 0.0, + "d": 60 + }, + "feedForward": { + "s": 0.0, + "v": 0.0, + "a": 0.0972, + "g": 4.357 + }, + "maxVelocity": { + "val": 1080, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 5000, + "uom": "deg/s^2" + } + }, + "simSystemId": { + "feedBack": { + "p": 9, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.0, + "v": 0.0, + "a": 0.0 + }, + "maxVelocity": { + "val": 180, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 90, + "uom": "deg/s^2" + } + }, + "length": { + "val": 9.466, + "uom": "in" + }, + "lowerHardLimit": { + "val": 12.723, + "uom": "deg" + }, + "upperHardLimit": { + "val": 45.723, + "uom": "deg" + }, + "startingAngle": { + "val": 12.723, + "uom": "deg" + }, + "lowerSoftLimit": { + "val": 12.723, + "uom": "deg" + }, + "upperSoftLimit": { + "val": 45.723, + "uom": "deg" + }, + "gearStages": "1015:33", + "mass": { + "val": 0.25, + "uom": "lbs" + }, + "horizontalZero": { + "val": 5.3, + "uom": "deg" + } +} diff --git a/src/main/deploy/rebuilt_robot/subsystems/launcher/turret.json b/src/main/deploy/rebuilt_robot/subsystems/launcher/turret.json new file mode 100644 index 00000000..053c180d --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/launcher/turret.json @@ -0,0 +1,100 @@ +{ + "motorSetup": { + "name": "turret", + "controllerType": "talonfx", + "motorType": "KrakenX44", + "canId": 18, + "inverted": true, + "logLevel": "HIGH", + "canBus": "canivore", + "robotToMotor": { + "x": { + "val": -4.856, + "uom": "in" + }, + "y": { + "val": 4.863, + "uom": "in" + }, + "z": { + "val": 14.466, + "uom": "in" + }, + "rotation": { + "val": 0.0, + "uom": "deg" + } + }, + "movementPlane": "XY" + }, + "controlAlgorithm": "SIMPLE", + "motorSystemId": { + "feedBack": { + "p": 1770.41578, + "i": 0.0, + "d": 1477.68211 + }, + "feedForward": { + "s": 12.2, + "v": 3.06, + "a": 1.94789461763 + }, + "maxVelocity": { + "val": 1080, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 10800, + "uom": "deg/s^2" + } + }, + "simSystemId": { + "feedBack": { + "p": 8, + "i": 0.0, + "d": 8.0 + }, + "feedForward": { + "s": 0.030215, + "v": 0.00087341, + "a": 0.98956 + }, + "maxVelocity": { + "val": 1080, + "uom": "deg/s" + }, + "maxAcceleration": { + "val": 10800, + "uom": "deg/s^2" + } + }, + "lowerHardLimit": { + "val": -160, + "uom": "deg" + }, + "upperHardLimit": { + "val": 160, + "uom": "deg" + }, + "startingAngle": { + "val": 0, + "uom": "deg" + }, + "lowerSoftLimit": { + "val": -150, + "uom": "deg" + }, + "upperSoftLimit": { + "val": 150, + "uom": "deg" + }, + "gearStages": "30:1", + "radius": { + "val": 5, + "uom": "in" + }, + "mass": { + "val": 15, + "uom": "lb" + } +} diff --git a/src/main/deploy/basic_robot/subsystems/led_strip.json b/src/main/deploy/rebuilt_robot/subsystems/led_strip.json similarity index 92% rename from src/main/deploy/basic_robot/subsystems/led_strip.json rename to src/main/deploy/rebuilt_robot/subsystems/led_strip.json index 25c307af..2196ba12 100644 --- a/src/main/deploy/basic_robot/subsystems/led_strip.json +++ b/src/main/deploy/rebuilt_robot/subsystems/led_strip.json @@ -1,5 +1,5 @@ { - "length": 30, + "length": 40, "dataPin": 0, "segments": [ { diff --git a/src/main/deploy/rebuilt_robot/subsystems/orchestra.json b/src/main/deploy/rebuilt_robot/subsystems/orchestra.json new file mode 100644 index 00000000..bd01e06f --- /dev/null +++ b/src/main/deploy/rebuilt_robot/subsystems/orchestra.json @@ -0,0 +1,39 @@ +{ + "rioIds": [ + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 19 + ], + "canivoreIds": [ + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 18 + ], + "music": [ + { + "name": "raiders", + "path": "music/raiders.chrp" + }, + { + "name": "mariachi", + "path": "music/mariachi.chrp" + }, + { + "name": "sea2", + "path": "music/sea2.chrp" + } + ] +} diff --git a/src/main/deploy/basic_robot/yagsl_drivetrain.json b/src/main/deploy/rebuilt_robot/yagsl_drivetrain.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_drivetrain.json rename to src/main/deploy/rebuilt_robot/yagsl_drivetrain.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/controllerproperties.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/controllerproperties.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/controllerproperties.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/controllerproperties.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backleft.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backleft.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backright.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/backright.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontleft.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontleft.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontright.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/frontright.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/physicalproperties.json similarity index 87% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/physicalproperties.json index c921b000..bfc8d99c 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json +++ b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/physicalproperties.json @@ -1,11 +1,11 @@ { "conversionFactors": { "angle": { - "gearRatio": 12.8, + "gearRatio": 24, "factor": 0 }, "drive": { - "gearRatio": 8.14, + "gearRatio": 6.0, "diameter": 4, "factor": 0 } diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/modules/pidfproperties.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/modules/pidfproperties.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json b/src/main/deploy/rebuilt_robot/yagsl_swerve/swervedrive.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json rename to src/main/deploy/rebuilt_robot/yagsl_swerve/swervedrive.json diff --git a/src/main/deploy/robots.json b/src/main/deploy/robots.json index 50eac16d..d6b660b3 100644 --- a/src/main/deploy/robots.json +++ b/src/main/deploy/robots.json @@ -1,14 +1,9 @@ { - "competitionPin": 0, + "competitionPin": 1, "robots": { - "basic_robot": { - "id": "basic_robot", - "robotClass": "frc.robot.example.ExampleRobot", - "simulate": true - }, - "baby_swerve": { - "id": "00:80:2F:24:6D:74", - "robotClass": "frc.robot.baby_swerve.BabySwerve", + "rebuilt_robot": { + "id": "00:80:2F:43:05:95", + "robotClass": "frc.robot.rebuilt.Rebuilt", "simulate": true, "competition": true } diff --git a/src/main/java/Scratch.java b/src/main/java/Scratch.java new file mode 100644 index 00000000..e4c00777 --- /dev/null +++ b/src/main/java/Scratch.java @@ -0,0 +1,5 @@ +import com.ctre.phoenix6.controls.MusicTone; + +public class Scratch { + MusicTone t = new MusicTone(440.0); +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c8ece43a..18c39dd2 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "FRC5010Example"; + public static final String MAVEN_NAME = "Rebuilt2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 81; - public static final String GIT_SHA = "e5bd64a909847009ebae0db75c2eb2a57ad49316"; - public static final String GIT_DATE = "2026-01-26 17:57:48 EST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2026-02-01 17:00:24 EST"; - public static final long BUILD_UNIX_TIME = 1769983224737L; + public static final int GIT_REVISION = 746; + public static final String GIT_SHA = "f9ced8f7a90d8a6ea280cb91d46b9907b786d7f2"; + public static final String GIT_DATE = "2026-05-02 11:14:50 EDT"; + public static final String GIT_BRANCH = "TheGrandFinale"; + public static final String BUILD_DATE = "2026-05-02 11:16:47 EDT"; + public static final long BUILD_UNIX_TIME = 1777735007396L; 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 index c8e97e07..df9c2a89 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,12 +15,20 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - /**** The simulation mode to run - Uncomment the one you want to use *****/ - public static final Mode SIM_MODE = Mode.SIM; // This is for running the Glass Simulator - // Mode.REPLAY; // This is for replaying from a log in Advantage Scope - /**** ------------------------------------------------------------- *****/ - - public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE; + /** + * Default simulation mode. REPLAY mode is auto-selected when the JVM is launched with {@code + * -Dlog=} (Gradle {@code -Plog=}) or with the {@code AKIT_LOG_PATH} environment + * variable set — AdvantageScope's "Spawn Replay" sets the latter, so leave this on {@link + * Mode#SIM} unless you specifically want REPLAY by default. + */ + public static final Mode SIM_MODE = Mode.SIM; + + public static final Mode CURRENT_MODE = + RobotBase.isReal() + ? Mode.REAL + : (System.getProperty("log") != null || System.getenv("AKIT_LOG_PATH") != null) + ? Mode.REPLAY + : SIM_MODE; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 46c3435d..423a196f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -68,7 +68,11 @@ public Robot() { case REPLAY: // Replaying a log, set up replay source setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); + // Read the path directly — LogFileUtil.findReplayLog() falls through to a stdin prompt + // when AdvantageScope is not connected, which crashes headless Gradle runs. + String logPath = System.getProperty("log"); + if (logPath == null) logPath = System.getenv("AKIT_LOG_PATH"); + if (logPath == null) logPath = LogFileUtil.findReplayLog(); Logger.setReplaySource(new WPILOGReader(logPath)); Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); break; @@ -148,8 +152,8 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setupDefaults(); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/baby_swerve/BabySwerve.java b/src/main/java/frc/robot/baby_swerve/BabySwerve.java deleted file mode 100644 index ae8be2a3..00000000 --- a/src/main/java/frc/robot/baby_swerve/BabySwerve.java +++ /dev/null @@ -1,43 +0,0 @@ -// 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.baby_swerve; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.example.DisplayValueSubsystem; -import org.frc5010.common.arch.GenericRobot; -import org.frc5010.common.config.ConfigConstants; -import org.frc5010.common.constants.SwerveConstants; -import org.frc5010.common.drive.GenericDrivetrain; -import org.frc5010.common.sensors.Controller; - -/** This is an example robot class. */ -public class BabySwerve extends GenericRobot { - SwerveConstants swerveConstants; - GenericDrivetrain drivetrain; - DisplayValueSubsystem displayValueSubsystem = new DisplayValueSubsystem(); - - public BabySwerve(String directory) { - super(directory); - drivetrain = (GenericDrivetrain) getSubsystem(ConfigConstants.DRIVETRAIN); - } - - @Override - public void configureButtonBindings(Controller driver, Controller operator) {} - - @Override - public void setupDefaultCommands(Controller driver, Controller operator) { - drivetrain.setDefaultCommand(drivetrain.createDefaultCommand(driver)); - } - - @Override - public void initAutoCommands() { - drivetrain.setAutoBuilder(); - } - - @Override - public Command generateAutoCommand(Command autoCommand) { - return drivetrain.generateAutoCommand(autoCommand); - } -} diff --git a/src/main/java/frc/robot/example/ConfiguredMechanisms.java b/src/main/java/frc/robot/example/ConfiguredMechanisms.java deleted file mode 100644 index 0082ec44..00000000 --- a/src/main/java/frc/robot/example/ConfiguredMechanisms.java +++ /dev/null @@ -1,80 +0,0 @@ -// 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.example; - -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.frc5010.common.config.json.devices.ArmParser; -import org.frc5010.common.config.json.devices.ElevatorParser; -import org.frc5010.common.config.json.devices.PivotParser; -import yams.mechanisms.positional.Arm; -import yams.mechanisms.positional.Elevator; -import yams.mechanisms.positional.Pivot; - -public class ConfiguredMechanisms extends SubsystemBase { - Elevator elevator; - Arm arm; // Assuming you have an Arm subsystem, otherwise remove this line - Pivot pivot; // Assuming you have a Pivot subsystem, otherwise remove this line - - /** Creates a new ConfiguredElevator. */ - public ConfiguredMechanisms() { - elevator = ElevatorParser.parse("mechanisms", "yams_elevator.json", this); - arm = ArmParser.parse("mechanisms", "yams_arm.json", this); - pivot = PivotParser.parse("mechanisms", "yams_pivot.json", this); - } - - public void periodic() { - elevator.updateTelemetry(); - arm.updateTelemetry(); - pivot.updateTelemetry(); - } - - public void simulationPeriodic() { - elevator.simIterate(); - arm.simIterate(); - pivot.simIterate(); - } - - public Command elevCmd(double dutycycle) { - return elevator.set(dutycycle); - } - - public Command setHeight(Distance height) { - return elevator.setHeight(height); - } - - public Command sysIdElevator() { - return elevator.sysId(Volts.of(12), Volts.of(12).per(Second), Second.of(30)); - } - - public Command armCmd(double dutycycle) { - return arm.set(dutycycle); - } - - public Command sysIdArm() { - return arm.sysId(Volts.of(3), Volts.of(3).per(Second), Second.of(30)); - } - - public Command setArmAngle(Angle angle) { - return arm.setAngle(angle); - } - - public Command turretCmd(double dutycycle) { - return pivot.set(dutycycle); - } - - public Command sysIdTurret() { - return pivot.sysId(Volts.of(3), Volts.of(3).per(Second), Second.of(30)); - } - - public Command setPivotAngle(Angle angle) { - return pivot.setAngle(angle); - } -} diff --git a/src/main/java/frc/robot/example/DisplayValueSubsystem.java b/src/main/java/frc/robot/example/DisplayValueSubsystem.java deleted file mode 100644 index c7207255..00000000 --- a/src/main/java/frc/robot/example/DisplayValueSubsystem.java +++ /dev/null @@ -1,76 +0,0 @@ -// 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.example; - -import static edu.wpi.first.units.Units.Degrees; - -import org.frc5010.common.arch.GenericSubsystem; -import org.frc5010.common.telemetry.DisplayAngle; -import org.frc5010.common.telemetry.DisplayBoolean; -import org.frc5010.common.telemetry.DisplayDouble; -import org.frc5010.common.telemetry.DisplayFloat; -import org.frc5010.common.telemetry.DisplayLength; -import org.frc5010.common.telemetry.DisplayLong; -import org.frc5010.common.telemetry.DisplayString; -import org.frc5010.common.telemetry.DisplayTime; - -/** Tests the classes in the {@link org.frc5010.common.telemetry} package that Display values */ -public class DisplayValueSubsystem extends GenericSubsystem { - DisplayAngle inputAngle; - DisplayAngle outputAngle; - DisplayBoolean inputBoolean; - DisplayBoolean outputBoolean; - DisplayDouble inputDouble; - DisplayDouble outputDouble; - DisplayFloat inputFloat; - DisplayFloat outputFloat; - DisplayLength inputLength; - DisplayLength outputLength; - DisplayLong inputLong; - DisplayLong outputLong; - DisplayString inputString; - DisplayString outputString; - DisplayTime inputTime; - DisplayTime outputTime; - DisplayAngle outAngle; - - public DisplayValueSubsystem() { - super(); - outAngle = DashBoard.makeInfoAngle("Out Angle"); - outputAngle = DashBoard.makeDisplayAngle("OUTPUT_ANGLE"); - outputBoolean = DashBoard.makeDisplayBoolean("OUTPUT_BOOLEAN"); - outputDouble = DashBoard.makeDisplayDouble("OUTPUT_DOUBLE"); - outputFloat = DashBoard.makeDisplayFloat("OUTPUT_FLOAT"); - DashBoard.nextColumn("Config"); - inputAngle = DashBoard.makeConfigAngle("INPUT_ANGLE"); - inputBoolean = DashBoard.makeConfigBoolean("INPUT_BOOLEAN"); - inputDouble = DashBoard.makeConfigDouble("INPUT_DOUBLE"); - inputFloat = DashBoard.makeConfigFloat("INPUT_FLOAT"); - DashBoard.nextColumn("Input"); - inputLength = DashBoard.makeConfigLength("INPUT_LENGTH"); - inputLong = DashBoard.makeConfigLong("INPUT_LONG"); - inputString = DashBoard.makeConfigString("INPUT_STRING"); - inputTime = DashBoard.makeConfigTime("INPUT_TIME"); - DashBoard.nextColumn("Debug-Info"); - outputLength = DashBoard.makeInfoLength("OUTPUT_LENGTH"); - outputLong = DashBoard.makeInfoLong("OUTPUT_LONG"); - outputString = DashBoard.makeInfoString("OUTPUT_STRING"); - outputTime = DashBoard.makeInfoTime("OUTPUT_TIME"); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - outputAngle.setAngle(inputAngle); - outputBoolean.setValue(inputBoolean.getValue()); - outputDouble.setValue(inputDouble.getValue()); - outputFloat.setValue(inputFloat.getValue()); - outputLength.setLength(inputLength); - outputLong.setValue(inputLong.getValue()); - outputString.setValue(inputString.getValue()); - outputTime.setTime(inputTime); - outAngle.setAngle(Degrees.of(Math.random() * 360.0)); - } -} diff --git a/src/main/java/frc/robot/example/ExampleRobot.java b/src/main/java/frc/robot/example/ExampleRobot.java deleted file mode 100644 index 0c8873a9..00000000 --- a/src/main/java/frc/robot/example/ExampleRobot.java +++ /dev/null @@ -1,59 +0,0 @@ -// 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.example; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.example.commands.ExampleCommands; -import frc.robot.example.subsystems.ExampleSubsystem; -import org.frc5010.common.arch.GenericRobot; -import org.frc5010.common.config.ConfigConstants; -import org.frc5010.common.constants.SwerveConstants; -import org.frc5010.common.drive.GenericDrivetrain; -import org.frc5010.common.sensors.Controller; - -/** This is an example robot class. */ -public class ExampleRobot extends GenericRobot { - SwerveConstants swerveConstants; - GenericDrivetrain drivetrain; - DisplayValueSubsystem displayValueSubsystem = new DisplayValueSubsystem(); - ExampleSubsystem exampleSubsystem; - ExampleCommands exampleCommands; - - public ExampleRobot(String directory) { - super(directory); - drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN); - exampleSubsystem = new ExampleSubsystem(); - exampleCommands = new ExampleCommands(subsystems); - } - - @Override - public void configureButtonBindings(Controller driver, Controller operator) { - exampleCommands.configureButtonBindings(driver, operator); - } - - @Override - public void setupDefaultCommands(Controller driver, Controller operator) { - exampleCommands.setDefaultCommands(driver, operator); - drivetrain.setDefaultCommand(drivetrain.createDefaultCommand(driver)); - } - - @Override - public void initAutoCommands() { - drivetrain.setAutoBuilder(); - } - - @Override - public Command generateAutoCommand(Command autoCommand) { - return drivetrain.generateAutoCommand(autoCommand); - } - - @Override - public void buildAutoCommands() { - super.buildAutoCommands(); - selectableCommand.addOption("Do Nothing", Commands.none()); - drivetrain.addAutoCommands(selectableCommand); - } -} diff --git a/src/main/java/frc/robot/example/commands/ExampleCommands.java b/src/main/java/frc/robot/example/commands/ExampleCommands.java deleted file mode 100644 index 99bdb403..00000000 --- a/src/main/java/frc/robot/example/commands/ExampleCommands.java +++ /dev/null @@ -1,131 +0,0 @@ -// 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.example.commands; - -import static edu.wpi.first.units.Units.Inches; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.example.subsystems.ExampleSubsystem; -import java.util.Map; -import org.frc5010.common.arch.GenericSubsystem; -import org.frc5010.common.arch.StateMachine; -import org.frc5010.common.arch.StateMachine.State; -import org.frc5010.common.config.ConfigConstants; -import org.frc5010.common.drive.GenericDrivetrain; -import org.frc5010.common.sensors.Controller; -import org.frc5010.common.telemetry.DisplayString; -import org.frc5010.common.telemetry.DisplayValuesHelper; - -/** Add your docs here. */ -public class ExampleCommands { - private StateMachine stateMachine; - private DisplayString commandState; - private DisplayValuesHelper DisplayHelper; - private State intakeState; - private State lowState; - private State prepState; - private State readyState; - private ExampleSubsystem launcher; - private GenericDrivetrain drivetrain; - private Map subsystems; - private Translation2d target = new Translation2d(Inches.of(182.11), Inches.of(158.84)); - - private static enum LauncherState { - INTAKE, - LOW_SPEED, - PREP_SHOOT, - READY_TO_SHOOT - } - - private LauncherState requestedState = LauncherState.INTAKE; - - public ExampleCommands(Map subsystems) { - this.subsystems = subsystems; - DisplayHelper = new DisplayValuesHelper("LauncherCommands", "Values"); - commandState = DisplayHelper.makeDisplayString("Launcher State"); - - launcher = (ExampleSubsystem) subsystems.get(ExampleSubsystem.class.getSimpleName()); - drivetrain = (GenericDrivetrain) this.subsystems.get(ConfigConstants.DRIVETRAIN); - - stateMachine = new StateMachine("LauncherStateMachine"); - intakeState = stateMachine.addState("INTAKE", intakeStateCommand()); - lowState = stateMachine.addState("LOW-SPEED", lowStateCommand()); - prepState = stateMachine.addState("PREP-SHOOT", prepStateCommand()); - readyState = stateMachine.addState("READY-TO-SHOOT", readyStateCommand()); - stateMachine.setInitialState(intakeState); - } - - public void setDefaultCommands(Controller driver, Controller operator) { - if (launcher != null) { - stateMachine.addRequirements(launcher); - launcher.setDefaultCommand(stateMachine); - } - } - - public void configureButtonBindings(Controller driver, Controller operator) { - driver.createRightBumper().onTrue(shouldPrepCommand()).onFalse(shouldIntakeCommand()); - driver.createLeftBumper().onTrue(shouldShootCommand()).onFalse(shouldPrepCommand()); - driver.createAButton().onTrue(shouldIntakeCommand()).onFalse(shouldUseLowSpeed()); - - driver.createBButton().whileTrue(launcher.sysIdPivot()); - - lowState.switchTo(prepState).when(() -> requestedState == LauncherState.PREP_SHOOT); - prepState.switchTo(lowState).when(() -> requestedState == LauncherState.LOW_SPEED); - - prepState.switchTo(readyState).when(() -> requestedState == LauncherState.READY_TO_SHOOT); - readyState.switchTo(prepState).when(() -> requestedState == LauncherState.PREP_SHOOT); - - intakeState.switchTo(lowState).when(() -> requestedState == LauncherState.LOW_SPEED); - lowState.switchTo(intakeState).when(() -> requestedState == LauncherState.INTAKE); - } - - public Command shouldPrepCommand() { - return Commands.runOnce(() -> requestedState = LauncherState.PREP_SHOOT); - } - - public Command shouldUseLowSpeed() { - return Commands.runOnce(() -> requestedState = LauncherState.LOW_SPEED); - } - - public Command shouldShootCommand() { - return Commands.runOnce(() -> requestedState = LauncherState.READY_TO_SHOOT); - } - - public Command shouldIntakeCommand() { - return Commands.runOnce(() -> requestedState = LauncherState.INTAKE); - } - - private Translation2d getTargetPose() { - return target.minus(drivetrain.getPoseEstimator().getCurrentPose().getTranslation()); - } - - private Command intakeStateCommand() { - return Commands.parallel( - Commands.runOnce(() -> commandState.setValue("Intake")), - launcher.stopTrackingCommand(), - launcher.intakeCommand()); - } - - private Command lowStateCommand() { - return Commands.parallel( - Commands.runOnce(() -> commandState.setValue("Low Speed")), - launcher.stopIntakeCommand(), - launcher.trackTargetCommand(() -> getTargetPose())); - } - - private Command prepStateCommand() { - return Commands.parallel( - Commands.runOnce(() -> commandState.setValue("Prep")), - launcher.trackTargetCommand(() -> getTargetPose())); - } - - private Command readyStateCommand() { - return Commands.parallel( - Commands.runOnce(() -> commandState.setValue("Ready")), - launcher.trackTargetCommand(() -> getTargetPose())); - } -} diff --git a/src/main/java/frc/robot/example/subsystems/ExampleIO.java b/src/main/java/frc/robot/example/subsystems/ExampleIO.java deleted file mode 100644 index 89a53ee0..00000000 --- a/src/main/java/frc/robot/example/subsystems/ExampleIO.java +++ /dev/null @@ -1,79 +0,0 @@ -// 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.example.subsystems; - -import static edu.wpi.first.units.Units.Degrees; - -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import org.littletonrobotics.junction.AutoLog; - -/** Add your docs here. */ -public interface ExampleIO { - @AutoLog - public static class ExampleIOInputs { - public double shooterSpeedDesired = 0.0; - public double elevatorHeightDesired = 0.0; - public Angle hoodAngleDesired = Degrees.of(0.0); - public Angle turretAngleDesired = Degrees.of(0.0); - - public double shooterSpeedActual = 0.0; - public double elevatorHeightActual = 0.0; - public Angle hoodAngleActual = Degrees.of(0.0); - public Angle turretAngleActual = Degrees.of(0.0); - - public boolean shooterSpeedAtGoal = false; - public boolean elevatorHeightAtGoal = false; - public boolean hoodAngleAtGoal = false; - public boolean turretAngleAtGoal = false; - - public double shooterSpeedError = 0.0; - public double elevatorHeightError = 0.0; - public double hoodAngleError = 0.0; - public double turretAngleError = 0.0; - - public double hoodVelocity = 0.0; - public double turretVelocity = 0.0; - public double elevatorVelocity = 0.0; - public double shooterMotorOutput = 0.0; - public double elevatorMotorOutput = 0.0; - } - - public default void updateInputs(ExampleIOInputs inputs) {} - - public default void updateSimulation() {} - - public void setPercentMotor(double output); - - public Command setDutyCycle(double output); - - public void runShooter(double speed); - - public Command setUpperSpeed(AngularVelocity speed); - - public Command setElevatorHeight(double height); - - public void setHoodAngle(Angle angle); - - public void setTurretRotation(Angle angle); - - public AngularVelocity getShooterVelocity(); - - public Command sysIdShooter(); - - public Command sysIdArm(); - - public Command sysIdPivot(); - - public Command sysIdTurret(); - - public default Command addBallToRobot() { - return Commands.none(); - } - - public Command launchBall(); -} diff --git a/src/main/java/frc/robot/example/subsystems/ExampleIOReal.java b/src/main/java/frc/robot/example/subsystems/ExampleIOReal.java deleted file mode 100644 index 6bfd0a00..00000000 --- a/src/main/java/frc/robot/example/subsystems/ExampleIOReal.java +++ /dev/null @@ -1,117 +0,0 @@ -// 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.example.subsystems; - -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import java.util.Map; -import org.frc5010.common.arch.GenericSubsystem; -import org.frc5010.common.motors.SystemIdentification; -import org.frc5010.common.motors.function.AngularControlMotor; -import org.frc5010.common.motors.function.PercentControlMotor; -import org.frc5010.common.motors.function.VelocityControlMotor; -import yams.mechanisms.positional.Arm; -import yams.mechanisms.positional.Pivot; -import yams.mechanisms.velocity.FlyWheel; - -/** Add your docs here. */ -public class ExampleIOReal implements ExampleIO { - protected Map devices; - protected PercentControlMotor percentMotor; - protected VelocityControlMotor controlledMotor; - protected AngularControlMotor angularMotor; - protected FlyWheel shooter; - protected Arm arm; - protected GenericSubsystem parent; - protected Pivot pivot; - - public ExampleIOReal(Map devices, GenericSubsystem parent) { - this.devices = devices; - this.parent = parent; - this.percentMotor = (PercentControlMotor) devices.get("percent_motor"); - this.controlledMotor = (VelocityControlMotor) devices.get("velocity_motor"); - this.shooter = (FlyWheel) devices.get("Shooter"); - this.arm = (Arm) devices.get("Hood"); - this.pivot = (Pivot) devices.get("Turret"); - this.angularMotor = (AngularControlMotor) devices.get("angular_motor"); - } - - @Override - public void updateInputs(ExampleIOInputs inputs) { - - angularMotor.periodicUpdate(); - } - - @Override - public void setPercentMotor(double output) { - percentMotor.set(output); - } - - @Override - public void runShooter(double speed) { - shooter.getMotor().setDutyCycle(speed); - } - - @Override - public Command setUpperSpeed(AngularVelocity speed) { - return shooter.setSpeed(speed); - } - - @Override - public Command setElevatorHeight(double height) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setElevatorHeight'"); - } - - @Override - public void setHoodAngle(Angle angle) { - arm.getMotorController().setPosition(angle); - } - - @Override - public void setTurretRotation(Angle angle) {} - - public AngularVelocity getShooterVelocity() { - return shooter.getSpeed(); - } - - public Command sysIdShooter() { - return SystemIdentification.getSysIdFullCommand( - SystemIdentification.rpmSysIdRoutine(shooter.getMotor(), parent.getName(), parent), - 5, - 3, - 3); - } - - public Command sysIdTurret() { - return SystemIdentification.getSysIdFullCommand( - SystemIdentification.angleSysIdRoutine( - pivot.getMotorController(), parent.getName(), parent), - 5, - 3, - 3); - } - - public Command sysIdArm() { - return arm.sysId(Volts.of(12), Volts.of(1).per(Seconds), Seconds.of(10)); - } - - public Command sysIdPivot() { - return pivot.sysId(Volts.of(12), Volts.of(1).per(Seconds), Seconds.of(10)); - } - - public Command launchBall() { - return shooter.set(0); - } - - public Command setDutyCycle(double output) { - return Commands.runOnce(() -> percentMotor.set(output), parent); - } -} diff --git a/src/main/java/frc/robot/example/subsystems/ExampleIOSim.java b/src/main/java/frc/robot/example/subsystems/ExampleIOSim.java deleted file mode 100644 index 3527c0a1..00000000 --- a/src/main/java/frc/robot/example/subsystems/ExampleIOSim.java +++ /dev/null @@ -1,115 +0,0 @@ -// 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.example.subsystems; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.Map; -import org.frc5010.common.arch.GenericSubsystem; -import org.frc5010.common.drive.GenericDrivetrain; -import org.frc5010.common.drive.swerve.YAGSLSwerveDrivetrain; -import org.frc5010.lobbinloco.FRC5010BallOnTheFly; -import org.littletonrobotics.junction.Logger; -import swervelib.simulation.ironmaple.simulation.IntakeSimulation; -import swervelib.simulation.ironmaple.simulation.IntakeSimulation.IntakeSide; -import swervelib.simulation.ironmaple.simulation.SimulatedArena; -import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile; -import swervelib.simulation.ironmaple.simulation.seasonspecific.crescendo2024.NoteOnFly; -import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; - -/** Add your docs here. */ -public class ExampleIOSim extends ExampleIOReal { - protected IntakeSimulation intakeSimulation; - protected NoteOnFly noteOnFly; - protected RebuiltFuelOnFly fuelOnFly; - protected GamePieceProjectile gamePieceProjectile; - - public ExampleIOSim(Map devices, GenericSubsystem parent) { - super(devices, parent); - intakeSimulation = - IntakeSimulation.InTheFrameIntake( - "FRC5010Ball", - GenericDrivetrain.getMapleSimDrive().get(), - Inches.of(24.25), - IntakeSide.FRONT, - 1); - } - - @Override - public void updateSimulation() { - angularMotor.simulationUpdate(); - } - - @Override - public void setPercentMotor(double speed) { - if (speed > 0.0 && !noteIsInsideIntake().getAsBoolean()) { - intakeSimulation.startIntake(); - } else { - intakeSimulation.stopIntake(); - } - super.setPercentMotor(speed); - } - - public Trigger obtainedGamePieceToScore() { - return new Trigger( - () -> { - return RobotBase.isSimulation() - ? intakeSimulation.getGamePiecesAmount() == 1 - && intakeSimulation.obtainGamePieceFromIntake() - : false; - }); - } - - public Trigger noteIsInsideIntake() { - return new Trigger( - () -> { - return RobotBase.isSimulation() ? intakeSimulation.getGamePiecesAmount() > 0 : false; - }); - } - - public Command addBallToRobot() { - return Commands.runOnce(() -> intakeSimulation.addGamePieceToIntake()); - } - - @Override - public Command launchBall() { - return super.launchBall() - .alongWith( - Commands.runOnce( - () -> { - if (RobotBase.isSimulation()) { - Pose2d worldPose = YAGSLSwerveDrivetrain.getSwerveDrive().getPose(); - gamePieceProjectile = - new FRC5010BallOnTheFly( - worldPose.getTranslation(), - controlledMotor - .getRobotToMotor() - .getTranslation() - .toTranslation2d(), - YAGSLSwerveDrivetrain.getSwerveDrive().getFieldVelocity(), - worldPose.getRotation(), - Meters.of(0.45), - MetersPerSecond.of(10), - Degrees.of(55)) - .withProjectileTrajectoryDisplayCallBack( - (pose3ds) -> { - Logger.recordOutput( - parent.getName() + "/GPTrajectory", - pose3ds.toArray(Pose3d[]::new)); - }); - SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile); - } - })); - } -} diff --git a/src/main/java/frc/robot/example/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/example/subsystems/ExampleSubsystem.java deleted file mode 100644 index b8398c82..00000000 --- a/src/main/java/frc/robot/example/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,179 +0,0 @@ -package frc.robot.example.subsystems; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; - -import com.revrobotics.spark.SparkMax; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.example.subsystems.ExampleIO.ExampleIOInputs; -import java.util.function.Supplier; -import org.frc5010.common.arch.GenericSubsystem; -import org.frc5010.common.constants.GenericPID; -import org.frc5010.common.constants.MotorFeedFwdConstants; -import org.frc5010.common.motors.MotorConstants.Motor; -import org.frc5010.common.motors.MotorFactory; -import org.frc5010.common.motors.function.AngularControlMotor; -import org.frc5010.common.sensors.absolute_encoder.RevAbsoluteEncoder; - -public class ExampleSubsystem extends GenericSubsystem { - protected ExampleIO io; - protected ExampleIOInputs inputs = new ExampleIOInputs(); - protected int scoredNotes = 0; - protected Rotation2d rotation = new Rotation2d(Degrees.of(180)); - - public ExampleSubsystem() { - super("example.json"); - devices.put("angular_motor", angularControlledMotor()); - if (RobotBase.isSimulation()) { - io = new ExampleIOSim(devices, this); - } else { - io = new ExampleIOReal(devices, this); - } - } - - public Command trackTargetCommand(Supplier targetSupplier) { - return Commands.run( - () -> { - Translation2d targetPose = targetSupplier.get(); - targetPose.getNorm(); - io.setTurretRotation(targetPose.getAngle().getMeasure()); - io.setHoodAngle(Degrees.of(45)); - }); - } - - public Command stopTrackingCommand() { - return Commands.runOnce( - () -> { - io.setTurretRotation(Degrees.of(0)); - io.setHoodAngle(Degrees.of(0)); - }); - } - - public Command intakeCommand() { - return Commands.runOnce(() -> io.setPercentMotor(0.5), this); - } - - public Command stopIntakeCommand() { - return Commands.runOnce(() -> io.setPercentMotor(0.0), this); - } - - public Command sysIdShooter() { - return io.sysIdShooter(); - } - - public Command addBallToRobot() { - return io.addBallToRobot(); - } - - public Command launchBall() { - return io.launchBall(); - } - - public Command sysIdArm() { - return io.sysIdArm(); - } - - public Command sysIdPivot() { - return io.sysIdPivot(); - } - - public Command sysIdTurret() { - return io.sysIdTurret(); - } - - @Override - public void periodic() { - super.periodic(); - io.updateInputs(inputs); - } - - @Override - public void simulationPeriodic() { - super.simulationPeriodic(); - io.updateSimulation(); - } - - public AngularControlMotor angularControlledMotor() { - AngularControlMotor angularMotor = - new AngularControlMotor( - MotorFactory.Spark(13, Motor.Neo), "angular", getDisplayValuesHelper()) - .setupSimulatedMotor( - (5.0 * 68.0 / 24.0) * (80.0 / 24.0), - Units.lbsToKilograms(22), - Inches.of(19), - Degrees.of(0), - Degrees.of(360), - false, - 0, - Degrees.of(0), - false, - 0.1) - .setVisualizer(mechanismSimulation, new Pose3d(0.75, 0, 0.25, new Rotation3d())); - angularMotor.setEncoder(new RevAbsoluteEncoder((SparkMax) angularMotor.getMotor(), 360)); - angularMotor.setValues(new GenericPID(0.01, 0.000025, 0.003)); - angularMotor.setMotorFeedFwd(new MotorFeedFwdConstants(0.0, 0.01, 0.0, false)); - angularMotor.setIZone(3); - angularMotor.setOutputRange(-12, 12); - return angularMotor; - } - - // public Command setVelocityControlMotorReference(DoubleSupplier reference) { - // return Commands.runOnce( - // () -> { - // double speed = reference.getAsDouble(); - // if (speed <= 0.0 && !noteIsInsideIntake().getAsBoolean()) { - // controlledMotor.setReference(speed); - // } else if (speed > 3000 - // && noteIsInsideIntake().getAsBoolean() - // && obtainedGamePieceToScore().getAsBoolean()) { - // controlledMotor.setReference(speed); - // if (RobotBase.isSimulation()) { - // Pose2d worldPose = YAGSLSwerveDrivetrain.getSwerveDrive().getPose(); - // gamePieceProjectile = - // new ReefscapeAlgaeOnFly( - // worldPose.getTranslation(), - // controlledMotor.getRobotToMotor().getTranslation().toTranslation2d(), - // YAGSLSwerveDrivetrain.getSwerveDrive().getFieldVelocity(), - // worldPose.getRotation(), - // Meters.of(0.45), - // MetersPerSecond.of(speed / 6000 * 20), - // Degrees.of(55)); - // SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile); - // } - // } else if (speed < 3000 - // && speed > 1000 - // && noteIsInsideIntake().getAsBoolean() - // && obtainedGamePieceToScore().getAsBoolean()) { - // controlledMotor.setReference(speed); - // if (RobotBase.isSimulation()) { - // Pose2d worldPose = YAGSLSwerveDrivetrain.getSwerveDrive().getPose(); - // gamePieceProjectile = - // new ReefscapeAlgaeOnFly( - // worldPose.getTranslation(), - // controlledMotor.getRobotToMotor().getTranslation().toTranslation2d(), - // YAGSLSwerveDrivetrain.getSwerveDrive().getFieldVelocity(), - // worldPose.getRotation(), - // Meters.of(0.45), - // MetersPerSecond.of(speed / 6000 * 20), - // Degrees.of(55)); - // SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile); - // } - // } else { - // controlledMotor.setReference(speed); - // } - // }, - // this); - // } - - // public Command setAngularMotorReference(DoubleSupplier reference) { - // return Commands.runOnce(() -> angularMotor.setReference(reference.getAsDouble()), this); - // } - -} diff --git a/src/main/java/frc/robot/rebuilt/Constants.java b/src/main/java/frc/robot/rebuilt/Constants.java new file mode 100644 index 00000000..6380c2b7 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/Constants.java @@ -0,0 +1,84 @@ +package frc.robot.rebuilt; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inch; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import frc.robot.rebuilt.subsystems.Climb.Climb; + +public class Constants { + public static final String INDEXER = Indexer.class.getSimpleName(); + public static final String CLIMB = Climb.class.getSimpleName(); + public static final String INTAKE = Intake.class.getSimpleName(); + public static final String LAUNCHER = Launcher.class.getSimpleName(); + /** Defines the maxinum distance for the climb */ + public static class ClimbConstants { + public static final Distance MAX = Inch.of(27); + } + + public static class Launcher { + public static final double SHOOTER_TOLERANCE_RPM = 50.0; + public static final double HOOD_ANGLE_TOLERANCE_DEGREES = 3.5; + public static final double TURRET_ANGLE_TOLERANCE_DEGREES = 5.0; + public static final Angle HOPPER_EXTENSION_BUFFER_BEFORE_AIM = Degrees.of(10); + public static final double HOOD_STALL_CURRENT_THRESHOLD = 20.0; + + public static final double HOOD_LEGACY_START_ANGLE_DEGREES = 30.0; + public static final double HOOD_CORRECTED_START_ANGLE_DEGREES = 12.723; + public static final double HOOD_CORRECTED_END_ANGLE_DEGREES = 45.723; + public static final double HOOD_CALIBRATION_OFFSET_DEGREES = + HOOD_CORRECTED_START_ANGLE_DEGREES - HOOD_LEGACY_START_ANGLE_DEGREES; + + public static double offsetLegacyHoodAngleDegrees(double legacyAngleDegrees) { + return legacyAngleDegrees + HOOD_CALIBRATION_OFFSET_DEGREES; + } + + public static final Angle LOW_HOOD_ANGLE = Degrees.of(offsetLegacyHoodAngleDegrees(31.0)); + public static final AngularVelocity LOW_FLYWHEEL_RPM = RotationsPerSecond.of(1); + + public static final Angle HUB_HOOD_ANGLE = LOW_HOOD_ANGLE; + public static final AngularVelocity HUB_FLYWHEEL_RPM = RotationsPerSecond.of(1.25); + + public static final Angle TOWER_HOOD_ANGLE = Degrees.of(offsetLegacyHoodAngleDegrees(40.0)); + public static final AngularVelocity TOWER_FLYWHEEL_RPM = RotationsPerSecond.of(1.5); + + public static final Angle TURRET_FORWARD = Degrees.of(0); + public static final AngularVelocity FWD_FLYWHEEL_RPM = LOW_FLYWHEEL_RPM; + public static final Angle FWD_HOOD_ANGLE = LOW_HOOD_ANGLE; + } + + public static class Indexer { + public static final double SPINDEXER_SPEED = 0.90; + public static final double TRANSFER_SPEED = 1.0; + public static final double TRANSFER_CHURN = 0.25; + } + + public static class Intake { + public static final double HOPPER_GO_OUT = -0.3; + public static final double HOPPER_GO_IN = 0.2; + public static final double INTAKE_IN = 1.0; + public static final double INTAKE_INNER_IN = 0.3; + public static final double INTAKE_AUTO = 1.0; + public static final double INTAKE_DEADZONE = 0.25; + public static final double INTAKE_CHURN = -0.25; + public static final double INTAKE_MAX_IN = 0.9; + public static final double INTAKE_MAX_OUT = -0.9; + public static final double HOPPER_ANGLE_TOLERANCE = 3; + public static final double HOPPER_STALL_TIME = 0.3; + public static final Angle HOPPER_RETRACTED_ANGLE = Degrees.of(120); + public static final Angle HOPPER_DEPLOYED_ANGLE = Degrees.of(0); + public static final Angle HOPPER_ANGLED = Degrees.of(45); + public static final double HOPPER_STALL_CURRENT_THRESHOLD = 80.0; + public static final double HOPPER_MOVING_VELOCITY_THRESHOLD = 1.0; + public static final double HOPPER_DEPLOY_STOP_REZERO_MAX_ANGLE = 20.0; + public static final double HOPPER_AUTO_REZERO_THRESHOLD = + -1.0; // degrees — if hopper goes past 0, auto-rezero + public static final double HOPPER_FIRST_DEPLOY_DUTY = + -0.2; // duty cycle for first-deploy zeroing nudge + public static final double HOPPER_DEPLOY_NUDGE_DUTY = + -0.35; // duty cycle for normal deploy nudge after PID + } +} diff --git a/src/main/java/frc/robot/rebuilt/FieldConstants.java b/src/main/java/frc/robot/rebuilt/FieldConstants.java new file mode 100644 index 00000000..9412d55c --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/FieldConstants.java @@ -0,0 +1,406 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.rebuilt; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; +import org.frc5010.common.vision.AprilTags; + +/** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ +public class FieldConstants { + + public static final FieldType fieldType = FieldType.ANDYMARK; + + // AprilTag related constants + public static final int aprilTagCount = AprilTags.aprilTagFieldLayout.getTags().size(); + public static final double aprilTagWidth = Units.inchesToMeters(6.5); + + // Field dimensions + public static final double fieldLength = AprilTags.aprilTagFieldLayout.getFieldLength(); + public static final double fieldWidth = AprilTags.aprilTagFieldLayout.getFieldWidth(); + public static final Distance FIELD_LENGTH = Meters.of(fieldLength); + public static final Distance FIELD_WIDTH = Meters.of(fieldWidth); + public static final Translation2d TRENCH_HALF_WIDTH = + new Translation2d(Meters.of(1.5), Meters.of(0)); + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - Units.inchesToMeters(120); + public static final double neutralZoneFar = center + Units.inchesToMeters(120); + public static final double oppHubCenter = + AprilTags.aprilTagFieldLayout.getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + AprilTags.aprilTagFieldLayout.getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + + // Dimensions + public static final double width = Units.inchesToMeters(47.0); + public static final double height = + Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = Units.inchesToMeters(41.7); + public static final double innerHeight = Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + AprilTags.aprilTagFieldLayout.getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + AprilTags.aprilTagFieldLayout.getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = + AprilTags.aprilTagFieldLayout.getTagPose(26).get().toPose2d(); + public static final Translation2d centerFace = + new Translation2d(nearFace.getX(), fieldWidth / 2.0); + public static final Pose2d farFace = + AprilTags.aprilTagFieldLayout.getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + AprilTags.aprilTagFieldLayout.getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = + AprilTags.aprilTagFieldLayout.getTagPose(21).get().toPose2d(); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + } + + public static class TrenchZoneTop { + public static Translation2d nearAlliance = LeftTrench.openingTopRight.toTranslation2d(); + public static Translation2d nearAllianceLeftDanger = + LeftTrench.openingTopRight.toTranslation2d().minus(TRENCH_HALF_WIDTH); + public static Translation2d nearAllianceRightDanger = + LeftTrench.openingTopRight.toTranslation2d().plus(TRENCH_HALF_WIDTH); + + public static Translation2d oppAlliance = LeftTrench.oppOpeningTopRight.toTranslation2d(); + public static Translation2d oppAllianceLeftDanger = + LeftTrench.oppOpeningTopRight.toTranslation2d().minus(TRENCH_HALF_WIDTH); + public static Translation2d oppAllianceRightDanger = + LeftTrench.oppOpeningTopRight.toTranslation2d().plus(TRENCH_HALF_WIDTH); + } + + public static class TrenchZoneBottom { + public static Translation2d nearAlliance = RightTrench.openingTopRight.toTranslation2d(); + public static Translation2d nearAllianceLeftDanger = + RightTrench.openingTopRight.toTranslation2d().minus(TRENCH_HALF_WIDTH); + public static Translation2d nearAllianceRightDanger = + RightTrench.openingTopRight.toTranslation2d().plus(TRENCH_HALF_WIDTH); + + public static Translation2d oppAlliance = RightTrench.openingTopLeft.toTranslation2d(); + public static Translation2d oppAllianceLeftDanger = + RightTrench.oppOpeningTopLeft.toTranslation2d().minus(TRENCH_HALF_WIDTH); + public static Translation2d oppAllianceRightDanger = + RightTrench.oppOpeningTopLeft.toTranslation2d().plus(TRENCH_HALF_WIDTH); + } + + public static class RightTrench { + + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + public static final double width = Units.inchesToMeters(49.25); + public static final double depth = Units.inchesToMeters(45.0); + public static final double height = Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = Units.inchesToMeters(32.250); + public static final double frontFaceX = Units.inchesToMeters(43.51); + + public static final double uprightHeight = Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = Units.inchesToMeters(27.0); + public static final double midRungHeight = Units.inchesToMeters(45.0); + public static final double highRungHeight = Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(frontFaceX, AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY()); + public static final Pose2d face = + new Pose2d( + frontFaceX, + AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY(), + AprilTags.aprilTagFieldLayout.getTagPose(31).get().getRotation().toRotation2d()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (AprilTags.aprilTagFieldLayout.getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, AprilTags.aprilTagFieldLayout.getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTags.aprilTagFieldLayout.getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTags.aprilTagFieldLayout.getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + } + + public static class Depot { + // Dimensions + public static final double width = Units.inchesToMeters(42.0); + public static final double depth = Units.inchesToMeters(27.0); + public static final double height = Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + } + + public static class Outpost { + // Dimensions + public static final double width = Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); + public static final double height = Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, AprilTags.aprilTagFieldLayout.getTagPose(29).get().getY()); + } + + public enum FieldType { + ANDYMARK("andymark"), + WELDED("welded"); + + private final String jsonFolder; + + FieldType(String jsonFolder) { + this.jsonFolder = jsonFolder; + } + + public String getJsonFolder() { + return jsonFolder; + } + } + + // public enum AprilTagLayoutType { + // OFFICIAL("2026-official"), + // NONE("2026-none"); + + // private final String name; + // private volatile AprilTagFieldLayout layout; + // private volatile String layoutString; + + // AprilTagLayoutType(String name) { + // this.name = name; + // } + + // public AprilTagFieldLayout getLayout() { + // if (layout == null) { + // synchronized (this) { + // if (layout == null) { + // try { + // Path p = + // Constants.disableHAL + // ? Path.of( + // "src", + // "main", + // "deploy", + // "apriltags", + // fieldType.getJsonFolder(), + // name + ".json") + // : Path.of( + // Filesystem.getDeployDirectory().getPath(), + // "apriltags", + // fieldType.getJsonFolder(), + // name + ".json"); + // layout = new AprilTagFieldLayout(p); + // layoutString = new ObjectMapper().writeValueAsString(layout); + // } catch (IOException e) { + // throw new RuntimeException(e); + // } + // } + // } + // } + // return layout; + // } + + // public String getLayoutString() { + // if (layoutString == null) { + // getLayout(); + // } + // return layoutString; + // } + // } +} diff --git a/src/main/java/frc/robot/rebuilt/HubTracker.java b/src/main/java/frc/robot/rebuilt/HubTracker.java new file mode 100644 index 00000000..33fb8045 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/HubTracker.java @@ -0,0 +1,193 @@ +package frc.robot.rebuilt; + +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import java.util.Optional; + +public class HubTracker { + /** + * Returns an {@link Optional} containing the current {@link Shift}. Will return {@link + * Optional#empty()} if disabled or in between auto and teleop. + */ + public static Optional getCurrentShift() { + double matchTime = getMatchTime(); + if (matchTime < 0) return Optional.empty(); + + for (Shift shift : Shift.values()) { + if (matchTime < shift.endTime) { + return Optional.of(shift); + } + } + return Optional.empty(); + } + + /** + * Returns an {@link Optional} containing the current {@link Time} remaining in the current shift. + * Will return {@link Optional#empty()} if disabled or in between auto and teleop. + */ + public static Optional

Values:

+ * + *
    + *
  • {@link Shift#AUTO} (0-20 sec) + *
  • {@link Shift#TRANSITION} (20-30 sec) + *
  • {@link Shift#SHIFT_1} (30-55 sec) + *
  • {@link Shift#SHIFT_2} (55-80 sec) + *
  • {@link Shift#SHIFT_3} (80-105 sec) + *
  • {@link Shift#SHIFT_4} (105-130 sec) + *
  • {@link Shift#ENDGAME} (130-160 sec) + *
+ */ + /** Configures the start and end times of shifts and stores ActiveType and */ + public enum Shift { + AUTO(0, 20, ActiveType.BOTH), + TRANSITION(20, 30, ActiveType.BOTH), + SHIFT_1(30, 55, ActiveType.AUTO_LOSER), + SHIFT_2(55, 80, ActiveType.AUTO_WINNER), + SHIFT_3(80, 105, ActiveType.AUTO_LOSER), + SHIFT_4(105, 130, ActiveType.AUTO_WINNER), + ENDGAME(130, 160, ActiveType.BOTH); + + final int startTime; + final int endTime; + final ActiveType activeType; + /** Constructs a shift with the range of time and active type constraints */ + private Shift(int startTime, int endTime, ActiveType activeType) { + this.startTime = startTime; + this.endTime = endTime; + this.activeType = activeType; + } + } + /** Defines possible states and types for the active hub tracker */ + private enum ActiveType { + BOTH, + AUTO_WINNER, + AUTO_LOSER + } +} diff --git a/src/main/java/frc/robot/rebuilt/Rebuilt.java b/src/main/java/frc/robot/rebuilt/Rebuilt.java new file mode 100644 index 00000000..e1af24b1 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/Rebuilt.java @@ -0,0 +1,165 @@ +// 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.rebuilt; + +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.commands.AutoCommands; +import frc.robot.rebuilt.commands.ClimbCommands; +import frc.robot.rebuilt.commands.IndexerCommands; +import frc.robot.rebuilt.commands.IntakeCommands; +import frc.robot.rebuilt.commands.LauncherCommands; +import frc.robot.rebuilt.commands.NamedCommandsReg; +import frc.robot.rebuilt.commands.TestCommands; +import frc.robot.rebuilt.subsystems.Climb.Climb; +import frc.robot.rebuilt.subsystems.DriverDisplay.HubStatus; +import frc.robot.rebuilt.subsystems.Indexer.Indexer; +import frc.robot.rebuilt.subsystems.Launcher.FieldRegions; +import frc.robot.rebuilt.subsystems.Launcher.Launcher; +import frc.robot.rebuilt.subsystems.intake.Intake; +import org.frc5010.common.arch.GenericRobot; +import org.frc5010.common.config.ConfigConstants; +import org.frc5010.common.drive.GenericDrivetrain; +import org.frc5010.common.sensors.Controller; +import org.frc5010.common.utils.OrchestraManager; +import org.frc5010.common.utils.geometry.AllianceFlipUtil; + +/** This is an example robot class. */ +/** Long's correction: Main robot class that initializes subsystems and commands */ +public class Rebuilt extends GenericRobot { + public static String configDirectory = "rebuilt_robot"; + public static HubStatus hubStatus = new HubStatus(); + public static GenericDrivetrain drivetrain; + public static Indexer indexer; + public static Climb climb; + public static Intake intake; + public static Launcher launcher; + public static LauncherCommands launcherCommands; + public static AutoCommands autocommands; + public static ClimbCommands climbCommands; + public static IntakeCommands intakecommands; + public static IndexerCommands indexerCommands; + public static TestCommands testCommands; + public static boolean isZeroingBurst = false; + private boolean isButtonsConfigured = false; + private boolean isAltButtonsConfigured = false; + + public Rebuilt(String directory) { + super(directory); + configDirectory = directory; + AllianceFlipUtil.configure(FieldConstants.FIELD_WIDTH, FieldConstants.FIELD_LENGTH); + /** creating robot subsystems */ + indexer = new Indexer(); + // climb = new Climb(); + intake = new Intake(); + launcher = new Launcher(subsystems); + drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN); + /** creates command containers */ + testCommands = new TestCommands(subsystems); + climbCommands = new ClimbCommands(subsystems); + launcherCommands = new LauncherCommands(subsystems); + intakecommands = new IntakeCommands(subsystems); + indexerCommands = new IndexerCommands(subsystems); + autocommands = new AutoCommands(subsystems); + // OrchestraManager.loadMusic("raiders"); + RobotController.setBrownoutVoltage(Volts.of(4.6)); + + if (operator.isPresent()) { + operator.get().createStartButton().onTrue(launcher.zeroTurretCommand()); + } + } + + @Override + public void disabledInit() { + OrchestraManager.play(); + } + + @Override + public void disabledPeriodic() { + super.disabledPeriodic(); + SmartDashboard.putBoolean("Orchestra Playing", OrchestraManager.isPlaying()); + if (launcher != null && !isZeroingBurst) { + if (launcher.isTurretAtZero()) { + org.frc5010.common.subsystems.LEDStrip.changeSegmentPattern( + org.frc5010.common.config.ConfigConstants.ALL_LEDS, + org.frc5010.common.subsystems.LEDStrip.getSolidPattern( + edu.wpi.first.wpilibj.util.Color.kGreen)); + } else { + org.frc5010.common.subsystems.LEDStrip.changeSegmentPattern( + org.frc5010.common.config.ConfigConstants.ALL_LEDS, + org.frc5010.common.subsystems.LEDStrip.getSolidPattern(chooseAllianceWpiColor())); + } + } + } + + @Override + /** Configures buttons with commands */ + public void configureButtonBindings(Controller driver, Controller operator) { + if (!isButtonsConfigured) { + FieldRegions.setupFieldRegions(); + driver.createYButton().onTrue(Commands.runOnce(() -> drivetrain.toggleFieldOrientedDrive())); + drivetrain.configureButtonBindings(driver, operator); + climbCommands.configureButtonBindings(driver, operator); + launcherCommands.configureButtonBindings(driver, operator); + intakecommands.configureButtonBindings(driver, operator); + indexerCommands.configureButtonBindings(driver, operator); + hubStatus.configureButtonBindings(driver, operator); + isButtonsConfigured = true; + } + } + + @Override + public void configureAltButtonBindings(Controller driver, Controller operator) { + // Add test mode specific button bindings here + if (!isAltButtonsConfigured) { + testCommands.configureButtonBindings(driver); + isAltButtonsConfigured = true; + } + } + + @Override + /** Assigns default commands for each subsystem */ + public void setupDefaultCommands(Controller driver, Controller operator) { + OrchestraManager.stop(); + // This is part of auto init, so a good place to run this + FieldRegions.setupFieldRegions(); + drivetrain.setDefaultCommand(drivetrain.createDefaultCommand(driver)); + launcherCommands.setDefaultCommands(); + indexerCommands.setupDefaultCommands(); + intakecommands.setupDefaultCommands(); + } + + @Override + public void initAutoCommands() { + NamedCommandsReg.createNamedCommands(); + drivetrain.setAutoBuilder(); + } + + // @Override + // public Command getAutonomousCommand() { + // if (DriverStation.isFMSAttached()) { + // intake.setHopperPosition(Constants.Intake.HOPPER_RETRACTED_ANGLE); + // } + // return super.getAutonomousCommand(); + // } + + @Override + public Command generateAutoCommand(Command autoCommand) { + return drivetrain.generateAutoCommand(autoCommand); + } + + @Override + /** Creates and registers available auto comands */ + public void buildAutoCommands() { + super.buildAutoCommands(); + selectableCommand.addOption("Do Nothing", Commands.none()); + drivetrain.addAutoCommands(selectableCommand); + autocommands.configureCharacterizationCommands(selectableCommand); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/AutoCommands.java b/src/main/java/frc/robot/rebuilt/commands/AutoCommands.java new file mode 100644 index 00000000..daeb02f1 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/AutoCommands.java @@ -0,0 +1,69 @@ +package frc.robot.rebuilt.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.subsystems.Launcher.Launcher; +import frc.robot.rebuilt.subsystems.intake.Intake; +import java.util.Map; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.drive.GenericDrivetrain; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +public class AutoCommands { + + private Map subsystems; + + public AutoCommands(Map subsystems) { + this.subsystems = subsystems; + } + + public void configureNamedCommands() {} + + public void configureCharacterizationCommands(LoggedDashboardChooser selectableCommand) { + selectableCommand.addOption( + "PRO: Intake Hopper Characterization", + ((Intake) subsystems.get(Constants.INTAKE)).getHopperCharacterizationCommand()); + selectableCommand.addOption( + "PRO: Launcher Hood Characterization", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getHoodCharacterizationCommand()); + selectableCommand.addOption( + "PRO: Launcher Turret Characterization", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretCharacterizationCommand()); + selectableCommand.addOption( + "TUNE: Shot Lookup Table Tuning", + ShotCalibrationCommand.createWithFeed( + (Launcher) subsystems.get(Constants.LAUNCHER), + (GenericDrivetrain) + subsystems.get(org.frc5010.common.config.ConfigConstants.DRIVETRAIN), + 2.0, + 0.5)); + selectableCommand.addOption( + "PRO: Turret Quasistatic (kS, kV)", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretQuasistaticCommand()); + selectableCommand.addOption( + "PRO: Turret Dynamic (kA)", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretDynamicCommand()); + selectableCommand.addOption( + "TUNE: Turret kS Map Generation", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretKsMapCommand()); + selectableCommand.addOption( + "TUNE: Turret Tracking Sinusoidal Tuning", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretTrackingTuneCommand()); + + selectableCommand.addOption( + "TUNE: Turret Seeking Tuning", + ((Launcher) subsystems.get(Constants.LAUNCHER)).getTurretSeekingTuneCommand()); + } + + public void configureBasicAutoCommands(LoggedDashboardChooser selectableCommand) { + selectableCommand.addOption( + "Shoot Preload Only", + Commands.sequence( + IntakeCommands.shouldIntaking(), + Commands.waitSeconds(2), + LauncherCommands.shouldPrepCommand(), + Commands.waitSeconds(2), + IndexerCommands.shouldForceCommand())); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/CharacterizationSample.java b/src/main/java/frc/robot/rebuilt/commands/CharacterizationSample.java new file mode 100644 index 00000000..ea9d9441 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/CharacterizationSample.java @@ -0,0 +1,17 @@ +package frc.robot.rebuilt.commands; + +/** + * A single high-frequency characterization sample collected at ~250 Hz. + * + * @param timestampSeconds FPGA timestamp in seconds when the sample was taken + * @param positionRot turret position in mechanism rotations + * @param velocityRotPerSec turret velocity in mechanism rot/s + * @param accelerationRotPerSecSq turret acceleration in mechanism rot/s^2 (from TalonFX signal) + * @param currentAmps measured torque current in Amps (from TalonFX signal) + */ +public record CharacterizationSample( + double timestampSeconds, + double positionRot, + double velocityRotPerSec, + double accelerationRotPerSecSq, + double currentAmps) {} diff --git a/src/main/java/frc/robot/rebuilt/commands/ClimbCommands.java b/src/main/java/frc/robot/rebuilt/commands/ClimbCommands.java new file mode 100644 index 00000000..daa5f24d --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/ClimbCommands.java @@ -0,0 +1,186 @@ +package frc.robot.rebuilt.commands; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.Constants.ClimbConstants; +import frc.robot.rebuilt.subsystems.Climb.Climb; +import java.util.Map; +import java.util.function.DoubleSupplier; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.arch.StateMachine; +import org.frc5010.common.arch.StateMachine.State; +import org.frc5010.common.sensors.Controller; + +public class ClimbCommands { + + private Map subsystems; + private StateMachine stateMachine; + private State idleState; + private State elevateState; + private State descendState; + private State liftedState; + private State loweredState; + private State disabledState; + private State manualState; + /** defines possible states for the climb */ + public static enum ClimbState { + IDLE, + ELEVATE, + LIFTED, + DESCEND, + LOWERED, + DISABLED, + MANUAL + } + + private static Climb climb; + private DoubleSupplier getOpleftY; + + public ClimbCommands(Map systems) { + this.subsystems = systems; + + // Create a simple state machine for climb and set it as the default command for the Climb + climb = (Climb) subsystems.get(Constants.CLIMB); + if (null == climb) { + return; + } + stateMachine = new StateMachine("ClimbStateMachine"); + // a simple idle state; transitions will be added in configureButtonBindings + idleState = + stateMachine.addState( + "idle", + Commands.runOnce(() -> climb.runClimb(0)) + .alongWith(Commands.runOnce(() -> climb.setCurrentState(ClimbState.IDLE)))); + loweredState = + stateMachine.addState( + "lowered", Commands.runOnce(() -> climb.setCurrentState(ClimbState.LOWERED))); + liftedState = + stateMachine.addState( + "lifted", Commands.runOnce(() -> climb.setCurrentState(ClimbState.LIFTED))); + disabledState = + stateMachine.addState( + "disabled", Commands.runOnce(() -> climb.setCurrentState(ClimbState.DISABLED))); + manualState = + stateMachine.addState( + "manual", + Commands.runOnce(() -> climb.setCurrentState(ClimbState.MANUAL)) + .alongWith( + Commands.run( + () -> + climb.setDefaultCommand( + Commands.run( + () -> { + climb.runClimb(getOpleftY.getAsDouble()); + }, + climb))))); + + // states that actually run the climber + if (climb != null) { + elevateState = + stateMachine.addState( + "elevate", + climb + .climberCommand(Meters.of(.5)) + .alongWith(Commands.runOnce(() -> climb.setCurrentState(ClimbState.ELEVATE)))); + descendState = + stateMachine.addState( + "lower", + climb + .climberCommand(Meters.of(0)) + .alongWith(Commands.runOnce(() -> climb.setCurrentState(ClimbState.DESCEND)))); + } else { + // fallback states if climb isn't available + elevateState = stateMachine.addState("elevate", Commands.idle()); + descendState = stateMachine.addState("lower", Commands.idle()); + } + + // Set DISABLED as the default initial state + stateMachine.setInitialState(disabledState); + + if (climb != null) { + stateMachine.addRequirements(climb); + climb.setDefaultCommand(stateMachine); + } + } + + public static Command shouldElevateCommand() { + return Commands.runOnce(() -> climb.setRequestedState(ClimbState.ELEVATE)); + } + + public static Command shouldStopCommand() { + return Commands.runOnce(() -> climb.setRequestedState(ClimbState.IDLE)); + } + + public static Command shouldDescendCommand() { + return Commands.runOnce(() -> climb.setRequestedState(ClimbState.DESCEND)); + } + + // New: command to enable the climb (requests IDLE) + public static Command shouldEnableCommand() { + return Commands.runOnce(() -> climb.setRequestedState(ClimbState.IDLE)); + } + + public void configureButtonBindings(Controller driver, Controller operator) { + if (null == climb) { + return; + } + + // Bind the "enable climb" button to transition DISABLED -> IDLE when pressed. + // Change createStartButton() to whatever button you prefer on your controller. + operator.createStartButton().onTrue(shouldEnableCommand()); + + // Driver POV-Up enables the climb (requests IDLE) + driver.createUpPovButton().onTrue(shouldEnableCommand()); + + // disabled -> idle when the enable command is requested + disabledState.switchTo(idleState).when(() -> climb.isRequested(ClimbState.IDLE)); + + configCommonStates(operator); + } + + public void configureAltButtonBindings(Controller driver, Controller operator) { + stateMachine.setInitialState(idleState); + + operator.createXButton().onTrue(shouldElevateCommand()).onFalse(shouldStopCommand()); + operator.createYButton().onTrue(shouldDescendCommand()).onFalse(shouldStopCommand()); + // lowered -> elevate when requested + + stateMachine.setInitialState(idleState); + + configCommonStates(operator); + } + + private void configCommonStates(Controller operator) { + + loweredState.switchTo(elevateState).when(() -> climb.isRequested(ClimbState.ELEVATE)); + // descend -> lowered when height is Zero + descendState.switchTo(loweredState).when(() -> climb.getHeight().isEquivalent(Meters.of(0))); + // elevate -> Lifted when height is =to Target + elevateState + .switchTo(liftedState) + .when(() -> climb.getHeight().isEquivalent(ClimbConstants.MAX)); + // lifted -> descend when asked to descend + liftedState.switchTo(descendState).when(() -> climb.isRequested(ClimbState.DESCEND)); + // elevate -> descend when asked to descend + elevateState.switchTo(descendState).when(() -> climb.isRequested(ClimbState.DESCEND)); + // descend -> elevating when asked to elevate + descendState.switchTo(elevateState).when(() -> climb.isRequested(ClimbState.ELEVATE)); + // elevate -> idle when stopped + elevateState.switchTo(idleState).when(() -> climb.isRequested(ClimbState.IDLE)); + // descend -> idle when stopped + descendState.switchTo(idleState).when(() -> climb.isRequested(ClimbState.IDLE)); + // idle -> elevate when requested + idleState.switchTo(elevateState).when(() -> climb.isRequested(ClimbState.ELEVATE)); + // idle -> descend when requested + idleState.switchTo(descendState).when(() -> climb.isRequested(ClimbState.DESCEND)); + // idle- > manual + idleState.switchTo(manualState).when(() -> operator.getLeftYAxis() != 0); + // manual to switch + manualState.switchTo(idleState).when(() -> operator.getLeftYAxis() == 0); + + getOpleftY = () -> operator.getLeftYAxis(); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/IndexerCommands.java b/src/main/java/frc/robot/rebuilt/commands/IndexerCommands.java new file mode 100644 index 00000000..7a53226f --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/IndexerCommands.java @@ -0,0 +1,183 @@ +package frc.robot.rebuilt.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.subsystems.Indexer.Indexer; +import frc.robot.rebuilt.subsystems.Launcher.Launcher; +import java.util.Map; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.config.ConfigConstants; +import org.frc5010.common.sensors.Controller; +import org.frc5010.common.subsystems.LEDStrip; + +public class IndexerCommands { + /** declares variables that will later hold state objects */ + private Map subsystems; + + private static Indexer indexer; + private static Launcher launcher; + + /** defines possible states of the indexer */ + public static enum IndexerState { + IDLE, + CHURN, + HARD_CHURN, + FORCE, + FEED + } + + /** Stores the subsystem map and retrieves the indexer instance */ + public IndexerCommands(Map systems) { + this.subsystems = systems; + IndexerCommands.indexer = (Indexer) subsystems.get(Constants.INDEXER); + IndexerCommands.launcher = (Launcher) subsystems.get(Constants.LAUNCHER); + configureTriggerStates(); + } + + public void configureButtonBindings(Controller driver, Controller operator) { + // driver.createLeftBumper().onTrue(toggleForceFeed()); + // driver.createLeftBumper().whileTrue(shouldForceCommand()).onFalse(shouldChurnCommand()); + driver.createLeftBumper().whileTrue(shouldHardChurnCommand()).onFalse(shouldChurnCommand()); + operator + .createLeftBumper() + .whileTrue( + Commands.either( + shouldForceCommand(), shouldChurnCommand(), () -> launcher.isOKToFire())) + .onFalse(shouldChurnCommand()); + } + + private void configureTriggerStates() { + // Map requested states to their commands and wire triggers in a compact loop. + // CHURN is handled separately below so it can be gated on flywheel readiness. + java.util.Map stateToCommand = + java.util.Map.of( + IndexerState.FEED, feedStateCommand(), + IndexerState.FORCE, forceStateCommand(), + IndexerState.IDLE, idleStateCommand(), + IndexerState.HARD_CHURN, hardChurnStateCommand()); + + stateToCommand.forEach( + (state, cmd) -> new Trigger(() -> indexer.isRequested(state)).onTrue(cmd)); + + // CHURN: the request can be set at any time, but the indexer only physically + // starts churning once LauncherCommands.isFlywheelReadyForChurn() is satisfied. + new Trigger(() -> indexer.isRequested(IndexerState.CHURN)).onTrue(churnStateCommand()); + } + + public void setupDefaultCommands() {} + + /** defines command behavio for the force state stops the indexer and runs the transfer at 50% */ + public static Command forceStateCommand() { + return Commands.runOnce( + () -> { + indexer.setCurrentState(IndexerState.FORCE); + indexer.runSpindexer(Constants.Indexer.SPINDEXER_SPEED); + indexer.runTransferFront(Constants.Indexer.TRANSFER_SPEED); + // indexer.runTransferBack(0.50); + }, + indexer); + } + /** defines command behavior for the churn state stops the indexer and runs the transfer at 25% */ + private static Command churnStateCommand() { + return Commands.runOnce( + () -> { + indexer.setCurrentState(IndexerState.CHURN); + indexer.runSpindexer(-0.1); + indexer.runTransferFront(Constants.Indexer.TRANSFER_CHURN); + }, + indexer) + .andThen(Commands.waitSeconds(1.0)) + .andThen( + Commands.runOnce( + () -> { + indexer.runSpindexer(0.0); + indexer.runTransferFront(0); + })); + } + + private static Command hardChurnStateCommand() { + return Commands.runOnce( + () -> { + indexer.setCurrentState(IndexerState.HARD_CHURN); + indexer.runSpindexer(-0.5); + indexer.runTransferFront(Constants.Indexer.TRANSFER_CHURN); + }, + indexer); + } + + /** + * defines command behavior for the idle state stops all motors and sets the LED patters to + * rainbow + */ + private static Command idleStateCommand() { + return Commands.runOnce( + () -> { + indexer.setCurrentState(IndexerState.IDLE); + indexer.runSpindexer(0); + indexer.runTransferFront(0); + // indexer.runTransferBack(0); + LEDStrip.changeSegmentPattern(ConfigConstants.ALL_LEDS, LEDStrip.getRainbowPattern(0)); + }, + indexer); + } + + // run feed command when Launcher State is idle and Operator Right Bumper is + // pressed + private static Command feedStateCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + indexer.setCurrentState(IndexerState.FEED); + indexer.runSpindexer(Constants.Indexer.SPINDEXER_SPEED); + indexer.runTransferFront(Constants.Indexer.TRANSFER_SPEED); + // indexer.runTransferBack(1); + LEDStrip.changeSegmentPattern( + ConfigConstants.ALL_LEDS, LEDStrip.getRainbowPattern(25)); + }, + indexer)); + } + /** Requests the indexer to enter the idle state */ + public static Command shouldIdleCommand() { + return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.IDLE)); + } + /** Requests the indexer to enter the churn state */ + public static Command shouldChurnCommand() { + return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.CHURN)); + } + + public static Command churnAuto() { + return Commands.run( + () -> { + indexer.runSpindexer(-0.1); + }); + } + + public static Command shouldHardChurnCommand() { + return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.HARD_CHURN)); + } + /** Requests the indexer to enter the feed state */ + public static Command shouldFeedCommand() { + return Commands.runOnce(() -> indexer.setRequestedState(IndexerState.FEED)); + } + + public static Command shouldForceCommand() { + return Commands.runOnce( + () -> { + indexer.setRequestedState(IndexerState.FORCE); + }); + } + + public static Command toggleForceFeed() { + return Commands.runOnce( + () -> { + if (indexer.isRequested(IndexerState.FEED)) { + indexer.setRequestedState(IndexerState.CHURN); + + } else { + indexer.setRequestedState(IndexerState.FEED); + } + }); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/IntakeCommands.java b/src/main/java/frc/robot/rebuilt/commands/IntakeCommands.java new file mode 100644 index 00000000..dd15fc63 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/IntakeCommands.java @@ -0,0 +1,317 @@ +package frc.robot.rebuilt.commands; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.subsystems.intake.Intake; +import java.util.Map; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.arch.StateMachine; +import org.frc5010.common.sensors.Controller; + +public class IntakeCommands { + static Intake intake; + /** Tracks whether the hopper has been zeroed at least once this match. */ + static boolean hopperZeroed = false; + + Map subsystems; + StateMachine intakeStateMachine = new StateMachine("IntakeStateMachine"); + + DoubleSupplier intakeSpeedSupplier = + () -> Constants.Intake.INTAKE_IN; // Default speed, can be overridden by triggers + Supplier intakeSpeed = () -> intakeSpeedSupplier; + + public static enum IntakeState { + UNKNOWN, + RETRACTED, + RETRACTING, + DEPLOYING, + INTAKING, + DEPLOYED, + ANGLED; + } + + public IntakeCommands(Map subsystems) { + this.subsystems = subsystems; + + intake = (Intake) subsystems.get(Constants.INTAKE); + + setupTriggerStates(); + } + + public void setupDefaultCommands() {} + + private void setupTriggerStates() { + // Map requested states to their commands and wire triggers in a compact loop. + // CHURN is handled separately below so it can be gated on flywheel readiness. + java.util.Map stateToCommand = + java.util.Map.of( + IntakeState.UNKNOWN, unknownStateCommand(), + IntakeState.RETRACTED, retractedCommand(), + IntakeState.DEPLOYED, deployedCommand()); + + stateToCommand.forEach( + (state, cmd) -> new Trigger(() -> intake.isRequested(state)).onTrue(cmd)); + + // Treat the hard stop as low movement plus stall current, not current alone. + Trigger hopperHardStopped = + new Trigger(() -> intake.isHopperHardStopDetected()) + .debounce(Constants.Intake.HOPPER_STALL_TIME); + + /** Trigger the deploying command */ + new Trigger( + () -> + intake.isRequested(IntakeState.INTAKING) && !intake.isCurrent(IntakeState.INTAKING)) + .onTrue(deployingCommand().until(() -> intake.isCurrent(IntakeState.INTAKING))); + + /** Trigger the intaking command */ + new Trigger( + () -> + intake.isRequested(IntakeState.INTAKING) + && intake.isCurrent(IntakeState.DEPLOYING) + && (intake.isDeployed() + || (isHopperZeroed() + && intake + .getHopperAngle() + .lt( + Degrees.of( + Constants.Intake.HOPPER_DEPLOY_STOP_REZERO_MAX_ANGLE)) + && hopperHardStopped.getAsBoolean()))) + .onTrue(intakingCommand(intakeSpeed)); + + /** Trigger the retracting command */ + new Trigger( + () -> + intake.isRequested(IntakeState.RETRACTING) + && !intake.isCurrent(IntakeState.RETRACTED)) + .onTrue(retractingCommand().until(() -> intake.isCurrent(IntakeState.RETRACTED))); + + /** Trigger the retracted command */ + new Trigger(() -> intake.isCurrent(IntakeState.RETRACTING) && (intake.isRetracted())) + .onTrue(shouldRetracted()); + + /** Trigger the angled command */ + new Trigger( + () -> intake.isRequested(IntakeState.ANGLED) && !intake.isCurrent(IntakeState.ANGLED)) + .onTrue(angledCommand()); + } + + public void configureButtonBindings(Controller controller, Controller operator) { + controller.setRightTrigger( + controller.createRightTrigger().limit(Constants.Intake.INTAKE_MAX_IN)); + Trigger rightTrigger = + new Trigger(() -> controller.getRightTrigger() > Constants.Intake.INTAKE_DEADZONE); + controller.setLeftTrigger( + controller + .createLeftTrigger() + .limit(Constants.Intake.INTAKE_MAX_IN)); // Axis are positive only hence IN + Trigger leftTrigger = + new Trigger(() -> controller.getLeftTrigger() > Constants.Intake.INTAKE_DEADZONE); + + rightTrigger.onTrue(shouldIntaking()); + leftTrigger.onTrue(shouldIntaking()); + + controller.createRightBumper().onTrue(shouldRetracting()); + controller.createStartButton().onTrue(Commands.run(() -> intake.setHopperRetracted())); + controller.createBackButton().onTrue(Commands.run(() -> intake.setHopperDeployed())); + + operator.createDownPovButton().onTrue(operatorHopperDownCommand()); + controller.createXButton().onTrue(operatorHopperDownCommand()); + operator.createRightBumper().onTrue(shouldAngled()).onFalse(shouldIntaking()); + + intakeSpeedSupplier = + () -> { + double rightTriggerSpeed = controller.getRightTrigger(); + double leftTriggerSpeed = controller.getLeftTrigger(); + double speed = Constants.Intake.INTAKE_IN; // Default speed if neither trigger is pressed + if (rightTriggerSpeed > Constants.Intake.INTAKE_DEADZONE + || leftTriggerSpeed > Constants.Intake.INTAKE_DEADZONE) { + speed = + rightTriggerSpeed + - leftTriggerSpeed; // Positive for intaking, negative for outtaking + } + if (RobotState.isAutonomous()) { + speed = Constants.Intake.INTAKE_AUTO; // Intake in speed in auto + } + return speed; + }; + } + + public static Command intakingCommand(Supplier speed) { + return Commands.runOnce( + () -> { + hopperZeroed = isHopperZeroed(); + intake.setCurrentState(IntakeState.INTAKING); + intake.setHopperZeroed(hopperZeroed); + }, + intake) + .andThen( + Commands.run( + () -> { + double runSpeed = speed.get().getAsDouble(); + + if (!isHopperZeroed() && intake.isHopperHardStopDetected()) { + markHopperZeroed(); + } + + if (!isHopperZeroed()) { + // First deploy homes open-loop until it sees the deployed hard stop. + intake.runHopper(Constants.Intake.HOPPER_FIRST_DEPLOY_DUTY); + } else if (intake.getHopperAngle().gt(Degrees.of(5)) + // || runSpeed > Constants.Intake.INTAKE_IN + || RobotState.isAutonomous()) { + // Still settling or forcing — duty cycle nudge + intake.runHopper(Constants.Intake.HOPPER_FIRST_DEPLOY_DUTY); + } else { + // Zeroed and at position — PID hold at 0° instead of constant duty cycle + intake.setDesiredHopperAngle(Constants.Intake.HOPPER_DEPLOYED_ANGLE); + } + intake.runSpintake(runSpeed); + })); + } + + public static Command waitUntilIntaking() { + return Commands.idle().until(() -> intake.isCurrent(IntakeState.INTAKING)); + } + + public static Command deployingCommand() { + return Commands.runOnce( + () -> { + intake.setCurrentState(IntakeState.DEPLOYING); + }, + intake) + .andThen( + Commands.either( + homeUnzeroedHopperCommand(), deployZeroedHopperCommand(), () -> !isHopperZeroed())) + .alongWith( + Commands.run( + () -> { + if (isHopperZeroed() && intake.getHopperAngle().lt(Degrees.of(60))) { + intake.runSpintake(Constants.Intake.INTAKE_IN); + } + })); + } + + public static Command deployedCommand() { + return Commands.runOnce( + () -> { + intake.setCurrentState(IntakeState.DEPLOYED); + intake.runHopper(0); + }, + intake); + } + + public static Command angledCommand() { + return Commands.runOnce( + () -> { + intake.setCurrentState(IntakeState.ANGLED); + }, + intake) + .andThen( + intake + .setDesiredHopperAngle(Constants.Intake.HOPPER_ANGLED) + .until(() -> intake.isHopperMoving()) + .andThen( + Commands.run( + () -> + intake.runSpintakes( + Constants.Intake.INTAKE_IN * 0.5, Constants.Intake.INTAKE_CHURN)))); + } + + public static Command retractingCommand() { + return Commands.runOnce( + () -> { + intake.setCurrentState(IntakeState.RETRACTING); + }, + intake) + .andThen(() -> intake.runSpintake(0), intake) + .andThen( + intake + .setDesiredHopperAngle(Constants.Intake.HOPPER_RETRACTED_ANGLE) + .until(() -> intake.isHopperMoving())); + } + + public static Command retractedCommand() { + return Commands.runOnce(() -> intake.setCurrentState(IntakeState.RETRACTED), intake) + .andThen(() -> intake.runSpintake(0), intake) + .andThen(() -> intake.runHopper(0), intake); + } + + public static Command unknownStateCommand() { + return Commands.runOnce( + () -> { + intake.setCurrentState(IntakeState.UNKNOWN); + }, + intake) + .andThen(Commands.runOnce(() -> intake.runHopper(0), intake)) + .andThen(Commands.runOnce(() -> intake.runSpintake(0), intake)); + } + + public Command operatorHopperDownCommand() { + return homeUnzeroedHopperCommand().andThen(intakingCommand(intakeSpeed)); + } + + private static Command deployZeroedHopperCommand() { + Trigger hopperHardStopped = + new Trigger(() -> intake.isHopperHardStopDetected()) + .debounce(Constants.Intake.HOPPER_STALL_TIME); + + return intake + .setDesiredHopperAngle(Constants.Intake.HOPPER_DEPLOYED_ANGLE) + .until(() -> intake.isHopperAtPosition(Constants.Intake.HOPPER_DEPLOYED_ANGLE)) + .andThen( + Commands.run(() -> intake.runHopper(Constants.Intake.HOPPER_DEPLOY_NUDGE_DUTY), intake) + .until(hopperHardStopped::getAsBoolean) + .withTimeout(1.5)) + .andThen(Commands.runOnce(() -> intake.runHopper(0))); + } + + private static Command homeUnzeroedHopperCommand() { + Trigger hopperHardStopped = + new Trigger(() -> intake.isHopperHardStopDetected()) + .debounce(Constants.Intake.HOPPER_STALL_TIME); + + return Commands.run(() -> intake.runHopper(Constants.Intake.HOPPER_FIRST_DEPLOY_DUTY), intake) + .until(hopperHardStopped::getAsBoolean) + .andThen( + Commands.runOnce( + () -> { + intake.runHopper(0); + if (hopperHardStopped.getAsBoolean()) { + markHopperZeroed(); + } + })); + } + + private static void markHopperZeroed() { + intake.zeroHopper(); + hopperZeroed = true; + intake.setHopperZeroed(true); + } + + private static boolean isHopperZeroed() { + return hopperZeroed || intake.isHopperZeroed(); + } + + public static Command shouldIntaking() { + return Commands.runOnce(() -> intake.setRequestedState(IntakeState.INTAKING)); + } + + public static Command shouldRetracting() { + return Commands.runOnce(() -> intake.setRequestedState(IntakeState.RETRACTING)); + } + + public static Command shouldRetracted() { + return Commands.runOnce(() -> intake.setRequestedState(IntakeState.RETRACTED)); + } + + public static Command shouldAngled() { + return Commands.runOnce(() -> intake.setRequestedState(IntakeState.ANGLED)); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/LauncherCommands.java b/src/main/java/frc/robot/rebuilt/commands/LauncherCommands.java new file mode 100644 index 00000000..7810f3c2 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/LauncherCommands.java @@ -0,0 +1,491 @@ +package frc.robot.rebuilt.commands; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Seconds; + +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.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.FieldConstants; +import frc.robot.rebuilt.commands.IntakeCommands.IntakeState; +import frc.robot.rebuilt.subsystems.Launcher.Launcher; +import frc.robot.rebuilt.subsystems.Launcher.ShotCalculator; +import frc.robot.rebuilt.subsystems.Launcher.ShotCalculator.ShootingParameters; +import frc.robot.rebuilt.subsystems.intake.Intake; +import java.util.Map; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.arch.StateMachine; +import org.frc5010.common.arch.StateMachine.State; +import org.frc5010.common.config.ConfigConstants; +import org.frc5010.common.drive.GenericDrivetrain; +import org.frc5010.common.sensors.Controller; +import org.frc5010.common.subsystems.LEDStrip; +import org.frc5010.common.utils.geometry.AllianceFlipUtil; +import org.frc5010.common.vision.AprilTags; + +/** defines commands and state launcher logic for the launcher */ +public class LauncherCommands { + + private StateMachine stateMachine; + private State idleState; + private State lowState; + private State prepState; + private State presetState; + private State hammerTimeState; + private State autoHammerTimeState; + private State escapeHammerTimeState; + private static Launcher launcher; + private static Intake intake; + private static GenericDrivetrain drivetrain; + private Map subsystems; + private static Translation2d hubTarget = FieldConstants.Hub.topCenterPoint.toTranslation2d(); + private static Translation2d allianceSideLeft = FieldConstants.Tower.leftUpright; + private static Translation2d allianceSideRight = FieldConstants.Tower.rightUpright; + private static Translation2d intakeToCenterTranslation = + new Translation2d(Inches.of(25), Inches.of(0)); + private static Transform2d intakeToCenter = + new Transform2d(intakeToCenterTranslation, Rotation2d.fromDegrees(180)); + private static Translation2d rearToCenterTranslation = + new Translation2d(Inches.of(13.5), Inches.of(0)); + private static Transform2d rearToCenter = + new Transform2d(rearToCenterTranslation, Rotation2d.fromDegrees(0)); + + // Stored preset targets — written once when a preset command is activated + private static Angle presetHoodAngle = Constants.Launcher.LOW_HOOD_ANGLE; + private static Angle presetTurretAngle = Constants.Launcher.TURRET_FORWARD; + private static AngularVelocity presetFlywheelSpeed = RPM.of(0); + + /** + * When true, the indexer will only churn once the flywheel has reached its goal speed. Set to + * false to allow churning at any time regardless of flywheel speed. + */ + public static boolean requireFlywheelAtGoalForChurn = true; + + public static Translation2d getRobotToTarget(Translation2d target) { + return target.minus(drivetrain.getPoseEstimator().getCurrentPose().getTranslation()); + } + // public static Angle getHoodAngle(Distance toTarget) {} Placeholder for now + /** declares possible states for the launcher */ + public static enum LauncherState { + IDLE, + LOW_SPEED, + PREP, + HAMMERTIME, + AUTO_HAMMERTIME, + ESCAPE_HAMMERTIME, + PRESET; + + @Override + public String toString() { + return this.name(); + } + } + /** initializes the launcher state machine and adds states */ + public LauncherCommands(Map subsystems) { + this.subsystems = subsystems; + launcher = (Launcher) subsystems.get(Constants.LAUNCHER); + intake = (Intake) subsystems.get(Constants.INTAKE); + launcher.setCurrentState(LauncherState.HAMMERTIME); + launcher.setRequestedState(LauncherState.HAMMERTIME); + + drivetrain = (GenericDrivetrain) this.subsystems.get(ConfigConstants.DRIVETRAIN); + configureStateMachine(); + } + /** sets the state machine as the default command of the launcher */ + public void setDefaultCommands() { + if (launcher != null) { + stateMachine.addRequirements(launcher); + launcher.setDefaultCommand(stateMachine); + } + } + + public void configureStateMachine() { + stateMachine = new StateMachine("LauncherStateMachine"); + presetState = stateMachine.addState("PRESET-SHOOT", presetStateCommand()); + idleState = stateMachine.addState("IDLE", idleStateCommand()); + lowState = stateMachine.addState("LOW-SPEED", lowStateCommand()); + prepState = stateMachine.addState("PREP-SHOOT", prepStateCommand()); + hammerTimeState = stateMachine.addState("HAMMER-TIME", hammerTimeStateCommand()); + autoHammerTimeState = stateMachine.addState("AUTO-HAMMER-TIME", autoHammerTimeStateCommand()); + escapeHammerTimeState = + stateMachine.addState("ESCAPE-HAMMER-TIME", escapeHammerTimeStateCommand()); + stateMachine.setInitialState(hammerTimeState); + idleState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED)); + idleState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP)); + idleState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET)); + idleState.switchTo(hammerTimeState).when(() -> launcher.isRequested(LauncherState.HAMMERTIME)); + idleState + .switchTo(autoHammerTimeState) + .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME)); + + lowState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE)); + lowState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP)); + lowState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET)); + lowState.switchTo(hammerTimeState).when(() -> launcher.isRequested(LauncherState.HAMMERTIME)); + lowState + .switchTo(autoHammerTimeState) + .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME)); + + prepState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED)); + prepState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE)); + prepState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET)); + prepState.switchTo(hammerTimeState).when(() -> launcher.isRequested(LauncherState.HAMMERTIME)); + prepState + .switchTo(autoHammerTimeState) + .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME)); + + presetState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE)); + presetState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED)); + presetState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP)); + presetState + .switchTo(hammerTimeState) + .when(() -> launcher.isRequested(LauncherState.HAMMERTIME)); + presetState + .switchTo(autoHammerTimeState) + .when(() -> launcher.isRequested(LauncherState.AUTO_HAMMERTIME)); + + autoHammerTimeState + .switchTo(escapeHammerTimeState) + .when(() -> launcher.isRequested(LauncherState.ESCAPE_HAMMERTIME)); + + escapeHammerTimeState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE)); + escapeHammerTimeState + .switchTo(lowState) + .when(() -> launcher.isRequested(LauncherState.LOW_SPEED)); + escapeHammerTimeState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP)); + + // Hammer Time is a special case since it's a toggle state + hammerTimeState.switchTo(idleState).when(() -> launcher.isRequested(LauncherState.IDLE)); + hammerTimeState.switchTo(lowState).when(() -> launcher.isRequested(LauncherState.LOW_SPEED)); + hammerTimeState.switchTo(prepState).when(() -> launcher.isRequested(LauncherState.PREP)); + hammerTimeState.switchTo(presetState).when(() -> launcher.isRequested(LauncherState.PRESET)); + + Trigger readyToFireTrigger = + new Trigger(() -> launcher.isCurrent(LauncherState.PREP) && launcher.isAtGoal()); + readyToFireTrigger.onTrue(IndexerCommands.shouldFeedCommand()); + Trigger churnWhileFiring = + new Trigger(() -> launcher.isCurrent(LauncherState.PREP) && !launcher.isAtGoal()); + churnWhileFiring.onTrue(IndexerCommands.shouldChurnCommand()); + // Only idle the indexer when transitioning away from a PREP sub-state (ready-to-fire or + // churning). Without the inPrep guard, both negated triggers are true in ALL non-PREP states, + // causing this trigger to fire on every state change and race against intentional indexer + // state commands from other parts of the code. + Trigger inPrep = new Trigger(() -> launcher.isCurrent(LauncherState.PREP)); + inPrep + .and(readyToFireTrigger.negate()) + .and(churnWhileFiring.negate()) + .onTrue(IndexerCommands.shouldIdleCommand()); + } + + /** + * Returns true when it is safe to begin churning the indexer. When {@code + * requireFlywheelAtGoalForChurn} is {@code true} (default), churning is only permitted once the + * flywheel has reached its goal speed. Set the flag to {@code false} to allow churning at any + * time regardless of flywheel speed. + */ + public static boolean isFlywheelReadyForChurn() { + return !requireFlywheelAtGoalForChurn + || (launcher != null && launcher.isFlywheelAtOrAboveGoal()); + } + + public void configureButtonBindings(Controller driver, Controller operator) { + + // driver.createAButton().onTrue(shouldPrepCommand()); + driver.createBButton().whileTrue(shouldPrepCommand()).onFalse(shouldLowCommand()); + + driver.createAButton().onTrue(shouldLowCommand()).onFalse(shouldLowCommand()); + + operator + .createLeftPovButton() + .onTrue(Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(-0.01))); + operator + .createRightPovButton() + .onTrue(Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(0.01))); + + operator.createAButton().whileTrue(towerPresetStateCommand()).onFalse(shouldLowCommand()); + + operator + .createBButton() + .whileTrue(rightCornerPresetStateCommandr()) + .onFalse(shouldLowCommand()); + + operator.createXButton().whileTrue(leftCornerPresetStateCommand()).onFalse(shouldLowCommand()); + operator + .createYButton() + .whileTrue(turretForwardPresetStateCommand()) + .onFalse(shouldLowCommand()); + + operator.createBackButton().whileTrue(zeroHoodSequence()); + + // This allowed auto-hammer time + // Trigger isTrenchTrigger = new Trigger(() -> launcher.isNearTrench()); + // isTrenchTrigger.onTrue(shouldAutoHammerTimeCommand()).onFalse(shouldEscapeHammerTimeCommand()); + + // When intake is retracting or retracted, force turret to HAMMERTIME (hopper arm interferes) + Trigger intakeUpTrigger = + new Trigger( + () -> + intake.isCurrent(IntakeState.RETRACTING) + || intake.isCurrent(IntakeState.RETRACTED)); + intakeUpTrigger.onTrue(shouldHammerTimeCommand()); + + // When intake deploys away from turret, return to LOW_SPEED + Trigger intakeDeployingTrigger = + new Trigger( + () -> + ((intake.isCurrent(IntakeState.DEPLOYING) + && intake + .getHopperAngle() + .lt( + Constants.Intake.HOPPER_RETRACTED_ANGLE.minus( + Constants.Launcher.HOPPER_EXTENSION_BUFFER_BEFORE_AIM))) + || intake.isCurrent(IntakeState.INTAKING)) + && launcher.isCurrent(LauncherState.HAMMERTIME)); + intakeDeployingTrigger.onTrue(shouldLowCommand()); + + operator + .createUpPovButton() + .onTrue( + Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(0.01)) + .ignoringDisable(true)); + operator + .createDownPovButton() + .onTrue( + Commands.runOnce(() -> ShotCalculator.incrementFlywheelMultiplier(-0.01)) + .ignoringDisable(true)); + } + + /** creates command behavior for the IDLE launcher state */ + private static Command idleStateCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + launcher.setCurrentState(LauncherState.IDLE); + }), + launcher.stopTrackingCommand()); + } + /** creates command behavior for when the launcher is at low speed */ + private static Command lowStateCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + launcher.setCurrentState(LauncherState.LOW_SPEED); + LEDStrip.changeSegmentPattern( + ConfigConstants.ALL_LEDS, LEDStrip.getSolidPattern(Color.kGreen)); + }), + launcher.trackTargetLowCommand()); + } + /** creates command behavior when the launcher is at prep state */ + private static Command prepStateCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + launcher.setCurrentState(LauncherState.PREP); + LEDStrip.changeSegmentPattern( + ConfigConstants.ALL_LEDS, LEDStrip.getRainbowPattern(0)); + }), + launcher.trackTargetCommand()); + } + /** creates command behavior for when the launcher is at preset */ + private static Command presetStateCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + launcher.setCurrentState(LauncherState.PRESET); + }), + Commands.run( + () -> launcher.usePresets(presetHoodAngle, presetTurretAngle, presetFlywheelSpeed))); + } + + public static Command shouldIdleCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.IDLE)); + } + + public static Command shouldLowCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.LOW_SPEED)); + } + + public static Command shouldPrepCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.PREP)); + } + + public static Command shouldPresetCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.PRESET)); + } + + public static Command shouldAutoHammerTimeCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.AUTO_HAMMERTIME)); + } + + public static Command shouldEscapeHammerTimeCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.ESCAPE_HAMMERTIME)); + } + + public static Command shouldToggleHammerTimeCommand() { + return Commands.runOnce( + () -> { + if (launcher.getCurrentState() == LauncherState.HAMMERTIME) { + launcher.setRequestedState(LauncherState.LOW_SPEED); + } else { + launcher.setRequestedState(LauncherState.HAMMERTIME); + } + }); + } + + public static Command shouldHammerTimeCommand() { + return Commands.runOnce(() -> launcher.setRequestedState(LauncherState.HAMMERTIME)); + } + + // Order is Hood Angle, Turret Angle, Flywheel Speed + // Values are placeholders and need to be tuned + public static Command leftCornerPresetStateCommand() { + return shouldPresetCommand() + .andThen( + Commands.runOnce( + () -> { + ShootingParameters params = + launcher.getShootingParameters( + () -> + AllianceFlipUtil.apply( + new Pose2d( + new Translation2d( + Inches.of( + AprilTags.aprilTagFieldLayout + .getTagPose(31) + .get() + .getX() + + 25), + FieldConstants.FIELD_WIDTH.minus(Inches.of(17.25))), + new Rotation2d())), + () -> FieldConstants.Hub.topCenterPoint.toTranslation2d()); + presetHoodAngle = Radians.of(params.hoodAngle()); + presetTurretAngle = params.turretAngle().getMeasure(); + presetFlywheelSpeed = + RPM.of(params.flywheelSpeed() * ShotCalculator.getFlywheelMultiplier()); + })); + } + + public static Command rightCornerPresetStateCommandr() { + return shouldPresetCommand() + .andThen( + Commands.runOnce( + () -> { + ShootingParameters params = + launcher.getShootingParameters( + () -> + AllianceFlipUtil.apply( + new Pose2d( + new Translation2d( + Inches.of( + AprilTags.aprilTagFieldLayout + .getTagPose(31) + .get() + .getX() + + 25), + Inches.of(17.5)), + new Rotation2d())), + () -> FieldConstants.Hub.topCenterPoint.toTranslation2d()); + presetHoodAngle = Radians.of(params.hoodAngle()); + presetTurretAngle = params.turretAngle().getMeasure(); + presetFlywheelSpeed = + RPM.of(params.flywheelSpeed() * ShotCalculator.getFlywheelMultiplier()); + })); + } + + public static Command towerPresetStateCommand() { + return shouldPresetCommand() + .andThen( + Commands.runOnce( + () -> { + ShootingParameters params = + launcher.getShootingParameters( + () -> + AllianceFlipUtil.apply(FieldConstants.Tower.face.plus(rearToCenter)), + () -> FieldConstants.Hub.topCenterPoint.toTranslation2d()); + presetHoodAngle = Radians.of(params.hoodAngle()); + presetTurretAngle = Constants.Launcher.TURRET_FORWARD; + presetFlywheelSpeed = + RPM.of(params.flywheelSpeed() * ShotCalculator.flywheelMultiplier); + })); + } + + public static Command turretForwardPresetStateCommand() { + return shouldPresetCommand() + .andThen( + Commands.runOnce( + () -> { + presetHoodAngle = Constants.Launcher.FWD_HOOD_ANGLE; + presetTurretAngle = Constants.Launcher.TURRET_FORWARD; + presetFlywheelSpeed = Constants.Launcher.FWD_FLYWHEEL_RPM; + })); + } + + public static Command hammerTimeStateCommand() { + return Commands.parallel( + Commands.run( + () -> { + launcher.setCurrentState(LauncherState.HAMMERTIME); + launcher.usePresets( + Constants.Launcher.LOW_HOOD_ANGLE, + Degrees.of(0), + Constants.Launcher.LOW_FLYWHEEL_RPM); + })); + } + + public static Command autoHammerTimeStateCommand() { + return Commands.parallel( + Commands.run( + () -> { + launcher.setCurrentState(LauncherState.AUTO_HAMMERTIME); + launcher.usePresets( + Constants.Launcher.LOW_HOOD_ANGLE, + Degrees.of(0), + Constants.Launcher.LOW_FLYWHEEL_RPM); + })); + } + + public static Command escapeHammerTimeStateCommand() { + return Commands.runOnce(() -> launcher.setCurrentState(LauncherState.ESCAPE_HAMMERTIME)) + .andThen( + Commands.runOnce( + () -> { + launcher.setRequestedState(launcher.getPreTrenchState()); + })); + } + + public static LauncherState getCurrentState() { + return launcher.getCurrentState(); + } + + public static Command zeroHoodSequence() { + return Commands.run(() -> launcher.runHoodDown()) + .withTimeout(Seconds.of(0.75)) + .andThen(Commands.runOnce(() -> launcher.stopHood())) + .andThen(Commands.runOnce(() -> launcher.zeroHood())) + .andThen( + Commands.run( + () -> { + org.frc5010.common.subsystems.LEDStrip.changeSegmentPattern( + org.frc5010.common.config.ConfigConstants.ALL_LEDS, + org.frc5010.common.subsystems.LEDStrip.getRainbowPattern(50.0)); + }) + .withTimeout(0.5) + .ignoringDisable(true)) + .beforeStarting(() -> frc.robot.rebuilt.Rebuilt.isZeroingBurst = true) + .finallyDo( + () -> { + frc.robot.rebuilt.Rebuilt.isZeroingBurst = false; + }); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/NamedCommandsReg.java b/src/main/java/frc/robot/rebuilt/commands/NamedCommandsReg.java new file mode 100644 index 00000000..92d644d6 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/NamedCommandsReg.java @@ -0,0 +1,34 @@ +package frc.robot.rebuilt.commands; + +import com.pathplanner.lib.auto.NamedCommands; + +public class NamedCommandsReg { + + public static void createNamedCommands() { + // Launcer + NamedCommands.registerCommand("launcherPrep", LauncherCommands.shouldPrepCommand()); + NamedCommands.registerCommand("launcherPreset", LauncherCommands.shouldPresetCommand()); + NamedCommands.registerCommand("launcherLow", LauncherCommands.shouldLowCommand()); + NamedCommands.registerCommand("launcherIdle", LauncherCommands.shouldIdleCommand()); + // intake + NamedCommands.registerCommand("intakeIntake", IntakeCommands.shouldIntaking()); + NamedCommands.registerCommand("intakeRetracted", IntakeCommands.shouldRetracted()); + NamedCommands.registerCommand("intakeRetracting", IntakeCommands.shouldRetracting()); + // climb + NamedCommands.registerCommand("climbDescend", ClimbCommands.shouldDescendCommand()); + NamedCommands.registerCommand("climbElevate", ClimbCommands.shouldElevateCommand()); + NamedCommands.registerCommand("climbEnable", ClimbCommands.shouldEnableCommand()); + NamedCommands.registerCommand("climbStop", ClimbCommands.shouldStopCommand()); + // indexer + NamedCommands.registerCommand("indexerChurn", IndexerCommands.shouldChurnCommand()); + NamedCommands.registerCommand("indexerIdle", IndexerCommands.shouldIdleCommand()); + NamedCommands.registerCommand("indexerFeed", IndexerCommands.shouldFeedCommand()); + // preset + NamedCommands.registerCommand("iForcePreset", IndexerCommands.shouldForceCommand()); + NamedCommands.registerCommand("hubPreset", LauncherCommands.leftCornerPresetStateCommand()); + NamedCommands.registerCommand("towerPreset", LauncherCommands.towerPresetStateCommand()); + NamedCommands.registerCommand( + "towerForwardPreset", LauncherCommands.turretForwardPresetStateCommand()); + NamedCommands.registerCommand("WaitUntilIntaking", IntakeCommands.waitUntilIntaking()); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/ShotCalibrationCommand.java b/src/main/java/frc/robot/rebuilt/commands/ShotCalibrationCommand.java new file mode 100644 index 00000000..7fdfdcd1 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/ShotCalibrationCommand.java @@ -0,0 +1,292 @@ +package frc.robot.rebuilt.commands; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; + +import edu.wpi.first.math.controller.ProfiledPIDController; +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.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.FieldConstants; +import frc.robot.rebuilt.subsystems.Launcher.Launcher; +import frc.robot.rebuilt.subsystems.Launcher.ShotCalculator; +import org.frc5010.common.drive.GenericDrivetrain; +import org.frc5010.common.drive.swerve.GenericSwerveDrivetrain; +import org.frc5010.common.utils.geometry.AllianceFlipUtil; + +/** + * A streamlined command to rapidly calibrate the shooter at various distances. + * + *

State Machine: + * + *

    + *
  1. ALIGN_AND_DRIVE: Automatically drives the robot to face the hub at + * `currentDistance`. + *
  2. TUNE_AND_FIRE: Populates dashboard with an initial guess, allows operator to tune + * RPM/Hood without bizarre scaling, and allows firing. + *
  3. NEXT_DISTANCE: Acknowledges operator confirmation, logs the tuned point, and backs + * up by an increment. + *
+ */ +public class ShotCalibrationCommand extends Command { + + private enum CalibrationState { + ALIGN_AND_DRIVE, + TUNE_AND_FIRE, + NEXT_DISTANCE + } + + private static final String PREFIX = "ShotCal/"; + + private final Launcher launcher; + private final GenericSwerveDrivetrain drivetrain; + private final ShotCalculator shotCalculator; + + private CalibrationState currentState = CalibrationState.ALIGN_AND_DRIVE; + + // Configuration + private double currentDistanceMeters; + private final double distanceStepMeters; + + // Controllers for physical robot alignment + private final ProfiledPIDController xController; + private final ProfiledPIDController yController; + private final ProfiledPIDController thetaController; + + // Target coordinates + private Translation2d hubTarget; + private Pose2d targetPoseForDistance; + + // Tuning state block + private boolean initialGuessPopulated = false; + + public ShotCalibrationCommand( + Launcher launcher, + GenericDrivetrain drivetrain, + double initialDistance, + double distanceStep) { + this.launcher = launcher; + this.drivetrain = (GenericSwerveDrivetrain) drivetrain; + this.shotCalculator = ShotCalculator.getInstance(); + + this.currentDistanceMeters = initialDistance; + this.distanceStepMeters = distanceStep; + + // Initialize alignment controllers based on standard DriveToPosition constants + xController = new ProfiledPIDController(2.0, 0, 0, new TrapezoidProfile.Constraints(0.2, 0.5)); + yController = new ProfiledPIDController(2.0, 0, 0, new TrapezoidProfile.Constraints(0.2, 0.5)); + thetaController = + new ProfiledPIDController(3.0, 0, 0, new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + + xController.setTolerance(0.05); // 5cm + yController.setTolerance(0.05); // 5cm + thetaController.setTolerance(0.035); // ~2 degrees + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + addRequirements(launcher, this.drivetrain); + } + + @Override + public void initialize() { + currentState = CalibrationState.ALIGN_AND_DRIVE; + initialGuessPopulated = false; + + // Determine the hub position + hubTarget = AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()); + + // Setup dashboard fields + SmartDashboard.putNumber(PREFIX + "Distance Step (m)", distanceStepMeters); + SmartDashboard.putNumber( + PREFIX + "Test Hood Angle", Constants.Launcher.offsetLegacyHoodAngleDegrees(35.0)); + SmartDashboard.putNumber(PREFIX + "Test Flywheel RPM", 1800.0); + SmartDashboard.putNumber(PREFIX + "Flywheel Multiplier", 1.0); + SmartDashboard.putBoolean(PREFIX + "Force Firing", false); + SmartDashboard.putBoolean(PREFIX + "Confirm & Next", false); + SmartDashboard.putBoolean(PREFIX + "Apply Guess", false); + + System.out.println("[ShotCalibration] Starting interactive calibration."); + } + + @Override + public void execute() { + SmartDashboard.putString(PREFIX + "State", currentState.name()); + SmartDashboard.putNumber(PREFIX + "Current Target Dist", currentDistanceMeters); + SmartDashboard.putNumber(PREFIX + "Actual Target Dist", getActualDistance()); + + switch (currentState) { + case ALIGN_AND_DRIVE: + handleAlignAndDrive(); + break; + + case TUNE_AND_FIRE: + handleTuneAndFire(); + break; + + case NEXT_DISTANCE: + handleNextDistance(); + break; + } + } + + private void handleAlignAndDrive() { + Pose2d currentPose = drivetrain.getPoseEstimator().getCurrentPose(); + + // We want to be `currentDistanceMeters` away from the hub. + // The easiest generic way to do this is to stand on a line drawn from the hub through our + // current position. + Translation2d hubToRobot = currentPose.getTranslation().minus(hubTarget); + Rotation2d angleFromHub = hubToRobot.getAngle(); + + // The target translation is the hub, plus a vector pointing towards the robot with length = + // currentDistance + Translation2d targetTranslation = hubTarget.plus(new Translation2d(currentDistanceMeters, 0.0)); + + // We want the robot to face the hub. (Angle from hub + 180 deg) + Rotation2d targetRotation = angleFromHub.plus(Rotation2d.fromDegrees(180)); + targetPoseForDistance = new Pose2d(targetTranslation, targetRotation); + + // Drive using controllers + double xSpeed = xController.calculate(currentPose.getX(), targetPoseForDistance.getX()); + double ySpeed = yController.calculate(currentPose.getY(), targetPoseForDistance.getY()); + double thetaSpeed = + thetaController.calculate( + currentPose.getRotation().getRadians(), + targetPoseForDistance.getRotation().getRadians()); + + ChassisSpeeds speeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, thetaSpeed, currentPose.getRotation()); + drivetrain.drive(speeds); + + SmartDashboard.putNumber(PREFIX + "X Error", xController.getPositionError()); + SmartDashboard.putNumber(PREFIX + "Y Error", yController.getPositionError()); + SmartDashboard.putNumber(PREFIX + "Theta Error", thetaController.getPositionError()); + SmartDashboard.putBoolean(PREFIX + "X At Goal", xController.atGoal()); + SmartDashboard.putBoolean(PREFIX + "Y At Goal", yController.atGoal()); + SmartDashboard.putBoolean(PREFIX + "Theta At Goal", thetaController.atGoal()); + + // Check transition + if (xController.atGoal() && yController.atGoal() && thetaController.atGoal()) { + drivetrain.drive(new ChassisSpeeds()); // stop + currentState = CalibrationState.TUNE_AND_FIRE; + initialGuessPopulated = false; + } + } + + private void handleTuneAndFire() { + boolean applyGuess = SmartDashboard.getBoolean(PREFIX + "Apply Guess", false); + + // 1. Give an initial guess if we just arrived or if the operator requested it + if (!initialGuessPopulated || applyGuess) { + if (applyGuess) { + SmartDashboard.putBoolean(PREFIX + "Apply Guess", false); // reset + } + + double multiplier = SmartDashboard.getNumber(PREFIX + "Flywheel Multiplier", 1.0); + double[] guess = shotCalculator.getBallisticGuess(currentDistanceMeters); + + if (false && guess != null) { + SmartDashboard.putNumber(PREFIX + "Test Hood Angle", guess[0]); + SmartDashboard.putNumber(PREFIX + "Test Flywheel RPM", guess[1] * multiplier); + } else { + // Fallback interpolation from lookup if ballistic isn't set up + SmartDashboard.putNumber( + PREFIX + "Test Hood Angle", + shotCalculator.getLookupHoodAngleDegrees(currentDistanceMeters)); + SmartDashboard.putNumber( + PREFIX + "Test Flywheel RPM", + shotCalculator.getLookupFlywheelSpeed(currentDistanceMeters) * multiplier); + } + initialGuessPopulated = true; + } + + // 2. Read explicit tuning values from operator. No funny business. + double hoodSetpoint = + SmartDashboard.getNumber( + PREFIX + "Test Hood Angle", Constants.Launcher.offsetLegacyHoodAngleDegrees(35.0)); + double rpmSetpoint = SmartDashboard.getNumber(PREFIX + "Test Flywheel RPM", 1800.0); + boolean fireRequested = SmartDashboard.getBoolean(PREFIX + "Force Firing", false); + boolean nextRequested = SmartDashboard.getBoolean(PREFIX + "Confirm & Next", false); + + // 3. Apply literal assignments to hardware + // Always zero turret for straightforward distances + launcher.usePresets(Degrees.of(hoodSetpoint), Degrees.of(0), RPM.of(rpmSetpoint)); + + // Telemetry feedback + SmartDashboard.putNumber(PREFIX + "Actual Hood", launcher.getHoodAngleActual().in(Degrees)); + SmartDashboard.putNumber(PREFIX + "Actual RPM", launcher.getFlywheelSpeedActual().in(RPM)); + SmartDashboard.putBoolean(PREFIX + "Is At Goal", launcher.isAtGoal()); + + // 4. Handle "Force Firing" integration -> we can't fully control the indexer natively inside a + // standard command without parallel racing, + // so we assume the operator maps a secondary button to IndexerCommands.shouldFeedCommand() to + // actually shoot the pre-spun ball. + // The "Force Firing" boolean here is mostly visual if they prefer button boards. + + // 5. Transition + if (nextRequested) { + SmartDashboard.putBoolean(PREFIX + "Confirm & Next", false); // reset + + // Log it! + System.out.println( + String.format( + "[ShotCalibration] POINT LOGGED -> DISTANCE: %.2fm | HOOD: %.1f° | RPM: %.1f", + getActualDistance(), hoodSetpoint, rpmSetpoint)); + + // Save it into live memory to test immediately + shotCalculator.addDataPoint( + getActualDistance(), hoodSetpoint, rpmSetpoint, getActualDistance() / 15.0); + + currentState = CalibrationState.NEXT_DISTANCE; + } + } + + private void handleNextDistance() { + // Increment the tracking parameter + double step = SmartDashboard.getNumber(PREFIX + "Distance Step (m)", distanceStepMeters); + currentDistanceMeters += step; + + // Loop back to driving state + currentState = CalibrationState.ALIGN_AND_DRIVE; + } + + private double getActualDistance() { + Pose2d currentPose = drivetrain.getPoseEstimator().getCurrentPose(); + return currentPose.getTranslation().getDistance(hubTarget); + } + + @Override + public void end(boolean interrupted) { + System.out.println("[ShotCalibration] command ended. Interrupted: " + interrupted); + drivetrain.drive(new ChassisSpeeds()); + launcher.stopAllMotors(); + } + + @Override + public boolean isFinished() { + return false; // Manually cancelled by operator when they're done with all distances + } + + /** + * Wrap the calibration command with an indexer force-feed so the operator can fire shots during + * tuning. + */ + public static Command createWithFeed( + Launcher launcher, + GenericDrivetrain drivetrain, + double initialDistance, + double distanceStep) { + return Commands.parallel( + new ShotCalibrationCommand(launcher, drivetrain, initialDistance, distanceStep), + Commands.either( + IndexerCommands.shouldForceCommand(), + IndexerCommands.shouldChurnCommand(), + () -> SmartDashboard.getBoolean(PREFIX + "Force Firing", false))); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/TestCommands.java b/src/main/java/frc/robot/rebuilt/commands/TestCommands.java new file mode 100644 index 00000000..2b9243d0 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/TestCommands.java @@ -0,0 +1,87 @@ +package frc.robot.rebuilt.commands; + +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +// import frc.robot.rebuilt.subsystems.Climb.Climb; +import frc.robot.rebuilt.subsystems.Indexer.Indexer; +import frc.robot.rebuilt.subsystems.Launcher.Launcher; +import frc.robot.rebuilt.subsystems.intake.Intake; +import java.util.Map; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.sensors.Controller; + +public class TestCommands { + + private Map subsystems; + + Indexer indexer; + // Climb climb; + Intake intake; + static Launcher launcher; + + public TestCommands(Map subsystems) { + this.subsystems = subsystems; + indexer = (Indexer) subsystems.get(Constants.INDEXER); + // climb = (Climb) subsystems.get(Constants.CLIMB); + intake = (Intake) subsystems.get(Constants.INTAKE); + launcher = (Launcher) subsystems.get(Constants.LAUNCHER); + } + + public void configureButtonBindings(Controller controller) { + controller.setRightYAxis(controller.createRightYAxis().negate().deadzone(0.07)); + controller.setLeftYAxis(controller.createLeftYAxis().negate().deadzone(0.07)); + launcher.setDefaultCommand(launcher.getDefaultCommand()); + intake.setDefaultCommand( + Commands.run( + () -> { + intake.runHopper(controller.getRightYAxis()); + intake.runSpintake(controller.getLeftYAxis()); + }, + intake)); + + indexer.configTestControls(controller); + // intake.configTestController(controller); + // climb.configTestControls(controller); + controller + .createBButton() + .whileTrue(launcher.getTurretSysIdCommand().finallyDo(() -> launcher.stopAllMotors())); + controller + .createAButton() + .whileTrue(launcher.getFlyWheelSysIdCommand().finallyDo(() -> launcher.stopAllMotors())); + + // SmartTurret tuning commands + controller + .createXButton() + .whileTrue( + launcher.getTurretQuasistaticCommand().finallyDo(() -> launcher.stopAllMotors())); + controller + .createYButton() + .whileTrue(launcher.getTurretKsMapCommand().finallyDo(() -> launcher.stopAllMotors())); + controller + .createLeftBumper() + .whileTrue(launcher.getTurretDynamicCommand().finallyDo(() -> launcher.stopAllMotors())); + controller + .createRightBumper() + .whileTrue( + launcher.getTurretTrackingTuneCommand().finallyDo(() -> launcher.stopAllMotors())); + + // Seeking (MotionMagicExpo) tuning — D-pad up to hold + controller + .createUpPovButton() + .whileTrue( + launcher.getTurretSeekingTuneCommand().finallyDo(() -> launcher.stopAllMotors())); + + // // Turret PID tuning — hold right bumper to enter tuning mode + // controller + // .createRightBumper() + // .whileTrue(new TurretTuningCommand(launcher).finallyDo(() -> launcher.stopAllMotors())); + + // // D-pad left/right cycles turret presets while tuning + // controller + // .createRightPovButton() + // .onTrue(Commands.runOnce(() -> TurretTuningCommand.nextPreset())); + // controller + // .createLeftPovButton() + // .onTrue(Commands.runOnce(() -> TurretTuningCommand.previousPreset())); + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretDynamicCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretDynamicCommand.java new file mode 100644 index 00000000..67344b05 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/TurretDynamicCommand.java @@ -0,0 +1,337 @@ +package frc.robot.rebuilt.commands; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.ConcurrentLinkedQueue; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Dynamic feedforward characterization for the turret using bidirectional current pulses. + * + *

Applies alternating positive and negative current steps to create many acceleration transients + * in both directions. The bidirectional pulsing keeps the turret oscillating near its starting + * position so more transients can be recorded before hitting a soft limit. + * + *

Two distinct quantities are measured from the same dataset: + * + *

    + *
  • kS_dynamic: kinetic (dynamic) friction during motion. Computed from near-steady- + * state samples (|acceleration| below {@link #STEADY_STATE_ACCEL_THRESHOLD}) where {@code + * kS_dyn = sgn(v) * (current - kV * velocity)}. This is often different from the static kS + * measured by the quasistatic command. + *
  • kA: inertia feedforward. Computed from transient samples (|acceleration| above + * {@link #MIN_ACCEL_ROT_PER_SEC_SQ}) using kS_dynamic in the residual: {@code (current - + * kS_dyn * sgn(v) - kV * v) = kA * acceleration}. + *
+ * + *

kS_dynamic is written back to the SmartDashboard key {@code "Dynamic kS"} at the end of each + * run so it is automatically available for the next run's input (closing the tuning loop). + * + *

Data is collected at 250 Hz via a dedicated {@link Notifier}. Results are logged under the + * {@code "TurretDynamic/"} prefix. + */ +public class TurretDynamicCommand extends Command { + + private final SmartTurretController controller; + private final TalonFX talonFX; + private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final Timer timer = new Timer(); + + // High-frequency status signals from SmartTurretController (250 Hz on CANivore). + private final StatusSignal positionSignal; + private final StatusSignal velocitySignal; + private final StatusSignal accelerationSignal; + private final StatusSignal torqueCurrentSignal; + + // 250 Hz sampling infrastructure. + private final ConcurrentLinkedQueue sampleQueue = + new ConcurrentLinkedQueue<>(); + private Notifier samplingNotifier; + private volatile boolean recording = false; + + private final double lowerLimitRot; + private final double upperLimitRot; + private boolean positionSafetyTriggered = false; + + // Values read from SmartDashboard at command start. + private double inputKV; + private double stepAmps; + + /** Time to hold zero current before applying pulses. */ + private static final double SETTLE_DELAY_SECONDS = 1.0; + + /** Default step current (Amps). Configurable via SmartDashboard. */ + private static final double DEFAULT_STEP_AMPS = 50.0; + + /** + * Duration of each current pulse in seconds. Short enough for multiple oscillation cycles, long + * enough to generate a clean acceleration transient at each direction change. + */ + private static final double PULSE_DURATION_SECS = 0.08; + + /** + * Acceleration threshold below which a sample is considered steady-state (used for kS_dynamic). + * Units: mechanism rot/s^2. + */ + private static final double STEADY_STATE_ACCEL_THRESHOLD = 0.3; + + /** + * Minimum acceleration magnitude for transient samples used in kA regression. Units: mechanism + * rot/s^2. + */ + private static final double MIN_ACCEL_ROT_PER_SEC_SQ = 0.5; + + /** Minimum velocity magnitude to include a sample (filters near-zero noise). */ + private static final double MIN_VELOCITY_ROT_PER_SEC = 0.05; + + /** Safety margin from soft limits in mechanism rotations. */ + private static final double POSITION_SAFETY_MARGIN_ROT = 5.0 / 360.0; + + /** Sampling period for the high-frequency Notifier (4 ms = 250 Hz). */ + private static final double SAMPLING_PERIOD_SECONDS = 1.0 / 250.0; + + private static final String PREFIX = "TurretDynamic/"; + + public TurretDynamicCommand(SmartTurretController controller, GenericSubsystem requirement) { + this.controller = controller; + this.talonFX = controller.getTalonFX(); + + this.positionSignal = controller.getPositionSignal(); + this.velocitySignal = controller.getVelocitySignal(); + this.accelerationSignal = controller.getAccelerationSignal(); + this.torqueCurrentSignal = controller.getTorqueCurrentSignal(); + this.lowerLimitRot = controller.getConfig().getLowerLimitRotations(); + this.upperLimitRot = controller.getConfig().getUpperLimitRotations(); + addRequirements(requirement); + } + + @Override + public void initialize() { + controller.stop(); + timer.restart(); + sampleQueue.clear(); + positionSafetyTriggered = false; + recording = false; + + // Publish defaults — kV comes from the controller config; kS is not needed as an input + // because this command measures kS_dynamic directly from the data. + SmartDashboard.putNumber( + "Dynamic kV", SmartDashboard.getNumber("Dynamic kV", controller.getConfig().getKV())); + SmartDashboard.putNumber( + "Dynamic Step Amps", SmartDashboard.getNumber("Dynamic Step Amps", DEFAULT_STEP_AMPS)); + + inputKV = SmartDashboard.getNumber("Dynamic kV", controller.getConfig().getKV()); + stepAmps = SmartDashboard.getNumber("Dynamic Step Amps", DEFAULT_STEP_AMPS); + + System.out.println("[TurretDynamic] Step=" + stepAmps + " A, kV=" + inputKV); + System.out.println( + "[TurretDynamic] Bidirectional pulsing: " + + PULSE_DURATION_SECS + + "s pulses. kS_dynamic will be computed from data."); + + samplingNotifier = new Notifier(this::collectSample); + samplingNotifier.setName("TurretDynamicSampler"); + samplingNotifier.startPeriodic(SAMPLING_PERIOD_SECONDS); + } + + /** Called at 250 Hz by the Notifier. Refreshes signals and enqueues a sample. */ + private void collectSample() { + if (!recording) return; + + BaseStatusSignal.refreshAll( + positionSignal, velocitySignal, accelerationSignal, torqueCurrentSignal); + + sampleQueue.add( + new CharacterizationSample( + RobotController.getFPGATime() / 1e6, + positionSignal.getValueAsDouble(), + velocitySignal.getValueAsDouble(), + accelerationSignal.getValueAsDouble(), + torqueCurrentSignal.getValueAsDouble())); + } + + @Override + public void execute() { + double elapsed = timer.get(); + if (elapsed < SETTLE_DELAY_SECONDS) { + talonFX.setControl(torqueRequest.withOutput(0)); + recording = false; + Logger.recordOutput(PREFIX + "State", "Settling"); + return; + } + + // Safety check (uses cached signal value). + double actualPos = positionSignal.getValueAsDouble(); + if (actualPos > (upperLimitRot - POSITION_SAFETY_MARGIN_ROT) + || actualPos < (lowerLimitRot + POSITION_SAFETY_MARGIN_ROT)) { + positionSafetyTriggered = true; + talonFX.setControl(torqueRequest.withOutput(0)); + recording = false; + Logger.recordOutput(PREFIX + "State", "Position Safety"); + return; + } + + // Bidirectional pulsing: alternate polarity every PULSE_DURATION_SECS. + // Each direction change creates a fresh acceleration transient and near-zero-accel + // steady-state. + double pulseElapsed = elapsed - SETTLE_DELAY_SECONDS; + int pulseIndex = (int) (pulseElapsed / PULSE_DURATION_SECS); + double appliedAmps = (pulseIndex % 2 == 0) ? stepAmps : -stepAmps; + + talonFX.setControl(torqueRequest.withOutput(appliedAmps)); + recording = true; + + Logger.recordOutput(PREFIX + "Applied Current (A)", appliedAmps); + Logger.recordOutput(PREFIX + "Velocity (rot-s)", velocitySignal.getValueAsDouble()); + Logger.recordOutput(PREFIX + "Acceleration (rot-s2)", accelerationSignal.getValueAsDouble()); + Logger.recordOutput(PREFIX + "Position (rot)", actualPos); + Logger.recordOutput(PREFIX + "PulseIndex", pulseIndex); + Logger.recordOutput(PREFIX + "SampleCount", sampleQueue.size()); + } + + @Override + public void end(boolean interrupted) { + recording = false; + if (samplingNotifier != null) { + samplingNotifier.stop(); + samplingNotifier.close(); + samplingNotifier = null; + } + talonFX.setControl(torqueRequest.withOutput(0)); + controller.stop(); + + if (positionSafetyTriggered) { + Logger.recordOutput(PREFIX + "Status", "Position safety triggered — partial data"); + System.out.println("[TurretDynamic] Ended: position safety triggered (using collected data)"); + } + + // Drain and filter samples: require minimum velocity to avoid near-zero noise. + List allSamples = new ArrayList<>(); + CharacterizationSample s; + while ((s = sampleQueue.poll()) != null) { + if (Math.abs(s.velocityRotPerSec()) > MIN_VELOCITY_ROT_PER_SEC) { + allSamples.add(s); + } + } + + if (allSamples.size() < 10) { + Logger.recordOutput(PREFIX + "Status", "Not enough samples (" + allSamples.size() + ")"); + System.out.println("[TurretDynamic] Not enough samples: " + allSamples.size()); + return; + } + + // ---- Pass 1: Compute kS_dynamic from near-steady-state samples ------------------------- + // At near-constant velocity: current ≈ kS_dyn * sgn(v) + kV * v + // → kS_dyn estimate per sample = sgn(v) * (current - kV * v) + List ksDynEstimates = new ArrayList<>(); + for (CharacterizationSample sample : allSamples) { + if (Math.abs(sample.accelerationRotPerSecSq()) < STEADY_STATE_ACCEL_THRESHOLD) { + double ksDynEst = + Math.signum(sample.velocityRotPerSec()) + * (sample.currentAmps() - inputKV * sample.velocityRotPerSec()); + ksDynEstimates.add(ksDynEst); + } + } + + double kSDynamic; + if (ksDynEstimates.size() >= 5) { + kSDynamic = ksDynEstimates.stream().mapToDouble(Double::doubleValue).average().orElse(0); + } else { + // Not enough steady-state samples — fall back to the controller config kS. + kSDynamic = controller.getConfig().getKS(); + Logger.recordOutput( + PREFIX + "kS_dynamic Status", + "Insufficient steady-state samples (" + ksDynEstimates.size() + "), using config kS"); + System.out.println( + "[TurretDynamic] Not enough steady-state samples for kS_dynamic (" + + ksDynEstimates.size() + + "); using config kS = " + + kSDynamic); + } + + // ---- Pass 2: Compute kA from transient samples using kS_dynamic ----------------------- + // Model: current - kS_dyn * sgn(v) - kV * v = kA * acceleration + // 1-parameter LS: kA = sum(residual * a) / sum(a^2) + double sumRA = 0, sumAA = 0; + int transientCount = 0; + for (CharacterizationSample sample : allSamples) { + if (Math.abs(sample.accelerationRotPerSecSq()) > MIN_ACCEL_ROT_PER_SEC_SQ) { + double residual = + sample.currentAmps() + - kSDynamic * Math.signum(sample.velocityRotPerSec()) + - inputKV * sample.velocityRotPerSec(); + double a = sample.accelerationRotPerSecSq(); + sumRA += residual * a; + sumAA += a * a; + transientCount++; + } + } + + if (transientCount < 5 || Math.abs(sumAA) < 1e-12) { + Logger.recordOutput( + PREFIX + "Status", "Not enough transient samples for kA (" + transientCount + ")"); + System.out.println("[TurretDynamic] Not enough transient samples: " + transientCount); + // Still output kS_dynamic even if kA failed. + outputKSDynamic(kSDynamic, ksDynEstimates.size()); + return; + } + + double kA = sumRA / sumAA; + + // ---- Output results ------------------------------------------------------------------- + Logger.recordOutput(PREFIX + "kS_dynamic (Amps)", kSDynamic); + Logger.recordOutput(PREFIX + "kS_dynamic Sample Count", ksDynEstimates.size()); + Logger.recordOutput(PREFIX + "kA (Amps per rot-s2)", kA); + Logger.recordOutput(PREFIX + "kA Sample Count", transientCount); + Logger.recordOutput(PREFIX + "Input kV (Amps per rot-s)", inputKV); + Logger.recordOutput(PREFIX + "Total Sample Count", allSamples.size()); + Logger.recordOutput(PREFIX + "Status", "Complete"); + + System.out.println( + "[TurretDynamic] kS_dynamic = " + + kSDynamic + + " A (" + + ksDynEstimates.size() + + " samples)"); + System.out.println( + "[TurretDynamic] kA = " + kA + " A/(rot/s^2) (" + transientCount + " samples)"); + System.out.println( + "[TurretDynamic] Used kV=" + inputKV + ", total samples=" + allSamples.size()); + + outputKSDynamic(kSDynamic, ksDynEstimates.size()); + } + + /** + * Publishes kS_dynamic to the SmartDashboard key {@code "Dynamic kS"} so it is available as the + * starting value for the next characterization run and for use in other commands. + */ + private void outputKSDynamic(double kSDynamic, int sampleCount) { + SmartDashboard.putNumber("Dynamic kS", kSDynamic); + System.out.println( + "[TurretDynamic] kS_dynamic=" + + kSDynamic + + " A written to SmartDashboard 'Dynamic kS' (" + + sampleCount + + " steady-state samples)."); + } + + @Override + public boolean isFinished() { + return positionSafetyTriggered; + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretKsMapCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretKsMapCommand.java new file mode 100644 index 00000000..309b5f9b --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/TurretKsMapCommand.java @@ -0,0 +1,219 @@ +package frc.robot.rebuilt.commands; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Generates a position-dependent kS map for the turret by probing at multiple positions. + * + *

At each test position, ramps TorqueCurrentFOC in both positive and negative directions until + * movement is detected, recording the minimum current needed. The result is two {@link + * InterpolatingDoubleTreeMap}s (positive and negative direction) that can be injected into the + * SmartTurretController for dynamic kS compensation. + */ +public class TurretKsMapCommand extends Command { + + private enum State { + MOVE_TO_POSITION, + SETTLE, + PROBE_POSITIVE, + PROBE_NEGATIVE, + RECORD_AND_ADVANCE, + DONE + } + + private final SmartTurretController controller; + private final TalonFX talonFX; + private final StatusSignal positionSignal; + private final StatusSignal velocitySignal; + private final MotionMagicTorqueCurrentFOC moveRequest = + new MotionMagicTorqueCurrentFOC(0).withSlot(0); + private final TorqueCurrentFOC probeRequest = new TorqueCurrentFOC(0); + private final Timer settleTimer = new Timer(); + + private final double[] testPositions; + private int currentIndex = 0; + private State currentState = State.MOVE_TO_POSITION; + + private double probeCurrent = 0.0; + private double ksPositive = 0.0; + private double ksNegative = 0.0; + + private static final double PROBE_RAMP_RATE_AMPS_PER_CYCLE = 0.05; // Amps per 20ms cycle + private static final double MOVEMENT_THRESHOLD_ROT_PER_SEC = 0.005; + private static final double POSITION_TOLERANCE_ROT = 0.01; + private static final double SETTLE_TIME_SECONDS = 0.5; + private static final double MAX_PROBE_CURRENT_AMPS = 30.0; + private static final int NUM_TEST_POSITIONS = 10; + private static final String PREFIX = "TurretKsMap/"; + + private final InterpolatingDoubleTreeMap resultMapPositive = new InterpolatingDoubleTreeMap(); + private final InterpolatingDoubleTreeMap resultMapNegative = new InterpolatingDoubleTreeMap(); + + public TurretKsMapCommand( + SmartTurretController controller, + Angle lowerLimit, + Angle upperLimit, + GenericSubsystem requirement) { + this.controller = controller; + this.talonFX = controller.getTalonFX(); + this.positionSignal = controller.getPositionSignal(); + this.velocitySignal = controller.getVelocitySignal(); + addRequirements(requirement); + + // Generate evenly-spaced test positions across the turret range. + double lowerRot = lowerLimit.in(edu.wpi.first.units.Units.Rotations); + double upperRot = upperLimit.in(edu.wpi.first.units.Units.Rotations); + testPositions = new double[NUM_TEST_POSITIONS]; + for (int i = 0; i < NUM_TEST_POSITIONS; i++) { + testPositions[i] = lowerRot + (upperRot - lowerRot) * i / (NUM_TEST_POSITIONS - 1); + } + } + + @Override + public void initialize() { + // Disable the SmartTurretController so the 200Hz Notifier stops sending competing commands. + controller.stop(); + currentIndex = 0; + currentState = State.MOVE_TO_POSITION; + resultMapPositive.clear(); + resultMapNegative.clear(); + Logger.recordOutput(PREFIX + "Status", "Running"); + } + + @Override + public void execute() { + // Refresh signals for latest 250 Hz data. + BaseStatusSignal.refreshAll(positionSignal, velocitySignal); + double actualPos = positionSignal.getValueAsDouble(); + double actualVel = velocitySignal.getValueAsDouble(); + + Logger.recordOutput(PREFIX + "State", currentState.name()); + Logger.recordOutput(PREFIX + "Position Index", currentIndex); + Logger.recordOutput(PREFIX + "Probe Current", probeCurrent); + + switch (currentState) { + case MOVE_TO_POSITION: + talonFX.setControl(moveRequest.withPosition(testPositions[currentIndex])); + if (Math.abs(actualPos - testPositions[currentIndex]) < POSITION_TOLERANCE_ROT + && Math.abs(actualVel) < MOVEMENT_THRESHOLD_ROT_PER_SEC) { + settleTimer.restart(); + currentState = State.SETTLE; + } + break; + + case SETTLE: + talonFX.setControl(probeRequest.withOutput(0)); + if (settleTimer.hasElapsed(SETTLE_TIME_SECONDS)) { + probeCurrent = 0; + currentState = State.PROBE_POSITIVE; + } + break; + + case PROBE_POSITIVE: + probeCurrent += PROBE_RAMP_RATE_AMPS_PER_CYCLE; + talonFX.setControl(probeRequest.withOutput(probeCurrent)); + if (Math.abs(velocitySignal.getValueAsDouble()) > MOVEMENT_THRESHOLD_ROT_PER_SEC) { + ksPositive = probeCurrent; + probeCurrent = 0; + settleTimer.restart(); + currentState = State.PROBE_NEGATIVE; + // Return to position first. + talonFX.setControl(probeRequest.withOutput(0)); + } else if (probeCurrent > MAX_PROBE_CURRENT_AMPS) { + ksPositive = MAX_PROBE_CURRENT_AMPS; + probeCurrent = 0; + settleTimer.restart(); + currentState = State.PROBE_NEGATIVE; + talonFX.setControl(probeRequest.withOutput(0)); + } + break; + + case PROBE_NEGATIVE: + if (!settleTimer.hasElapsed(SETTLE_TIME_SECONDS)) { + talonFX.setControl(probeRequest.withOutput(0)); + return; + } + probeCurrent -= PROBE_RAMP_RATE_AMPS_PER_CYCLE; + talonFX.setControl(probeRequest.withOutput(probeCurrent)); + if (Math.abs(velocitySignal.getValueAsDouble()) > MOVEMENT_THRESHOLD_ROT_PER_SEC) { + ksNegative = Math.abs(probeCurrent); + probeCurrent = 0; + currentState = State.RECORD_AND_ADVANCE; + } else if (Math.abs(probeCurrent) > MAX_PROBE_CURRENT_AMPS) { + ksNegative = MAX_PROBE_CURRENT_AMPS; + probeCurrent = 0; + currentState = State.RECORD_AND_ADVANCE; + } + break; + + case RECORD_AND_ADVANCE: + talonFX.setControl(probeRequest.withOutput(0)); + resultMapPositive.put(testPositions[currentIndex], ksPositive); + resultMapNegative.put(testPositions[currentIndex], ksNegative); + System.out.printf( + "[TurretKsMap] Position %.4f rot: kS+ = %.3f A, kS- = %.3f A%n", + testPositions[currentIndex], ksPositive, ksNegative); + Logger.recordOutput( + PREFIX + "kS+ @ " + String.format("%.1f", testPositions[currentIndex] * 360) + "deg", + ksPositive); + Logger.recordOutput( + PREFIX + "kS- @ " + String.format("%.1f", testPositions[currentIndex] * 360) + "deg", + ksNegative); + + currentIndex++; + if (currentIndex < testPositions.length) { + currentState = State.MOVE_TO_POSITION; + } else { + currentState = State.DONE; + } + break; + + case DONE: + break; + } + } + + @Override + public void end(boolean interrupted) { + talonFX.setControl(probeRequest.withOutput(0)); + // Re-disable the controller so the turret doesn't chase a stale target. + controller.stop(); + if (currentState == State.DONE) { + Logger.recordOutput(PREFIX + "Status", "Complete"); + System.out.println("[TurretKsMap] Mapping complete. Results:"); + for (int i = 0; i < testPositions.length; i++) { + double pos = testPositions[i]; + System.out.printf( + " %.1f deg: kS+ = %.3f A, kS- = %.3f A%n", + pos * 360, resultMapPositive.get(pos), resultMapNegative.get(pos)); + } + } else { + Logger.recordOutput(PREFIX + "Status", "Interrupted at index " + currentIndex); + } + } + + @Override + public boolean isFinished() { + return currentState == State.DONE; + } + + public InterpolatingDoubleTreeMap getResultMapPositive() { + return resultMapPositive; + } + + public InterpolatingDoubleTreeMap getResultMapNegative() { + return resultMapNegative; + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretQuasistaticCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretQuasistaticCommand.java new file mode 100644 index 00000000..952c7782 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/TurretQuasistaticCommand.java @@ -0,0 +1,219 @@ +package frc.robot.rebuilt.commands; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController; +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.ConcurrentLinkedQueue; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Quasistatic feedforward characterization for the turret using {@link TorqueCurrentFOC}. + * + *

Ramps torque current very slowly so the turret reaches near-steady-state velocity at each + * sample. Fits a 2-parameter model: {@code current = kS + kV * velocity} via least-squares + * regression. Samples are only recorded once velocity exceeds a threshold (turret is moving). + * + *

Data is collected at ~250 Hz via a dedicated {@link Notifier}, not at the 50 Hz robot loop. + * This provides ~5x more samples for significantly smoother regression data. + * + *

Results are logged under the "TurretQuasistatic/" prefix. + */ +public class TurretQuasistaticCommand extends Command { + + private final SmartTurretController controller; + private final TalonFX talonFX; + private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final Timer timer = new Timer(); + + // High-frequency status signals from SmartTurretController (250 Hz on CANivore). + private final StatusSignal positionSignal; + private final StatusSignal velocitySignal; + private final StatusSignal accelerationSignal; + private final StatusSignal torqueCurrentSignal; + + // 250 Hz sampling infrastructure. + private final ConcurrentLinkedQueue sampleQueue = + new ConcurrentLinkedQueue<>(); + private Notifier samplingNotifier; + private volatile boolean recording = false; + + private final double lowerLimitRot; + private final double upperLimitRot; + private boolean positionSafetyTriggered = false; + + /** Slow ramp rate so velocity has time to reach steady state at each current level. */ + private static final double RAMP_RATE_AMPS_PER_SEC = 0.5; + + /** Time to wait before ramping, letting the system settle. */ + private static final double SETTLE_DELAY_SECONDS = 1.0; + + /** Minimum velocity before we include a sample in the regression (turret must be moving). */ + private static final double MIN_VELOCITY_ROT_PER_SEC = 0.01; + + /** Safety margin from soft limits in mechanism rotations. */ + private static final double POSITION_SAFETY_MARGIN_ROT = 5.0 / 360.0; + + /** Sampling period for the high-frequency Notifier (4 ms = 250 Hz). */ + private static final double SAMPLING_PERIOD_SECONDS = 1.0 / 250.0; + + private static final String PREFIX = "TurretQuasistatic/"; + + public TurretQuasistaticCommand(SmartTurretController controller, GenericSubsystem requirement) { + this.controller = controller; + this.talonFX = controller.getTalonFX(); + this.positionSignal = controller.getPositionSignal(); + this.velocitySignal = controller.getVelocitySignal(); + this.accelerationSignal = controller.getAccelerationSignal(); + this.torqueCurrentSignal = controller.getTorqueCurrentSignal(); + this.lowerLimitRot = controller.getConfig().getLowerLimitRotations(); + this.upperLimitRot = controller.getConfig().getUpperLimitRotations(); + addRequirements(requirement); + } + + @Override + public void initialize() { + controller.stop(); + timer.restart(); + sampleQueue.clear(); + positionSafetyTriggered = false; + recording = false; + + // Start the 250 Hz sampling Notifier. + samplingNotifier = new Notifier(this::collectSample); + samplingNotifier.setName("TurretQuasistaticSampler"); + samplingNotifier.startPeriodic(SAMPLING_PERIOD_SECONDS); + } + + /** Called at 250 Hz by the Notifier. Refreshes signals and enqueues a sample. */ + private void collectSample() { + if (!recording) return; + + BaseStatusSignal.refreshAll( + positionSignal, velocitySignal, accelerationSignal, torqueCurrentSignal); + + sampleQueue.add( + new CharacterizationSample( + RobotController.getFPGATime() / 1e6, + positionSignal.getValueAsDouble(), + velocitySignal.getValueAsDouble(), + accelerationSignal.getValueAsDouble(), + torqueCurrentSignal.getValueAsDouble())); + } + + @Override + public void execute() { + double elapsed = timer.get(); + if (elapsed < SETTLE_DELAY_SECONDS) { + talonFX.setControl(torqueRequest.withOutput(0)); + recording = false; + Logger.recordOutput(PREFIX + "State", "Settling"); + return; + } + + // Safety check (uses cached signal value — no refreshAll needed at 50 Hz). + double actualPos = positionSignal.getValueAsDouble(); + if (actualPos > (upperLimitRot - POSITION_SAFETY_MARGIN_ROT) + || actualPos < (lowerLimitRot + POSITION_SAFETY_MARGIN_ROT)) { + positionSafetyTriggered = true; + talonFX.setControl(torqueRequest.withOutput(0)); + recording = false; + Logger.recordOutput(PREFIX + "State", "Position Safety"); + return; + } + + double rampTime = elapsed - SETTLE_DELAY_SECONDS; + double current = rampTime * RAMP_RATE_AMPS_PER_SEC; + talonFX.setControl(torqueRequest.withOutput(current)); + recording = true; + + // 50 Hz dashboard telemetry for live viewing. + Logger.recordOutput(PREFIX + "Current (A)", current); + Logger.recordOutput(PREFIX + "Velocity (rot-s)", velocitySignal.getValueAsDouble()); + Logger.recordOutput(PREFIX + "Position (rot)", actualPos); + Logger.recordOutput(PREFIX + "SampleCount", sampleQueue.size()); + } + + @Override + public void end(boolean interrupted) { + // Stop sampling first, then stop the motor. + recording = false; + if (samplingNotifier != null) { + samplingNotifier.stop(); + samplingNotifier.close(); + samplingNotifier = null; + } + talonFX.setControl(torqueRequest.withOutput(0)); + controller.stop(); + + if (positionSafetyTriggered) { + Logger.recordOutput(PREFIX + "Status", "Position safety triggered"); + System.out.println("[TurretQuasistatic] Ended: position safety triggered"); + } + + // Drain the queue, filtering for samples where the turret is actually moving. + List samples = new ArrayList<>(); + CharacterizationSample s; + while ((s = sampleQueue.poll()) != null) { + if (Math.abs(s.velocityRotPerSec()) > MIN_VELOCITY_ROT_PER_SEC) { + samples.add(s); + } + } + + if (samples.size() < 10) { + Logger.recordOutput(PREFIX + "Status", "Not enough samples (" + samples.size() + ")"); + System.out.println("[TurretQuasistatic] Not enough samples: " + samples.size()); + return; + } + + // 2-parameter least-squares fit: current = kS + kV * velocity + // Normal equations: + // [n, sum_v ] [kS] [sum_i ] + // [sum_v, sum_vv] [kV] = [sum_iv] + double n = samples.size(); + double sumV = 0, sumVV = 0, sumI = 0, sumIV = 0; + + for (CharacterizationSample sample : samples) { + double v = sample.velocityRotPerSec(); + double c = sample.currentAmps(); + sumV += v; + sumVV += v * v; + sumI += c; + sumIV += c * v; + } + + double det = n * sumVV - sumV * sumV; + if (Math.abs(det) < 1e-12) { + Logger.recordOutput(PREFIX + "Status", "Singular matrix"); + System.out.println("[TurretQuasistatic] Singular matrix — no solution"); + return; + } + + double kS = (sumI * sumVV - sumV * sumIV) / det; + double kV = (n * sumIV - sumV * sumI) / det; + + Logger.recordOutput(PREFIX + "kS (Amps)", kS); + Logger.recordOutput(PREFIX + "kV (Amps per rot-s)", kV); + Logger.recordOutput(PREFIX + "Status", "Complete (" + samples.size() + " samples)"); + + System.out.println("[TurretQuasistatic] kS = " + kS + " A, kV = " + kV + " A/(rot/s)"); + System.out.println("[TurretQuasistatic] Samples: " + samples.size()); + } + + @Override + public boolean isFinished() { + return positionSafetyTriggered; + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretSeekingTuneCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretSeekingTuneCommand.java new file mode 100644 index 00000000..ee3e7049 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/TurretSeekingTuneCommand.java @@ -0,0 +1,175 @@ +package frc.robot.rebuilt.commands; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Tunes the SEEKING (MotionMagicExpo) mode by alternating between two target positions. + * + *

All Slot0 PID/FF gains and the MotionMagicExpo plant model (kV/kA in Volts) are adjustable via + * SmartDashboard in real-time. The target positions and dwell time at each target are also tunable. + * + *

This lets the operator observe overshoot, settling time, and profile shape by simply watching + * the turret move back and forth while tweaking gains on the dashboard. + */ +public class TurretSeekingTuneCommand extends Command { + + private final SmartTurretController controller; + private final TalonFX talonFX; + private final Timer timer = new Timer(); + private final Timer settleTimer = new Timer(); + + private boolean atTargetA = true; + + // Cached Slot0 gains for change detection. + private double lastKP = -1, lastKI = -1, lastKD = -1; + private double lastKS = -1, lastKV = -1, lastKA = -1; + // Cached expo gains for change detection. + private double lastExpoKV = -1, lastExpoKA = -1; + + private static final String PREFIX = "TurretSeekTune/"; + + public TurretSeekingTuneCommand(SmartTurretController controller, GenericSubsystem requirement) { + this.controller = controller; + this.talonFX = controller.getTalonFX(); + addRequirements(requirement); + } + + @Override + public void initialize() { + timer.restart(); + settleTimer.restart(); + atTargetA = true; + + var config = controller.getConfig(); + double lowerDeg = config.getLowerLimitRotations() * 360.0; + double upperDeg = config.getUpperLimitRotations() * 360.0; + + // Target positions (degrees) — default to ±50% of range from center. + SmartDashboard.putNumber( + PREFIX + "Target A (deg)", + SmartDashboard.getNumber(PREFIX + "Target A (deg)", lowerDeg * 0.5)); + SmartDashboard.putNumber( + PREFIX + "Target B (deg)", + SmartDashboard.getNumber(PREFIX + "Target B (deg)", upperDeg * 0.5)); + SmartDashboard.putNumber( + PREFIX + "Dwell Time (s)", SmartDashboard.getNumber(PREFIX + "Dwell Time (s)", 2.0)); + + // Slot0 PID gains. + SmartDashboard.putNumber(PREFIX + "Seeking kP", config.getSeekingKP()); + SmartDashboard.putNumber(PREFIX + "Seeking kI", config.getSeekingKI()); + SmartDashboard.putNumber(PREFIX + "Seeking kD", config.getSeekingKD()); + lastKP = config.getSeekingKP(); + lastKI = config.getSeekingKI(); + lastKD = config.getSeekingKD(); + + // Slot0 feedforward gains (Amps — TorqueCurrentFOC). + SmartDashboard.putNumber(PREFIX + "Seeking kS (A)", config.getKS()); + SmartDashboard.putNumber(PREFIX + "Seeking kV (A/rps)", config.getKV()); + SmartDashboard.putNumber(PREFIX + "Seeking kA (A/rps2)", config.getKA()); + lastKS = config.getKS(); + lastKV = config.getKV(); + lastKA = config.getKA(); + + // Expo plant model (Volts — always V/rps and V/rps^2). + SmartDashboard.putNumber(PREFIX + "Expo kV (V/rps)", config.getExpoKV()); + SmartDashboard.putNumber(PREFIX + "Expo kA (V/rps2)", config.getExpoKA()); + lastExpoKV = config.getExpoKV(); + lastExpoKA = config.getExpoKA(); + } + + @Override + public void execute() { + // Read target positions and dwell time. + double targetADeg = SmartDashboard.getNumber(PREFIX + "Target A (deg)", -60); + double targetBDeg = SmartDashboard.getNumber(PREFIX + "Target B (deg)", 60); + double dwellTimeSec = SmartDashboard.getNumber(PREFIX + "Dwell Time (s)", 2.0); + + // Switch targets after dwell time elapses. + if (settleTimer.hasElapsed(dwellTimeSec)) { + atTargetA = !atTargetA; + settleTimer.restart(); + } + + double targetDeg = atTargetA ? targetADeg : targetBDeg; + double targetRot = targetDeg / 360.0; + + // Command the turret (zero velocity/accel FF — SEEKING mode handles the profile). + controller.setTarget(Rotations.of(targetRot), 0, 0); + + // --- Live gain updates from SmartDashboard --- + double newKP = SmartDashboard.getNumber(PREFIX + "Seeking kP", lastKP); + double newKI = SmartDashboard.getNumber(PREFIX + "Seeking kI", lastKI); + double newKD = SmartDashboard.getNumber(PREFIX + "Seeking kD", lastKD); + double newKS = SmartDashboard.getNumber(PREFIX + "Seeking kS (A)", lastKS); + double newKV = SmartDashboard.getNumber(PREFIX + "Seeking kV (A/rps)", lastKV); + double newKA = SmartDashboard.getNumber(PREFIX + "Seeking kA (A/rps2)", lastKA); + + if (newKP != lastKP + || newKI != lastKI + || newKD != lastKD + || newKS != lastKS + || newKV != lastKV + || newKA != lastKA) { + Slot0Configs slot0 = new Slot0Configs(); + slot0.kP = newKP; + slot0.kI = newKI; + slot0.kD = newKD; + slot0.kS = newKS; + slot0.kV = newKV; + slot0.kA = newKA; + talonFX.getConfigurator().apply(slot0); + lastKP = newKP; + lastKI = newKI; + lastKD = newKD; + lastKS = newKS; + lastKV = newKV; + lastKA = newKA; + } + + // Expo plant model updates (Volts). + double newExpoKV = SmartDashboard.getNumber(PREFIX + "Expo kV (V/rps)", lastExpoKV); + double newExpoKA = SmartDashboard.getNumber(PREFIX + "Expo kA (V/rps2)", lastExpoKA); + + if (newExpoKV != lastExpoKV || newExpoKA != lastExpoKA) { + MotionMagicConfigs mmConfig = new MotionMagicConfigs(); + mmConfig.MotionMagicExpo_kV = newExpoKV; + mmConfig.MotionMagicExpo_kA = newExpoKA; + mmConfig.MotionMagicCruiseVelocity = controller.getConfig().getMaxVelocityMechRotPerSec(); + talonFX.getConfigurator().apply(mmConfig); + lastExpoKV = newExpoKV; + lastExpoKA = newExpoKA; + } + + // Logging. + double actualPos = controller.getActualPositionMechRot(); + double positionError = targetRot - actualPos; + + Logger.recordOutput(PREFIX + "TargetPositionDeg", targetDeg); + Logger.recordOutput(PREFIX + "ActualPositionDeg", actualPos * 360.0); + Logger.recordOutput(PREFIX + "PositionErrorDeg", positionError * 360.0); + Logger.recordOutput( + PREFIX + "ActualVelocityRPS", controller.getActualVelocityRadPerSec() / (2.0 * Math.PI)); + Logger.recordOutput(PREFIX + "AtTargetA", atTargetA); + Logger.recordOutput(PREFIX + "State", controller.getCurrentTurretState().name()); + } + + @Override + public void end(boolean interrupted) { + controller.stop(); + } + + @Override + public boolean isFinished() { + return false; // Run until interrupted. + } +} diff --git a/src/main/java/frc/robot/rebuilt/commands/TurretTrackingTuneCommand.java b/src/main/java/frc/robot/rebuilt/commands/TurretTrackingTuneCommand.java new file mode 100644 index 00000000..492950a3 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/commands/TurretTrackingTuneCommand.java @@ -0,0 +1,158 @@ +package frc.robot.rebuilt.commands; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.rebuilt.subsystems.Launcher.SmartTurretController; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; + +/** + * Generates a sinusoidal reference signal for the turret and measures tracking error. + * + *

Amplitude, frequency, PID gains (Slot1: tracking), and feedforward gains (kV, kA) are + * adjustable via SmartDashboard in real-time. The amplitude is automatically clamped so the + * sinusoidal stays within safe turret limits centered at 0. The analytically-derived velocity and + * acceleration of the sinusoid are passed as feedforward to the SmartTurretController. + * + *

Slot1 kS is always 0 in the firmware — the position-dependent kS is injected externally by the + * SmartTurretController. kV and kA run at full firmware frequency (1 kHz) for best performance. + */ +public class TurretTrackingTuneCommand extends Command { + + private final SmartTurretController controller; + private final TalonFX talonFX; + private final Timer timer = new Timer(); + + private final double maxSafeAmplitudeRot; + + private double amplitudeRotations; + private double frequencyHz; + + // Cached gain values for change detection. + private double lastKP = -1, lastKI = -1, lastKD = -1; + private double lastKV = -1, lastKA = -1; + + private static final double SAFETY_MARGIN_ROT = 10.0 / 360.0; // 10 degrees + private static final String PREFIX = "TurretTrackTune/"; + + public TurretTrackingTuneCommand(SmartTurretController controller, GenericSubsystem requirement) { + this.controller = controller; + this.talonFX = controller.getTalonFX(); + addRequirements(requirement); + + // Compute max safe amplitude: min(|lower|, |upper|) - safety margin, centered at 0. + double lowerLimit = Math.abs(controller.getConfig().getLowerLimitRotations()); + double upperLimit = Math.abs(controller.getConfig().getUpperLimitRotations()); + maxSafeAmplitudeRot = Math.max(0, Math.min(lowerLimit, upperLimit) - SAFETY_MARGIN_ROT); + } + + @Override + public void initialize() { + timer.restart(); + + // Initialize SmartDashboard defaults. + amplitudeRotations = SmartDashboard.getNumber(PREFIX + "Amplitude (rot)", 0.1); + frequencyHz = SmartDashboard.getNumber(PREFIX + "Frequency (Hz)", 0.5); + SmartDashboard.putNumber(PREFIX + "Amplitude (rot)", amplitudeRotations); + SmartDashboard.putNumber(PREFIX + "Frequency (Hz)", frequencyHz); + + double currentKP = controller.getConfig().getTrackingKP(); + double currentKI = controller.getConfig().getTrackingKI(); + double currentKD = controller.getConfig().getTrackingKD(); + SmartDashboard.putNumber(PREFIX + "Tracking kP", currentKP); + SmartDashboard.putNumber(PREFIX + "Tracking kI", currentKI); + SmartDashboard.putNumber(PREFIX + "Tracking kD", currentKD); + lastKP = currentKP; + lastKI = currentKI; + lastKD = currentKD; + + // Publish tunable feedforward gains (Amps). kV and kA run in the firmware slot. + double currentKV = controller.getConfig().getKV(); + double currentKA = controller.getConfig().getKA(); + SmartDashboard.putNumber(PREFIX + "Tracking kV (A/rps)", currentKV); + SmartDashboard.putNumber(PREFIX + "Tracking kA (A/rps2)", currentKA); + lastKV = currentKV; + lastKA = currentKA; + + Logger.recordOutput(PREFIX + "MaxSafeAmplitudeRot", maxSafeAmplitudeRot); + } + + @Override + public void execute() { + // Read tunable parameters. + amplitudeRotations = SmartDashboard.getNumber(PREFIX + "Amplitude (rot)", 0.1); + frequencyHz = SmartDashboard.getNumber(PREFIX + "Frequency (Hz)", 0.5); + + // Clamp amplitude to safe range. + amplitudeRotations = MathUtil.clamp(amplitudeRotations, 0, maxSafeAmplitudeRot); + + double t = timer.get(); + double omega = 2.0 * Math.PI * frequencyHz; + + // Sinusoidal position reference (mechanism rotations). + double positionRot = amplitudeRotations * Math.sin(omega * t); + // Analytical velocity (rad/s): d/dt [A * sin(wt)] * 2pi = A * w * cos(wt) * 2pi + double velocityRadPerSec = amplitudeRotations * omega * Math.cos(omega * t) * 2.0 * Math.PI; + // Analytical acceleration (rad/s^2): d/dt [velocity] = -A * w^2 * sin(wt) * 2pi + double accelRadPerSecSq = + -amplitudeRotations * omega * omega * Math.sin(omega * t) * 2.0 * Math.PI; + + controller.setTarget(Rotations.of(positionRot), velocityRadPerSec, accelRadPerSecSq); + + // Check if any gains changed on dashboard and apply Slot1 update. + double newKP = SmartDashboard.getNumber(PREFIX + "Tracking kP", lastKP); + double newKI = SmartDashboard.getNumber(PREFIX + "Tracking kI", lastKI); + double newKD = SmartDashboard.getNumber(PREFIX + "Tracking kD", lastKD); + double newKV = SmartDashboard.getNumber(PREFIX + "Tracking kV (A/rps)", lastKV); + double newKA = SmartDashboard.getNumber(PREFIX + "Tracking kA (A/rps2)", lastKA); + + if (newKP != lastKP + || newKI != lastKI + || newKD != lastKD + || newKV != lastKV + || newKA != lastKA) { + Slot1Configs slot1 = new Slot1Configs(); + slot1.kP = newKP; + slot1.kI = newKI; + slot1.kD = newKD; + // kS = 0: position-dependent kS is injected externally by SmartTurretController. + slot1.kS = 0; + slot1.kV = newKV; + slot1.kA = newKA; + talonFX.getConfigurator().apply(slot1); + lastKP = newKP; + lastKI = newKI; + lastKD = newKD; + lastKV = newKV; + lastKA = newKA; + } + + // Log tracking data via AdvantageKit. + double actualPos = controller.getActualPositionMechRot(); + double trackingError = positionRot - actualPos; + + Logger.recordOutput(PREFIX + "ReferencePositionRot", positionRot); + Logger.recordOutput(PREFIX + "ActualPositionRot", actualPos); + Logger.recordOutput(PREFIX + "TrackingErrorRot", trackingError); + Logger.recordOutput(PREFIX + "TrackingErrorDeg", trackingError * 360.0); + Logger.recordOutput(PREFIX + "VelocityFFRadPerSec", velocityRadPerSec); + Logger.recordOutput(PREFIX + "AccelFFRadPerSecSq", accelRadPerSecSq); + Logger.recordOutput(PREFIX + "ClampedAmplitudeRot", amplitudeRotations); + } + + @Override + public void end(boolean interrupted) { + controller.stop(); + } + + @Override + public boolean isFinished() { + return false; // Run until interrupted. + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/Climb.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/Climb.java new file mode 100644 index 00000000..c02a3c27 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/Climb.java @@ -0,0 +1,90 @@ +// 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.rebuilt.subsystems.Climb; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.commands.ClimbCommands.ClimbState; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.sensors.Controller; +import org.littletonrobotics.junction.Logger; + +/** The class climb controlls the climb */ +public class Climb extends GenericSubsystem { + private final ClimbIO io; + private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + /** Creates the climb subsystem and chooses the IO */ + public Climb() { + super("climb.json"); + if (RobotBase.isSimulation()) { + io = new ClimbIOSim(devices); + } else { + io = new ClimbIOReal(devices); + } + } + /** Sets a command that holds the climb at a given height */ + public Command climberCommand(Distance height) { + return Commands.run( + () -> { + setClimbHeight(height); + }) + .finallyDo( + () -> { + /** Resets to 0 when inactive */ + setClimbHeight(Meters.of(0)); + }); + } + /** Sets the climber io to idle */ + public Command idleCommand() { + return Commands.runOnce( + () -> { + io.idle(); + }, + this); + } + + public void configTestControls(Controller controller) { + controller.createBButton().whileTrue(climberCommand(Meters.of(.5))); + } + + public void setClimbHeight(Distance height) { + io.setHeight(height); + } + + public Distance getHeight() { + return inputs.climbHeight; + } + + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + Logger.processInputs("Climb", inputs); + } + + public void runClimb(double speed) { + io.runClimb(speed); + } + + public boolean isRequested(ClimbState state) { + return inputs.stateRequested == state; + } + + public boolean isCurrent(ClimbState state) { + return inputs.stateCurrent == state; + } + + public void setCurrentState(ClimbState state) { + inputs.stateCurrent = state; + } + + public void setRequestedState(ClimbState state) { + inputs.stateRequested = state; + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIO.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIO.java new file mode 100644 index 00000000..6d284de0 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIO.java @@ -0,0 +1,26 @@ +package frc.robot.rebuilt.subsystems.Climb; + +import static edu.wpi.first.units.Units.Inches; + +import edu.wpi.first.units.measure.Distance; +import frc.robot.rebuilt.commands.ClimbCommands; +import org.littletonrobotics.junction.AutoLog; + +/** IO interface for the Launcher subsystem. */ +public interface ClimbIO { + + @AutoLog + public static class ClimbIOInputs { + public Distance climbHeight = Inches.of(0); + public ClimbCommands.ClimbState stateRequested = ClimbCommands.ClimbState.DISABLED; + public ClimbCommands.ClimbState stateCurrent = ClimbCommands.ClimbState.DISABLED; + } + + public void runClimb(double speed); + + public void idle(); + + public void setHeight(Distance height); + + public default void updateInputs(ClimbIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOReal.java new file mode 100644 index 00000000..f3fcb193 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOReal.java @@ -0,0 +1,40 @@ +// 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.rebuilt.subsystems.Climb; + +import edu.wpi.first.units.measure.Distance; +import java.util.Map; +import yams.mechanisms.positional.Elevator; + +/** Add your docs here. */ +public class ClimbIOReal implements ClimbIO { + private static Elevator climber; + + protected Map devices; + + public ClimbIOReal(Map devices) { + this.devices = devices; + + climber = (Elevator) devices.get("lifter"); + } + + @Override + public void updateInputs(ClimbIOInputs inputs) { + + inputs.climbHeight = climber.getHeight(); + } + + public void idle() { + climber.getMotorController().setDutyCycle(0); + } + + public void setHeight(Distance height) { + climber.getMotorController().setPosition(height); + } + + public void runClimb(double speed) { + climber.getMotorController().setDutyCycle(speed); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOSim.java new file mode 100644 index 00000000..b1cfa642 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Climb/ClimbIOSim.java @@ -0,0 +1,17 @@ +// 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.rebuilt.subsystems.Climb; + +import java.util.Map; + +/** Add your docs here. */ +public class ClimbIOSim extends ClimbIOReal { + + protected Map devices; + + public ClimbIOSim(Map devices) { + super(devices); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatus.java b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatus.java new file mode 100644 index 00000000..4e567796 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatus.java @@ -0,0 +1,26 @@ +package frc.robot.rebuilt.subsystems.DriverDisplay; + +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.sensors.Controller; +import org.littletonrobotics.junction.Logger; + +public class HubStatus extends GenericSubsystem { + private HubStatusIO io = new HubStatusIOImpl(); + private HubStatusIOInputsAutoLogged inputs = new HubStatusIOInputsAutoLogged(); + + public void configureButtonBindings(Controller driver, Controller operator) { + // Trigger rumble = new Trigger(() -> inputs.timeRemainingInCurrentShift.lte(Seconds.of(3))); + // rumble.and(() -> inputs.timeRemainingInCurrentShift.gt(Seconds.of(2.5))); + // rumble + // .onTrue(Commands.runOnce(() -> driver.setRumble(0.5))) + // .onFalse(Commands.runOnce(() -> driver.setRumble(0))); + } + + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + + Logger.processInputs("HubStatus", inputs); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIO.java b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIO.java new file mode 100644 index 00000000..4b390d96 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIO.java @@ -0,0 +1,20 @@ +package frc.robot.rebuilt.subsystems.DriverDisplay; + +import edu.wpi.first.units.measure.Time; +import frc.robot.rebuilt.HubTracker.Shift; +import org.littletonrobotics.junction.AutoLog; + +public interface HubStatusIO { + @AutoLog + public static class HubStatusIOInputs { + public boolean activeNow = false; + Shift currentShift; + Time timeRemainingInCurrentShift; + Shift nextShift; + boolean isActiveNext; + String autoWinner; + double matchTime; + } + + public void updateInputs(HubStatusIOInputs Inputs); +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIOImpl.java b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIOImpl.java new file mode 100644 index 00000000..d40d1a4a --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/DriverDisplay/HubStatusIOImpl.java @@ -0,0 +1,22 @@ +package frc.robot.rebuilt.subsystems.DriverDisplay; + +import static edu.wpi.first.units.Units.Seconds; + +import frc.robot.rebuilt.HubTracker; +import frc.robot.rebuilt.HubTracker.Shift; + +public class HubStatusIOImpl implements HubStatusIO { + + @Override + public void updateInputs(HubStatusIOInputs inputs) { + inputs.activeNow = HubTracker.isActive(); + + inputs.currentShift = HubTracker.getCurrentShift().orElse(Shift.AUTO); + inputs.timeRemainingInCurrentShift = + HubTracker.timeRemainingInCurrentShift().orElse(Seconds.of(0)); + inputs.nextShift = HubTracker.getNextShift().orElse(Shift.AUTO); + inputs.isActiveNext = HubTracker.isActiveNext(); + inputs.autoWinner = HubTracker.getAutoWinner().map(it -> it.toString()).orElse("NA"); + inputs.matchTime = HubTracker.getMatchTime(); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/Indexer.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/Indexer.java new file mode 100644 index 00000000..77372997 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/Indexer.java @@ -0,0 +1,106 @@ +// 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.rebuilt.subsystems.Indexer; + +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.commands.IndexerCommands.IndexerState; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.arch.StateMachine; +import org.frc5010.common.sensors.Controller; +import org.littletonrobotics.junction.Logger; + +public class Indexer extends GenericSubsystem { + private final IndexerIO io; + private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); + + /** Creates a new Index and selects the IO to real or simulated. */ + public Indexer() { + super("indexer.json"); + + if (RobotBase.isSimulation()) { + io = (IndexerIO) new IndexerIOSim(devices); + } else { + io = new IndexerIOReal(devices); + } + } + + public void runSpindexer(double speed) { + io.runSpindexer(speed); + } + + public void runFeeder(double speed) { + io.runTransferFront(speed); + } + + // public void runTransferBack(double speed) { + // io.runTransferBack(speed); + // } + + public void runTransferFront(double speed) { + io.runTransferFront(speed); + } + + public void configTestControls(Controller controller) { + controller.createLeftBumper().whileTrue((spindexerCommand(.25)).alongWith(feederCommand(0.25))); + } + /** Command that runs the feeder at a given speed and stops when done */ + public Command feederCommand(double speed) { + return Commands.run( + () -> { + runFeeder(0.25); + }, + this) + .finallyDo( + () -> { + runFeeder(0); + }); + } + /** returns a command that runs the spindexer at a set speed and stops when done */ + public Command spindexerCommand(double speed) { + return Commands.run( + () -> { + runSpindexer(speed); + }) + .finallyDo( + () -> { + runSpindexer(0); + }); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + super.periodic(); + io.updateInputs(inputs); + Logger.processInputs("Indexer", inputs); + } + + @Override + public void simulationPeriodic() { + super.simulationPeriodic(); + } + + public boolean isRequested(IndexerState state) { + return inputs.stateRequested.compareTo(state) == 0; + } + + public boolean isCurrent(IndexerState state) { + return inputs.stateCurrent.compareTo(state) == 0; + } + + public void setCurrentState(IndexerState state) { + inputs.stateCurrent = state; + } + + public void setRequestedState(IndexerState state) { + inputs.stateRequested = state; + } + + public void setDefaultCommands(StateMachine stateMachine) { + inputs.stateRequested = IndexerState.IDLE; + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIO.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIO.java new file mode 100644 index 00000000..2719e9d3 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIO.java @@ -0,0 +1,23 @@ +package frc.robot.rebuilt.subsystems.Indexer; + +import frc.robot.rebuilt.commands.IndexerCommands; +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + @AutoLog + public static class IndexerIOInputs { + public double spindexerSpeed = 0; + public double transferFrontSpeed = 0; + public double transferBackSpeed = 0; + public IndexerCommands.IndexerState stateRequested = IndexerCommands.IndexerState.IDLE; + public IndexerCommands.IndexerState stateCurrent = IndexerCommands.IndexerState.IDLE; + } + + public void runSpindexer(double speed); + + public void runTransferFront(double speed); + + // public void runTransferBack(double speed); + + public default void updateInputs(IndexerIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOReal.java new file mode 100644 index 00000000..02ec5b42 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOReal.java @@ -0,0 +1,50 @@ +package frc.robot.rebuilt.subsystems.Indexer; + +import static edu.wpi.first.units.Units.Amps; + +import java.util.Map; +import org.frc5010.common.motors.function.PercentControlMotor; +import yams.mechanisms.velocity.FlyWheel; + +/** Implements the hardware Indexer IO */ +public class IndexerIOReal implements IndexerIO { + protected Map devices; + private PercentControlMotor spindexer; + // private PercentControlMotor transferFront, transferBack; + private FlyWheel transferFront; + + public IndexerIOReal(Map devices) { + spindexer = (PercentControlMotor) devices.get("spindexer"); + transferFront = (FlyWheel) devices.get("transfer"); + spindexer.setCurrentLimit(Amps.of(120)); + // transferFront = (PercentControlMotor) devicess.get("transfer_front"); + // transferBack = (PercentControlMotor) devices.get("transfer_back"); + // transferFront.invert(true); + // transferFront.setFollow(transferBack, false); + this.devices = devices; + } + /** Updates indexer input values with current motor speed */ + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.spindexerSpeed = spindexer.get(); + inputs.transferFrontSpeed = transferFront.getMotor().getDutyCycle(); + // inputs.transferFrontSpeed = transferFront.get(); + // inputs.transferBackSpeed = transferBack.get(); + } + /** Sets the spindexer motor speed */ + @Override + public void runSpindexer(double speed) { + spindexer.set(speed); + } + /** Sets the front transfer motor speed */ + @Override + public void runTransferFront(double speed) { + // transferFront.set(speed); + transferFront.getMotor().setDutyCycle(speed); + } + + // @Override + // public void runTransferBack(double speed) { + // transferBack.set(speed); + // } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOSim.java new file mode 100644 index 00000000..4e744926 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Indexer/IndexerIOSim.java @@ -0,0 +1,11 @@ +package frc.robot.rebuilt.subsystems.Indexer; + +import java.util.Map; + +public class IndexerIOSim extends IndexerIOReal { + protected Map devices; + + public IndexerIOSim(Map devices) { + super(devices); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/FieldRegions.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/FieldRegions.java new file mode 100644 index 00000000..466e6c6a --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/FieldRegions.java @@ -0,0 +1,161 @@ +// 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.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.rebuilt.FieldConstants; +import java.util.Optional; +import org.frc5010.common.utils.geometry.AllianceFlipUtil; + +/** Add your docs here. */ +public class FieldRegions { + static double topTrenchLeftX = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getX(); + static double topTrenchRightX = FieldConstants.TrenchZoneTop.nearAllianceRightDanger.getX(); + + static double topTrenchY = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getY(); + + static double topOppTrenchLeftX = FieldConstants.TrenchZoneTop.oppAllianceLeftDanger.getX(); + static double topOppTrenchRightX = FieldConstants.TrenchZoneTop.oppAllianceRightDanger.getX(); + + static double lowerTrenchLeftX = FieldConstants.TrenchZoneBottom.nearAllianceLeftDanger.getX(); + static double lowerTrenchRightX = FieldConstants.TrenchZoneBottom.nearAllianceRightDanger.getX(); + static double lowerTrenchY = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getY(); + + static double lowerOppTrenchLeftX = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getX(); + static double lowerOppTrenchRightX = + FieldConstants.TrenchZoneBottom.oppAllianceRightDanger.getX(); + + static Translation2d allianceCornerOrigin = new Translation2d(0, 0); + static Translation2d AllianceCornerTrench = + new Translation2d( + FieldConstants.TrenchZoneBottom.nearAlliance.getX() + - 1 / 2 * FieldConstants.LeftTrench.depth, + FieldConstants.fieldWidth); + static Translation2d topRightMidTrenchCorner = + new Translation2d( + FieldConstants.TrenchZoneTop.oppAlliance.getX() + - 1 / 2 * FieldConstants.RightTrench.depth, + FieldConstants.fieldWidth); + static Translation2d bottomRightMidTrenchCorner = + new Translation2d( + FieldConstants.TrenchZoneTop.oppAlliance.getX() - 1 / 2 * FieldConstants.LeftTrench.depth, + 0); + static Translation2d oppTopRightOrigin = + new Translation2d(FieldConstants.fieldLength, FieldConstants.fieldWidth); + static Translation2d oppBottemRightOrigin = new Translation2d(FieldConstants.fieldLength, 0); + + static Rectangle2d allianceField = new Rectangle2d(allianceCornerOrigin, AllianceCornerTrench); + static Rectangle2d upperMidField = + new Rectangle2d(FieldConstants.Hub.farLeftCorner, topRightMidTrenchCorner); + static Rectangle2d lowerMidField = + new Rectangle2d(FieldConstants.Hub.farRightCorner, bottomRightMidTrenchCorner); + static Rectangle2d oppUpperField = + new Rectangle2d(FieldConstants.Hub.oppFarLeftCorner, oppTopRightOrigin); + static Rectangle2d oppLowerField = + new Rectangle2d(FieldConstants.Hub.oppFarRightCorner, oppBottemRightOrigin); + + public static void setupFieldRegions() { + topTrenchLeftX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.nearAllianceLeftDanger).getX(); + topTrenchRightX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.nearAllianceRightDanger).getX(); + topTrenchY = AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.nearAllianceLeftDanger).getY(); + topOppTrenchLeftX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.oppAllianceLeftDanger).getX(); + topOppTrenchRightX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneTop.oppAllianceRightDanger).getX(); + lowerTrenchLeftX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.nearAllianceLeftDanger).getX(); + lowerTrenchRightX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.nearAllianceRightDanger).getX(); + lowerTrenchY = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger).getY(); + lowerOppTrenchLeftX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger).getX(); + lowerOppTrenchRightX = + AllianceFlipUtil.apply(FieldConstants.TrenchZoneBottom.oppAllianceRightDanger).getX(); + + allianceCornerOrigin = AllianceFlipUtil.apply(new Translation2d(0, 0)); + AllianceCornerTrench = + AllianceFlipUtil.apply( + new Translation2d( + FieldConstants.TrenchZoneBottom.nearAlliance.getX() + - 1 / 2 * FieldConstants.LeftTrench.depth, + FieldConstants.fieldWidth)); + topRightMidTrenchCorner = + AllianceFlipUtil.apply( + new Translation2d( + FieldConstants.TrenchZoneTop.oppAlliance.getX() + - 1 / 2 * FieldConstants.RightTrench.depth, + FieldConstants.fieldWidth)); + bottomRightMidTrenchCorner = + AllianceFlipUtil.apply( + new Translation2d( + FieldConstants.TrenchZoneTop.oppAlliance.getX() + - 1 / 2 * FieldConstants.LeftTrench.depth, + 0)); + oppTopRightOrigin = + AllianceFlipUtil.apply( + new Translation2d(FieldConstants.fieldLength, FieldConstants.fieldWidth)); + oppBottemRightOrigin = AllianceFlipUtil.apply(new Translation2d(FieldConstants.fieldLength, 0)); + + allianceField = new Rectangle2d(allianceCornerOrigin, AllianceCornerTrench); + upperMidField = new Rectangle2d(FieldConstants.Hub.farLeftCorner, topRightMidTrenchCorner); + lowerMidField = new Rectangle2d(FieldConstants.Hub.farRightCorner, bottomRightMidTrenchCorner); + oppUpperField = new Rectangle2d(FieldConstants.Hub.oppFarLeftCorner, oppTopRightOrigin); + oppLowerField = new Rectangle2d(FieldConstants.Hub.oppFarRightCorner, oppBottemRightOrigin); + } + + public static boolean isNearTrench(double currentX, double currentY) { + boolean nearAllianceTop = + ((currentX > topTrenchLeftX && currentX < topTrenchRightX) && currentY > topTrenchY); + + boolean nearOppAllianceTop = + ((currentX > topOppTrenchLeftX && currentX < topOppTrenchRightX) && currentY > topTrenchY); + + boolean nearAllianceBottom = + ((currentX > lowerTrenchLeftX && currentX < lowerTrenchRightX) && currentY < lowerTrenchY); + + boolean nearOppAllianceBottom = + ((currentX > lowerOppTrenchLeftX && currentX < lowerOppTrenchRightX) + && currentY < lowerTrenchY); + + return nearAllianceTop || nearOppAllianceTop || nearAllianceBottom || nearOppAllianceBottom; + } + + static Translation2d leftAdjustment = new Translation2d(Meters.of(1), Meters.of(1.5)); + static Translation2d rightAdjustment = new Translation2d(Meters.of(1), Meters.of(-1.5)); + + public static Optional determineTargetPose(Pose2d currentPose) { + Boolean inAllianceField = allianceField.contains(currentPose.getTranslation()); + Boolean inUpperMidField = upperMidField.contains(currentPose.getTranslation()); + Boolean inLowerMidField = lowerMidField.contains(currentPose.getTranslation()); + Boolean inOppUpperField = oppUpperField.contains(currentPose.getTranslation()); + Boolean inOppLowerField = oppLowerField.contains(currentPose.getTranslation()); + SmartDashboard.putBoolean("In Alliance Field", inAllianceField); + SmartDashboard.putBoolean("In Upper Mid Field", inUpperMidField); + SmartDashboard.putBoolean("In Lower Mid Field", inLowerMidField); + SmartDashboard.putBoolean("In Opp Upper Field", inOppUpperField); + SmartDashboard.putBoolean("In Opp Lower Field", inOppLowerField); + if (inAllianceField) { + return Optional.of(FieldConstants.Hub.topCenterPoint.toTranslation2d()); + } else if (inUpperMidField) { + return Optional.of(FieldConstants.Tower.leftUpright.plus(leftAdjustment)); + } else if (inLowerMidField) { + return Optional.of(FieldConstants.Tower.rightUpright.plus(rightAdjustment)); + } else if (inOppUpperField) { + return Optional.of(FieldConstants.Tower.leftUpright.plus(leftAdjustment)); + } else if (inOppLowerField) { + return Optional.of(FieldConstants.Tower.rightUpright.plus(rightAdjustment)); + } else { + return Optional.empty(); + } + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/Launcher.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/Launcher.java new file mode 100644 index 00000000..7b24b91f --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/Launcher.java @@ -0,0 +1,488 @@ +// 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.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.Rebuilt; +import frc.robot.rebuilt.commands.LauncherCommands.LauncherState; +import java.util.Map; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; +import yams.mechanisms.positional.Arm; +import yams.mechanisms.positional.Pivot; + +public class Launcher extends GenericSubsystem { + private final LauncherIO io; + private final Arm hood; + private final LauncherIOInputsAutoLogged inputs = new LauncherIOInputsAutoLogged(); + public static Transform3d robotToTurret = new Transform3d(); + private Map subsystems; + private SmartTurretController smartTurretController; + private Notifier turretProfileNotifier; + + private static final double PROFILE_PERIOD_SECONDS = 0.005; // 200 Hz + + /** Creates a new Launcher. */ + public Launcher(Map subsystems) { + super("launcher.json"); + + this.subsystems = subsystems; + Pivot turret = (Pivot) devices.get("turret"); + hood = (Arm) devices.get("hood"); + robotToTurret = + new Transform3d( + turret.getPivotConfig().getMechanismPositionConfig().getRelativePosition().get(), + new Rotation3d()); + /** Chooses the IO implimentation to be real or simulated */ + if (RobotBase.isSimulation()) { + io = new LauncherIOSim(devices, subsystems); + } else { + io = new LauncherIOReal(devices, subsystems); + } + + io.configureShotCalculator(ShotCalculator.getInstance()); + + // Register the SmartTurretController's high-frequency stepping loop. + // This runs at 200 Hz (5 ms) via a Notifier, evaluating state transitions and + // sending control requests to the TalonFX. + smartTurretController = io.getSmartTurretController(); + if (smartTurretController != null) { + turretProfileNotifier = + new Notifier(() -> smartTurretController.step(PROFILE_PERIOD_SECONDS)); + turretProfileNotifier.setName("SmartTurret"); + turretProfileNotifier.startPeriodic(PROFILE_PERIOD_SECONDS); + } + + new edu.wpi.first.wpilibj2.command.button.Trigger(io.getTurretZeroButtonSupplier()) + .onTrue(zeroTurretCommand()); + } + + /** + * Updates the inputs of the Launcher subsystem from the physical devices. + * + *

This method is called periodically by the GenericSubsystem class. + */ + @Override + public void periodic() { + super.periodic(); + + io.updateInputs(inputs); + + Logger.processInputs("Launcher", inputs); + } + + /** + * Called every time the scheduler runs while the robot is in simulation mode. Used to update + * simulation models. + */ + @Override + public void simulationPeriodic() { + super.simulationPeriodic(); + io.updateSimulation(this, Rebuilt.indexer); + } + + /** + * Run the shooter at the given speed. This is a convenience method which simply calls + * setUpperSpeed with the given speed. + * + * @param speed the speed to set the upper shooter motor to, in units of RPM. + */ + public void runShooter(double speed) { + io.runShooter(speed); + } + + public void setHoodAngle(Angle angle) { + io.setHoodAngle(angle); + } + + public void setTurretRotation(Angle angle) { + io.setTurretRotation(angle); + } + + public boolean isShooting() { + return inputs.stateCurrent == LauncherState.PREP || inputs.stateCurrent == LauncherState.PRESET; + } + + public Command getHoodSysIdCommand() { + return io.getHoodSysIdCommand(this); + } + + public Command getTurretSysIdCommand() { + return io.getTurretSysIdCommand(this); + } + + public Command getFlyWheelSysIdCommand() { + return io.getFlyWheelSysIdCommand(this); + } + + public Command getHoodCharacterizationCommand() { + return io.getHoodCharacterizationCommand(this); + } + + public Command getTurretCharacterizationCommand() { + return io.getTurretCharacterizationCommand(this); + } + + public Command getTurretQuasistaticCommand() { + return io.getTurretQuasistaticCommand(this); + } + + public Command getTurretDynamicCommand() { + return io.getTurretDynamicCommand(this); + } + + public Command getTurretKsMapCommand() { + return io.getTurretKsMapCommand(this); + } + + public Command getTurretTrackingTuneCommand() { + return io.getTurretTrackingTuneCommand(this); + } + + public Command getTurretSeekingTuneCommand() { + return io.getTurretSeekingTuneCommand(this); + } + + public Translation2d getRobotTarget() { + return io.determineTarget().get(); + } + + @Override + public Command getDefaultCommand() { + return Commands.runOnce( + () -> { + io.stopAllMotors(); + }, + this); + } + + public void stopAllMotors() { + io.stopAllMotors(); + } + /** Command that aims the launcher using hood turret and flywheel values from calculations */ + public Command trackTargetCommand() { + return Commands.run( + () -> { + io.setHoodAngle(inputs.hoodAngleCalculated); + io.setTurretRotationWithFeedforward( + inputs.turretAngleCalculated, + inputs.turretFeedforwardRadPerSec, + inputs.turretFeedforwardAccelRadPerSecSq); + io.setFlyWheelVelocity(inputs.flyWheelSpeedCalculated); + }); + } + /** Aims the launcher using the preset hood angle and calculates flywheel and turret values */ + public Command trackTargetLowCommand() { + return Commands.run( + () -> { + io.setHoodAngleLow(); + io.setTurretRotationWithFeedforward( + inputs.turretAngleCalculated, + inputs.turretFeedforwardRadPerSec, + inputs.turretFeedforwardAccelRadPerSecSq); + io.setFlyWheelVelocity(inputs.flyWheelSpeedCalculated); + }); + } + /** Aims the turret and sets the flywheel to a given speed */ + public Command trackTargetCommand(double speed) { + return Commands.run( + () -> { + io.setHoodAngle(hood.getMotorController().getConfig().getMechanismLowerLimit().get()); + io.setTurretRotationWithFeedforward( + inputs.turretAngleCalculated, + inputs.turretFeedforwardRadPerSec, + inputs.turretFeedforwardAccelRadPerSecSq); + io.setFlyWheelVelocity(RPM.of(speed)); + }); + } + + /** + * A command which stops the tracking of a target and resets the turret rotation and hood angle to + * 0 degrees. + * + * @return a command which stops tracking and resets the turret rotation and hood angle. + */ + public Command stopTrackingCommand() { + return Commands.runOnce( + () -> { + setTurretRotation(Degrees.of(0)); + setHoodAngle(Constants.Launcher.LOW_HOOD_ANGLE); + io.setFlyWheelVelocity(RPM.of(0)); + }); + } + + /** + * Checks if the robot is at the desired speed and angle. This method returns true if the robot + * speed and angle are within the allowed tolerances of the desired values. + * + * @return true if the robot is at the desired speed and angle, false otherwise. + */ + public boolean isAtGoal() { + return inputs.flyWheelSpeedAtGoal && inputs.turretAngleAtGoal && inputs.isValidCalculation; + } + + public boolean isOKToFire() { + return inputs.isValidCalculation; + } + + public boolean isRequested(LauncherState state) { + return inputs.stateRequested == state; + } + /** Checks if the current launcher matches the given state */ + public boolean isCurrent(LauncherState state) { + return inputs.stateCurrent == state; + } + /** Updates the launcher's current state */ + public void setCurrentState(LauncherState state) { + inputs.stateCurrent = state; + } + /** Sets the launcher's requested state to transition to */ + public void setRequestedState(LauncherState state) { + inputs.stateRequested = state; + } + + public LauncherState getCurrentState() { + return inputs.stateCurrent; + } + + public LauncherState getPreTrenchState() { + return inputs.preTrenchState; + } + + public boolean isNearTrench() { + boolean nearTrench = io.isNearTrench(); + if (nearTrench && (getCurrentState() != LauncherState.AUTO_HAMMERTIME)) { + inputs.preTrenchState = getCurrentState(); + if (getCurrentState() == LauncherState.PRESET) { + inputs.preTrenchState = LauncherState.LOW_SPEED; + } + } + return nearTrench; + } + + /** Applies the hood and turret angle, and the flywheel speed */ + public void usePresets(Angle hoodAngle, Angle turretAngle, AngularVelocity flywheelSpeed) { + io.setHoodAngle(hoodAngle); + io.setTurretRotation(turretAngle); + io.setFlyWheelVelocity(flywheelSpeed); + } + + public ShotCalculator.ShootingParameters getShootingParameters( + Supplier robotPoseSupplier, Supplier targetPositionSupplier) { + return io.getShootingParameters(robotPoseSupplier, targetPositionSupplier); + } + + // ---- Actual (measured) value getters for tuning/telemetry ---- + + /** Get the actual hood angle as measured by the encoder. */ + public Angle getHoodAngleActual() { + return inputs.hoodAngleActual; + } + + /** Get the actual turret angle as measured by the encoder. */ + public Angle getTurretAngleActual() { + return inputs.turretAngleActual; + } + + /** Get the actual flywheel speed as measured by the encoder. */ + public AngularVelocity getFlywheelSpeedActual() { + return inputs.flyWheelSpeedActual; + } + + // ---- Desired (setpoint) value getters for tuning/telemetry ---- + + /** Get the desired hood angle setpoint. */ + public Angle getHoodAngleDesired() { + return inputs.hoodAngleDesired; + } + + /** Get the desired turret angle setpoint. */ + public Angle getTurretAngleDesired() { + return inputs.turretAngleDesired; + } + + /** Get the desired flywheel speed setpoint. */ + public AngularVelocity getFlywheelSpeedDesired() { + return inputs.flyWheelSpeedDesired; + } + + public Boolean isHoodMoving() { + return inputs.hoodMoving; + } + + public Boolean isHoodStalled() { + return io.isHoodStalled(); + } + + public void zeroHood() { + io.resetHoodAngle(Degrees.of(12.723)); + } + + // ---- Error and at-goal getters for tuning/telemetry ---- + + /** Get the flywheel speed error (actual - desired). */ + public AngularVelocity getFlywheelSpeedError() { + return inputs.flyWheelSpeedError; + } + + /** Get the hood angle error in degrees (actual - desired). */ + public double getHoodAngleError() { + return inputs.hoodAngleError; + } + + /** Get the turret angle error in degrees (actual - desired). */ + public double getTurretAngleError() { + return inputs.turretAngleError; + } + + /** Whether the flywheel speed is within tolerance of the setpoint. */ + public boolean isFlywheelAtGoal() { + return inputs.flyWheelSpeedAtGoal; + } + + public double getHoodVelocity() { + return inputs.hoodVelocity; + } + + public boolean isFlywheelAtOrAboveGoal() { + return inputs.flyWheelSpeedAtGoal || inputs.flyWheelSpeedActual.gt(inputs.flyWheelSpeedDesired); + } + + /** Whether the hood angle is within tolerance of the setpoint. */ + public boolean isHoodAtGoal() { + return inputs.hoodAngleAtGoal; + } + + /** Whether the turret angle is within tolerance of the setpoint. */ + public boolean isTurretAtGoal() { + return inputs.turretAngleAtGoal; + } + + public Command increaseHoodAngleCommand() { + return Commands.runOnce( + () -> { + Angle newAngle = inputs.hoodAngleActual.plus(Degrees.of(0.5)); + Angle upperLimit = + hood.getMotorController().getConfig().getMechanismUpperLimit().orElse(Degrees.of(60)); + if (newAngle.lt(upperLimit)) { + io.setHoodAngle(newAngle); + } + }); + } + /** Decreases the hood angle by 0.5 degrees and respects the configured lower limit. */ + public Command decreaseHoodAngleCommand() { + return Commands.runOnce( + () -> { + Angle newAngle = inputs.hoodAngleActual.minus(Degrees.of(0.5)); + Angle lowerLimit = + hood.getMotorController().getConfig().getMechanismLowerLimit().orElse(Degrees.of(30)); + if (newAngle.gt(lowerLimit)) { + io.setHoodAngle(newAngle); + } + }); + } + /** Decreases the flywheel speed by 10 RPM and ensures it does not go below 0 RPM */ + public Command decreaseFlywheelSpeedCommand() { + return Commands.runOnce( + () -> { + AngularVelocity newSpeed = inputs.flyWheelSpeedActual.minus(RPM.of(10)); + if (newSpeed.gt(RPM.of(0))) { + io.setFlyWheelVelocity(newSpeed); + } + }); + } + /** Increases the flywheel speed by 10 RPM up to the configured upper soft limit */ + public Command increaseFlywheelSpeedCommand() { + return Commands.runOnce( + () -> { + AngularVelocity newSpeed = inputs.flyWheelSpeedActual.plus(RPM.of(10)); + AngularVelocity upperLimit = io.getFlywheelUpperLimit(); + if (newSpeed.lt(upperLimit)) { + io.setFlyWheelVelocity(newSpeed); + } + }); + } + + public void runHoodDown() { + io.runHoodDown(); + } + + public void stopHood() { + io.stopHood(); + } + /** Decreases the turret angle by 10 degrees and ensures it does not go below -90 */ + public Command decreaseTurretAngleCommand() { + return Commands.runOnce( + () -> { + Angle newAngle = inputs.turretAngleActual.minus(Degrees.of(1)); + if (newAngle.gt(Degrees.of(-90))) { // Assuming -90 degrees as the left limit + io.setTurretRotation(newAngle); + } + }); + } + /** Increases the turret angle by 1 degree and ensures it does not go above 90 degrees */ + public Command increaseTurretAngleCommand() { + return Commands.runOnce( + () -> { + Angle newAngle = inputs.turretAngleActual.plus(Degrees.of(1)); + if (newAngle.lt(Degrees.of(90))) { // Assuming 90 degrees as the right limit + io.setTurretRotation(newAngle); + } + }); + } + + public void zeroTurret() { + io.zeroTurret(); + } + + public boolean isTurretAtZero() { + return io.isTurretAtZero(); + } + + public Command zeroTurretCommand() { + return Commands.sequence( + Commands.runOnce( + () -> { + zeroTurret(); + zeroHood(); + }, + this), + Commands.run( + () -> { + org.frc5010.common.utils.OrchestraManager.playTone(261.63); + org.frc5010.common.subsystems.LEDStrip.changeSegmentPattern( + org.frc5010.common.config.ConfigConstants.ALL_LEDS, + org.frc5010.common.subsystems.LEDStrip.getRainbowPattern(2.0)); + }) + .withTimeout(1.5) + .ignoringDisable(true)) + .beforeStarting(() -> frc.robot.rebuilt.Rebuilt.isZeroingBurst = true) + .finallyDo( + () -> { + frc.robot.rebuilt.Rebuilt.isZeroingBurst = false; + org.frc5010.common.utils.OrchestraManager.stopTone(); + }) + .onlyIf( + () -> + edu.wpi.first.wpilibj.DriverStation.isDisabled() + && !(frc.robot.rebuilt.Rebuilt.hasEverEnabled() + && edu.wpi.first.wpilibj.DriverStation.isFMSAttached())) + .ignoringDisable(true); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIO.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIO.java new file mode 100644 index 00000000..20ef64df --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIO.java @@ -0,0 +1,196 @@ +package frc.robot.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.commands.LauncherCommands; +import frc.robot.rebuilt.subsystems.Indexer.Indexer; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.AutoLog; + +/** IO interface for the Launcher subsystem. */ +public interface LauncherIO { + + @AutoLog + public static class LauncherIOInputs { + /** Initializes the requested and current launcher states to idle */ + public LauncherCommands.LauncherState stateRequested = LauncherCommands.LauncherState.IDLE; + + public LauncherCommands.LauncherState stateCurrent = LauncherCommands.LauncherState.IDLE; + public LauncherCommands.LauncherState preTrenchState = LauncherCommands.LauncherState.IDLE; + /** + * Intializes the distance to the virtual target and desired flywheel speed to start at 0 and + * calculation validity to false + */ + public boolean isValidCalculation = false; + + public Distance distanceToVirtualTarget = Meters.of(0.0); + public AngularVelocity flyWheelSpeedDesired = RPM.of(0.0); + /** + * Intializes calculated and desired angles to 0 degrees and calculated flywheel speed to 0 RPS + */ + public AngularVelocity flyWheelSpeedCalculated = RotationsPerSecond.of(0.0); + + public Angle hoodAngleCalculated = Degrees.of(0.0); + public Angle turretAngleCalculated = Degrees.of(0.0); + public Angle hoodAngleDesired = Degrees.of(0.0); + public Angle turretAngleDesired = Degrees.of(0.0); + /** Initializes actual flywheel speed to 0 RPM and actual hood and turret angles to 0 degrees */ + public AngularVelocity flyWheelSpeedActual = RPM.of(0.0); + + public Angle hoodAngleActual = Degrees.of(0.0); + public Angle turretAngleActual = Degrees.of(0.0); + /** Decides whether the flywheel speed and turret and hood angle have reached their goals */ + public boolean flyWheelSpeedAtGoal = false; + + public boolean hoodAngleAtGoal = false; + public boolean turretAngleAtGoal = false; + /** Initializes the hood and turret angle errors to 0 and the flywheel speed error to 0 RPM */ + public AngularVelocity flyWheelSpeedError = RPM.of(0.0); + + public double hoodAngleError = 0.0; + public double turretAngleError = 0.0; + /** Intiializes the hood and turret velocities to 0 and the flywheel motor output to 0 */ + public double hoodVelocity = 0.0; + + public boolean hoodMoving = true; + + public double turretVelocity = 0.0; + public double flyWheelMotorOutput = 0.0; + + /** Kinematic feedforward for the turret (rad/s), computed via numerical differentiation. */ + public double turretFeedforwardRadPerSec = 0.0; + + /** + * Acceleration feedforward for the turret (rad/s^2), computed via numerical differentiation. + */ + public double turretFeedforwardAccelRadPerSecSq = 0.0; + + public Translation2d robotToTarget = new Translation2d(); + + public Distance targetDistance = Meters.of(0.0); + + public Angle uniqueCoverage = Degrees.of(0.0); + public boolean coverageSatisfiesRange = false; + } + + public default void updateInputs(LauncherIOInputs inputs) {} + + public void runShooter(double speed); + + public void setFlyWheelVelocity(AngularVelocity speed); + + public void setHoodAngle(Angle angle); + + public void setHoodAngleLow(); + + public void setTurretRotation(Angle angle); + + /** + * Sets the turret to the given angle while simultaneously applying kinematic feedforward velocity + * and acceleration. The feedforward values are passed directly to the underlying motor + * controller's closed-loop request. + * + * @param angle desired turret mechanism angle + * @param feedforwardRadPerSec angular velocity feedforward in rad/s (mechanism units) + * @param accelerationRadPerSecSq angular acceleration feedforward in rad/s^2 (mechanism units) + */ + public default void setTurretRotationWithFeedforward( + Angle angle, double feedforwardRadPerSec, double accelerationRadPerSecSq) { + setTurretRotation(angle); + } + + public LinearVelocity getFlyWheelExitSpeed(AngularVelocity velocity); + + public Command getHoodCharacterizationCommand(GenericSubsystem launcher); + + public Command getHoodSysIdCommand(); + + public Command getHoodSysIdCommand(GenericSubsystem launcher); + + public Command getTurretCharacterizationCommand(GenericSubsystem launcher); + + public Command getTurretSysIdCommand(); + + public Command getFlyWheelSysIdCommand(GenericSubsystem launcher); + + public Command getFlyWheelSysIdCommand(); + + public Command getTurretSysIdCommand(GenericSubsystem launcher); + + public void runHoodDown(); + + public void stopHood(); + + public Boolean isHoodStalled(); + + public void resetHoodAngle(Angle angle); + + public ShotCalculator.ShootingParameters getShootingParameters( + Supplier robotPoseSupplier, Supplier targetPositionSupplier); + + public void stopAllMotors(); + + public default void configureShotCalculator(ShotCalculator shotCalculator) {} + + public default SmartTurretController getSmartTurretController() { + return null; + } + + public default void updateSimulation(Launcher launcher, Indexer indexer) {} + + public boolean isNearTrench(); + + public Optional determineTarget(); + + public default Command getTurretQuasistaticCommand(GenericSubsystem launcher) { + return Commands.none(); + } + + public default Command getTurretDynamicCommand(GenericSubsystem launcher) { + return Commands.none(); + } + + public default Command getTurretKsMapCommand(GenericSubsystem launcher) { + return Commands.none(); + } + + public default Command getTurretTrackingTuneCommand(GenericSubsystem launcher) { + return Commands.none(); + } + + public default Command getTurretSeekingTuneCommand(GenericSubsystem launcher) { + return Commands.none(); + } + + public void zeroTurret(); + + /** + * Returns the flywheel upper soft limit from the mechanism config, or a safe fallback of 6000 RPM + * if no limit is configured. + */ + public default AngularVelocity getFlywheelUpperLimit() { + return RPM.of(6000.0); + } + + public default boolean isTurretAtZero() { + return false; + } + + public default BooleanSupplier getTurretZeroButtonSupplier() { + return () -> false; + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOReal.java new file mode 100644 index 00000000..ac7fcd60 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOReal.java @@ -0,0 +1,841 @@ +// 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.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radian; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.filter.Debouncer; +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.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.FieldConstants; +import frc.robot.rebuilt.subsystems.intake.Intake; +import frc.robot.rebuilt.util.TorqueCurrentArmSupport; +import java.util.Map; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.config.ConfigConstants; +import org.frc5010.common.drive.GenericDrivetrain; +import org.frc5010.common.motors.SystemIdentification; +import org.frc5010.common.subsystems.LEDStrip; +import org.frc5010.common.utils.geometry.AllianceFlipUtil; +import org.frc5010.common.vision.AprilTags; +import org.littletonrobotics.junction.Logger; +import yams.mechanisms.config.SensorConfig; +import yams.mechanisms.positional.Arm; +import yams.mechanisms.positional.Pivot; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.simulation.Sensor; +import yams.units.EasyCRT; +import yams.units.EasyCRTConfig; + +/** Add your docs here. */ +public class LauncherIOReal implements LauncherIO { // -0.030679615757712823 + protected static final Angle HARD_STOP = Radians.of(2.9437091319525455); + protected static final double encoder40Offset = -0.46923828125; + protected static final double encoder36Offset = 0.129638671875; + private static final double MIN_DYNAMIC_TURRET_TOLERANCE_DEGREES = 2.0; + private static final double MIN_DYNAMIC_TURRET_SHUTTLE_TOLERANCE_DEGREES = 4.0; + private static final double MAX_DYNAMIC_TURRET_SHUTTLE_TOLERANCE_DEGREES = 20.0; + + protected Map devices; + protected Pivot turret; + protected Arm hood; + private TalonFX hoodTalonFX; + private final MotionMagicTorqueCurrentFOC hoodMotionMagicRequest = + new MotionMagicTorqueCurrentFOC(0).withSlot(0); + private Angle hoodAngleSetpoint = Degrees.of(0.0); + private TorqueCurrentArmSupport.Config hoodTorqueCurrentConfig = + TorqueCurrentArmSupport.Config.defaults(true); + protected GenericDrivetrain drivetrain; + protected FlyWheel flyWheel; + protected CANcoder crtEncoder40; + protected CANcoder crtEncoder36; + protected final Sensor crtSensor40; + protected final Sensor crtSensor36; + protected EasyCRT easyCrtSolver; + /** Initializes the launcher hardware, encoders, simulated sensors, and angle solver */ + EasyCRTConfig easyCrt; + + private enum TargetProfile { + NONE, + HUB, + SHUTTLE + } + + private boolean isNearTrench = false; + + protected Intake intake; + + protected static Translation2d robotToTurret; + private DigitalInput turretZeroButton; + + Angle turretLowLimit = Degrees.of(-90); + Angle turretHighLimit = Degrees.of(90); + Angle hoodLowLimit = Degrees.of(12); + Angle hoodHighLimit = Degrees.of(42); + + private Debouncer hoodNotMoving; + + /** 2-state turret controller: SEEKING (MotionMagic) and TRACKING (Position + FF). */ + protected SmartTurretController smartTurretController; + + /** Previous turret velocity feedforward (rad/s) for numerical acceleration computation. */ + private double previousTurretVelocityRadPerSec = 0.0; + + public LauncherIOReal(Map devices, Map subsystems) { + this.devices = devices; + drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN); + intake = (Intake) subsystems.get(Constants.INTAKE); + turret = (Pivot) devices.get("turret"); + robotToTurret = + turret + .getPivotConfig() + .getMechanismPositionConfig() + .getRelativePosition() + .get() + .toTranslation2d(); + + hoodNotMoving = new Debouncer(0.25, Debouncer.DebounceType.kRising); + + turretZeroButton = new DigitalInput(0); + + hood = (Arm) devices.get("hood"); + hoodTorqueCurrentConfig = + TorqueCurrentArmSupport.loadConfig("launcher/hood.json", true, "hood"); + hoodAngleSetpoint = hood.getAngle(); + Object rawHoodController = hood.getMotorController().getMotorController(); + if (!RobotBase.isSimulation() && rawHoodController instanceof TalonFX talonFX) { + hoodTalonFX = talonFX; + TorqueCurrentArmSupport.syncSlot0Feedforward(hood, hoodTalonFX); + } + flyWheel = (FlyWheel) devices.get("flywheel"); + + turretLowLimit = + turret.getMotorController().getConfig().getMechanismLowerLimit().orElse(turretLowLimit); + turretHighLimit = + turret.getMotorController().getConfig().getMechanismUpperLimit().orElse(turretHighLimit); + + CANBus canivoreBus = new CANBus("canivore"); + crtEncoder40 = new CANcoder(21, canivoreBus); + crtEncoder36 = new CANcoder(22, canivoreBus); + double sensor40Sim = 0.391; + double sensor36Sim = 0.274; + crtSensor40 = + new SensorConfig("CRT sensor 40") + .withField("angle", () -> crtEncoder40.getAbsolutePosition().getValueAsDouble(), 0.0) + .withSimulatedValue("angle", Seconds.of(0), Seconds.of(0.5), sensor40Sim) + .getSensor(); + crtSensor36 = + new SensorConfig("CRT sensor 36") + .withField("angle", () -> crtEncoder36.getAbsolutePosition().getValueAsDouble(), 0.0) + .withSimulatedValue("angle", Seconds.of(0), Seconds.of(0.5), sensor36Sim) + .getSensor(); + + easyCrt = + new EasyCRTConfig( + () -> Rotations.of(crtSensor40.getAsDouble("angle")), + () -> Rotations.of(crtSensor36.getAsDouble("angle"))) + .withCommonDriveGear( + /* commonRatio (mech:drive) */ 30.0, + /* driveGearTeeth */ 12, + /* encoder1Pinion */ 40, + /* encoder2Pinion */ 36) + .withAbsoluteEncoderOffsets( // -0.474609375 + Rotations.of(encoder40Offset), + Rotations.of(encoder36Offset)) // set after mechanical zero + .withMechanismRange(Degrees.of(-168), Degrees.of(173)) // -360 deg to +720 deg + .withMatchTolerance(Rotations.of(0.06)) // ~1.08 deg at encoder2 for the example ratio + .withAbsoluteEncoderInversions(true, false) + .withCrtGearRecommendationConstraints( + /* coverageMargin */ 1.2, + /* minTeeth */ 15, + /* maxTeeth */ 45, + /* maxIterations */ 30); + + easyCrtSolver = new EasyCRT(easyCrt); + // // Test Values + SmartDashboard.putNumber( + "EasyCRT/Unique Coverage", easyCrt.getUniqueCoverage().orElse(Degrees.of(0.0)).in(Degrees)); + SmartDashboard.putBoolean("EasyCRT/Coverage Satisfies Range", easyCrt.coverageSatisfiesRange()); + SmartDashboard.putNumber("EasyCRT/Enc 1", easyCrt.getAbsoluteEncoder1Angle().in(Degrees)); + SmartDashboard.putNumber( + "EasyCRT/Enc 1 Ratio", easyCrt.getEncoder1RotationsPerMechanismRotation()); + SmartDashboard.putNumber("EasyCRT/Enc 2", easyCrt.getAbsoluteEncoder2Angle().in(Degrees)); + SmartDashboard.putNumber( + "EasyCRT/Enc 2 Ratio", easyCrt.getEncoder2RotationsPerMechanismRotation()); + Angle calculatedAngle; + Optional optionalAngle = (easyCrtSolver.getAngleOptional()); + if (optionalAngle.isPresent()) { + calculatedAngle = optionalAngle.get(); + } else { + calculatedAngle = Degrees.of(0); + } + + SmartDashboard.putNumber("EasyCRT/CRT Angle", calculatedAngle.in(Degrees)); + SmartDashboard.putString("EasyCRT/CRT Status", easyCrtSolver.getLastStatus().name()); + SmartDashboard.putNumber("EasyCRT/CRT Error Rot", easyCrtSolver.getLastErrorRotations()); + turret.getMotor().setEncoderPosition(calculatedAngle); + + // Create the 2-state SmartTurretController (replaces TurretProfileController). + // Uses MotionMagicTorqueCurrentFOC for seeking and PositionTorqueCurrentFOC for tracking. + { + var turretConfig = turret.getMotorController().getConfig(); + var trapConstraints = turretConfig.getTrapezoidProfile(); + // Mechanism rot/s and rot/s^2; fallback values from turret.json maxVelocity/maxAcceleration. + double maxVelMechRotPerSec = trapConstraints.map(c -> c.maxVelocity).orElse(1080.0 / 360.0); + double maxAccelMechRotPerSecSq = + trapConstraints.map(c -> c.maxAcceleration).orElse(5080.0 / 360.0); + double lowerLimitRot = + turretConfig.getMechanismLowerLimit().orElse(Degrees.of(-150)).in(Rotations); + double upperLimitRot = + turretConfig.getMechanismUpperLimit().orElse(Degrees.of(150)).in(Rotations); + + Object rawController = turret.getMotorController().getMotorController(); + if (rawController instanceof com.ctre.phoenix6.hardware.TalonFX talonFXRaw) { + // Load feedforward from YAMS mechanism config (populated from turret.json motorSystemId). + // Turret is a Pivot so getArmFeedforward() contains the characterised kS/kV/kA in SI units. + // Fallback values match turret.json in case the YAMS FF was not set. + ArmFeedforward yamsFf = turretConfig.getArmFeedforward().orElse(null); + double kS = yamsFf != null ? yamsFf.getKs() : 12.12; + double kV = yamsFf != null ? yamsFf.getKv() : 3.06; + double kA = yamsFf != null ? yamsFf.getKa() : 2.0; + SmartTurretConfig smartConfig = + new SmartTurretConfig.Builder() + .withTalonFX(talonFXRaw) + .withYAMSController(turret.getMotorController()) + .withGearRatio(30.0) + .withMotionConstraints(maxVelMechRotPerSec, maxAccelMechRotPerSecSq) + .withSeekingPID(1050, 0, 144.886) // Initial values from turret.json + .withTrackingPID(1050, 0, 144.886) // Start same, tune separately + .withFeedforward(kS, kV, kA) + .withSeekingThreshold(Degrees.of(5).in(Rotations)) + .withHysteresisBuffer(Degrees.of(12).in(Rotations)) + .withSoftLimits(lowerLimitRot, upperLimitRot) + .build(); + + smartTurretController = new SmartTurretController(smartConfig); + // Reset controller to the CRT-solved initial position. + smartTurretController.reset(calculatedAngle.in(Rotations), 0); + } + } + + turret.min().or(turret.max()).onTrue(Commands.runOnce(() -> turret.getMotor().setDutyCycle(0))); + } + + public ShotCalculator.ShootingParameters getShootingParameters( + Supplier robotPoseSupplier, Supplier targetPositionSupplier) { + Translation2d targetPosition = targetPositionSupplier.get(); + ShotCalculator.getInstance().useShotProfile(getShotProfile(targetPosition)); + ShotCalculator.getInstance().clearShootingParameters(); + return ShotCalculator.getInstance() + .getParameters( + robotToTurret, + Rotation2d.fromDegrees(turret.getAngle().in(Degrees)), + robotPoseSupplier, + () -> targetPosition); + } + + @Override() + /** Updating launcher sensor data, calculates shot parameters, and populates input telemetry */ + public void updateInputs(LauncherIOInputs inputs) { + SmartDashboard.putNumber("EasyCRT/Encoder 40", crtSensor40.getAsDouble("angle")); + SmartDashboard.putNumber("EasyCRT/Enc 2", easyCrt.getAbsoluteEncoder2Angle().in(Degrees)); + SmartDashboard.putNumber("EasyCRT/Encoder 36", crtSensor36.getAsDouble("angle")); + SmartDashboard.putNumber("EasyCRT/Enc 1", easyCrt.getAbsoluteEncoder1Angle().in(Degrees)); + org.littletonrobotics.junction.Logger.recordOutput( + "Turret Zero Button", turretZeroButton.get()); + SmartDashboard.putNumber( + "Distance to tag 27", + drivetrain + .getPoseEstimator() + .getCurrentPose3d() + .toPose2d() + .minus(AprilTags.aprilTagFieldLayout.getTagPose(21).get().toPose2d()) + .getTranslation() + .getNorm()); + + // Angle calculatedAngle = + // easyCrtSolver.getAngleOptional().orElse(Degrees.of(0.0)); + // SmartDashboard.putNumber("CRT Angle", calculatedAngle.in(Degrees)); + // SmartDashboard.putString("CRT Status", easyCrtSolver.getLastStatus().name()); + // SmartDashboard.putNumber("CRT Error Rot", + // easyCrtSolver.getLastErrorRotations()); + + Pose2d currentPose = drivetrain.getPoseEstimator().getCurrentPose(); + Optional targetPose = FieldRegions.determineTargetPose(currentPose); + TargetProfile targetProfile = TargetProfile.NONE; + inputs.isValidCalculation = false; + SmartDashboard.putNumber("Flywheel Multiplier", ShotCalculator.getFlywheelMultiplier()); + + Translation2d SOTMOffset = new Translation2d(); + Distance distanceToVirtualTarget = Meters.of(0.0001); + + inputs.hoodMoving = + !hoodNotMoving.calculate( + hood.getMotorController().getMechanismVelocity().in(Degrees.per(Second)) < 1.0); + + if (targetPose.isPresent()) { + targetProfile = getTargetProfile(targetPose.get()); + ShotCalculator.getInstance().useShotProfile(getShotProfile(targetPose.get())); + ShotCalculator.getInstance().clearShootingParameters(); + ShotCalculator.ShootingParameters params = + ShotCalculator.getInstance() + .getParameters( + robotToTurret, + Rotation2d.fromDegrees(turret.getAngle().in(Degrees)), + () -> currentPose, + () -> targetPose.get()); + if (params != null) { + inputs.isValidCalculation = params.isValid(); + inputs.hoodAngleCalculated = Radian.of(params.hoodAngle()); + inputs.turretAngleCalculated = params.turretAngle().getMeasure(); + inputs.flyWheelSpeedCalculated = + RPM.of(params.flywheelSpeed() * ShotCalculator.getFlywheelMultiplier()); + inputs.distanceToVirtualTarget = params.distanceToVirtualTarget(); + inputs.turretFeedforwardRadPerSec = params.solution().turretFeedforwardRadPerSec(); + inputs.turretFeedforwardAccelRadPerSecSq = + (inputs.turretFeedforwardRadPerSec - previousTurretVelocityRadPerSec) + / org.frc5010.common.constants.Constants.loopPeriodSecs; + previousTurretVelocityRadPerSec = inputs.turretFeedforwardRadPerSec; + + ChassisSpeeds virtualTargetOffsetparams = + params + .solution() + .finalSolverState() + .robotStateAtFire() + .velocity() + .times(-params.solution().estimatedTimeOfFlight()); + SOTMOffset = + new Translation2d( + virtualTargetOffsetparams.vxMetersPerSecond, + virtualTargetOffsetparams.vyMetersPerSecond); + distanceToVirtualTarget = params.distanceToVirtualTarget(); + } + Translation2d fieldTarget = AllianceFlipUtil.apply(targetPose.get()); + inputs.robotToTarget = fieldTarget.minus(currentPose.getTranslation()); + + inputs.targetDistance = Meters.of(inputs.robotToTarget.getDistance(new Translation2d())); + } else { + ShotCalculator.getInstance().useShotProfile(ShotCalculator.ShotProfile.NORMAL); + } + /** Reads the desired flywheel, hood, and turret setpoints */ + inputs.flyWheelSpeedDesired = + flyWheel + .getMotorController() + .getMechanismSetpointVelocity() + .map(it -> it) + .orElse(RPM.of(0.0)); + inputs.hoodAngleDesired = + hoodTalonFX != null + ? hoodAngleSetpoint + : hood.getMotorController().getMechanismPositionSetpoint().orElse(Degrees.of(0.0)); + // Read turret desired angle from the SmartTurretController's goal, not YAMS (which is + // bypassed). + inputs.turretAngleDesired = + smartTurretController != null + ? Rotations.of(smartTurretController.getGoalPositionMechRot()) + : turret.getMotorController().getMechanismPositionSetpoint().orElse(Degrees.of(0.0)); + + double[] turretAngleToleranceDegrees = + getTurretAngleToleranceDegrees( + currentPose, + inputs.turretAngleDesired, + targetPose.orElse(null), + SOTMOffset, + distanceToVirtualTarget, + targetProfile); + SmartDashboard.putString("Launcher/Target Profile", targetProfile.name()); + Logger.recordOutput("Launcher/Lower Turret Tolerance Deg", turretAngleToleranceDegrees[0]); + Logger.recordOutput("Launcher/Upper Turret Tolerance Deg", turretAngleToleranceDegrees[1]); + + inputs.flyWheelSpeedActual = flyWheel.getSpeed(); + inputs.hoodAngleActual = hood.getAngle(); + inputs.turretAngleActual = turret.getAngle(); + + inputs.flyWheelSpeedError = inputs.flyWheelSpeedActual.minus(inputs.flyWheelSpeedDesired); + inputs.hoodAngleError = inputs.hoodAngleActual.minus(inputs.hoodAngleDesired).in(Degrees); + inputs.turretAngleError = inputs.turretAngleActual.minus(inputs.turretAngleDesired).in(Degrees); + + inputs.flyWheelSpeedAtGoal = + Math.abs(inputs.flyWheelSpeedError.in(RPM)) <= Constants.Launcher.SHOOTER_TOLERANCE_RPM; + inputs.hoodAngleAtGoal = + Math.abs(inputs.hoodAngleError) <= Constants.Launcher.HOOD_ANGLE_TOLERANCE_DEGREES; + inputs.turretAngleAtGoal = + turretAngleToleranceDegrees[0] <= inputs.turretAngleError + && inputs.turretAngleError <= turretAngleToleranceDegrees[1]; + + inputs.hoodVelocity = hood.getMotorController().getMechanismVelocity().in(Degrees.per(Second)); + inputs.turretVelocity = + turret.getMotorController().getMechanismVelocity().in(Degrees.per(Second)); + inputs.flyWheelMotorOutput = flyWheel.getMotor().getStatorCurrent().in(Amps); + isNearTrench(); + } + + /** Configuring the shot calculator with limits and constraints */ + @Override + public void configureShotCalculator(ShotCalculator shotCalculator) { + var turretConfig = turret.getMotorController().getConfig(); + + ShotCalculator.ShotTables defaultTables = ShotCalculator.createDefaultTables(); + shotCalculator.setShotTables(defaultTables); + shotCalculator.setShuttleShotTables(ShotCalculator.copyShotTables(defaultTables)); + + // Turret angular limits and aim tolerance — read directly from the YAMS config so they stay + // in sync with the soft-limit values defined in launcher/turret.json. + Rotation2d aimTolerance = + Rotation2d.fromDegrees( + turretConfig.getClosedLoopTolerance().orElse(Degrees.of(10.0)).in(Degrees)); + shotCalculator.setTurretConstraints( + Rotation2d.fromDegrees(turretConfig.getMechanismLowerLimit().get().in(Degrees)), + Rotation2d.fromDegrees(turretConfig.getMechanismUpperLimit().get().in(Degrees)), + aimTolerance); + + // Trapezoidal motion profile constraints — read from the YAMS config (populated from + // launcher/turret.json motorSystemId.maxVelocity / maxAcceleration). + // YAMS stores these in rot/s and rot/s², so multiply by 2π to get rad/s and rad/s². + var trapConstraints = turretConfig.getTrapezoidProfile(); + double maxVelRadPerSec = + trapConstraints.map(c -> c.maxVelocity * 2.0 * Math.PI).orElse(Math.toRadians(1080.0)); + double maxAccelRadPerSecSq = + trapConstraints.map(c -> c.maxAcceleration * 2.0 * Math.PI).orElse(Math.toRadians(360.0)); + shotCalculator.setTurretMotionConstraints(maxVelRadPerSec, maxAccelRadPerSecSq, 0.85); + + // Provide the turret motor's actual velocity to the shot calculator for + // velocity-aware settling time estimation. Uses encoder velocity (not profile velocity) + // to avoid a feedback loop between the solver and the controller. + if (smartTurretController != null) { + shotCalculator.setTurretVelocitySupplier(smartTurretController::getActualVelocityRadPerSec); + } + } + + @Override + public SmartTurretController getSmartTurretController() { + return smartTurretController; + } + + public void resetHoodAngle(Angle angle) { + hood.getMotor().setEncoderPosition(angle); + } + + /** Sets the flywheel motor's duty cycle */ + public void runShooter(double speed) { + flyWheel.getMotor().setDutyCycle(speed); + } + + /** Sets the flywheel motor's angular velocity */ + public void setFlyWheelVelocity(AngularVelocity speed) { + flyWheel.getMotor().setVelocity(speed); + } + + /** Sets the hood angle and overrides the requested angle if the hood is near the trench */ + public void setHoodAngle(Angle angle) { + requestHoodAngle(angle); + } + + /** Sets the low hard limit to 30 degrees and updates LED's */ + public void setHoodAngleLow() { + requestHoodAngle(hood.getArmConfig().getLowerHardLimit().orElse(Degrees.of(30))); + LEDStrip.changeSegmentPattern(ConfigConstants.ALL_LEDS, LEDStrip.getSolidPattern(Color.kGreen)); + } + + public void runHoodDown() { + hood.getMotor().setDutyCycle(-1.0); + } + + public void stopHood() { + hood.getMotor().setDutyCycle(0.0); + } + + public Boolean isHoodStalled() { + return hood.getMotor().getStatorCurrent().in(Amps) + > Constants.Launcher.HOOD_STALL_CURRENT_THRESHOLD; + } + + private void requestHoodAngle(Angle angle) { + hoodAngleSetpoint = angle; + if (hoodTalonFX != null) { + hoodTalonFX.setControl( + hoodMotionMagicRequest + .withPosition(angle.in(Rotations)) + .withFeedForward( + TorqueCurrentArmSupport.calculateGravityFeedforward( + angle, hoodTorqueCurrentConfig))); + return; + } + hood.getMotorController().setPosition(angle); + } + + /** Sets the angle of the turret via the SmartTurretController (zero feedforward). */ + public void setTurretRotation(Angle angle) { + if (angle.gt(turretHighLimit)) { + SmartDashboard.putBoolean("Launcher/Turret Limit", true); + angle = turretHighLimit; + } else if (angle.lt(turretLowLimit)) { + SmartDashboard.putBoolean("Launcher/Turret Limit", true); + angle = turretLowLimit; + } else { + SmartDashboard.putBoolean("Launcher/Turret Limit", false); + } + if (smartTurretController != null) { + smartTurretController.setTarget(angle, 0.0, 0.0); + } else { + turret.getMotorController().setPosition(angle); + } + } + + /** + * Sets the turret angle with velocity and acceleration feedforward for the SmartTurretController. + * + * @param angle desired turret mechanism angle + * @param feedforwardRadPerSec angular velocity feedforward in rad/s (mechanism units) + * @param accelerationRadPerSecSq angular acceleration feedforward in rad/s^2 (mechanism units) + */ + @Override + public void setTurretRotationWithFeedforward( + Angle angle, double feedforwardRadPerSec, double accelerationRadPerSecSq) { + if (angle.gt(turretHighLimit)) { + SmartDashboard.putBoolean("Launcher/Turret Limit", true); + angle = turretHighLimit; + } else if (angle.lt(turretLowLimit)) { + SmartDashboard.putBoolean("Launcher/Turret Limit", true); + angle = turretLowLimit; + } else { + SmartDashboard.putBoolean("Launcher/Turret Limit", false); + } + + if (smartTurretController != null) { + smartTurretController.setTarget(angle, feedforwardRadPerSec, accelerationRadPerSecSq); + } else { + // Fallback: YAMS setPosition without feedforward + turret.getMotorController().setPosition(angle); + } + } + + /** Converts the flywheel angular velocity into speed */ + public LinearVelocity getFlyWheelExitSpeed(AngularVelocity velocity) { + return MetersPerSecond.of( + flyWheel.getShooterConfig().getCircumference().in(Meters) + * Math.PI // This is a total fudge on the math, but it gives us a more realistic exit + // velocity for the flywheel speeds we are commanding + * (velocity.in(RadiansPerSecond))); + } + + /** Returns SysId command for the hood */ + public Command getHoodSysIdCommand() { + return hood.sysId(Volts.of(4), Volts.of(0.5).per(Seconds), Seconds.of(8)); + } + + /** Runs sysid for the cahracterized hood motor and stops at limits */ + public Command getHoodSysIdCommand(GenericSubsystem launcher) { + return SystemIdentification.getSysIdFullCommand( + SystemIdentification.angleSysIdRoutine(hood.getMotorController(), hood.getName(), launcher), + 5, + 3, + 3, + () -> + hood.isNear( + hood.getMotorController().getConfig().getMechanismUpperLimit().get(), + Degrees.of(10)) + .getAsBoolean(), + () -> + hood.isNear( + hood.getMotorController().getConfig().getMechanismLowerLimit().get(), + Degrees.of(10)) + .getAsBoolean(), + () -> hood.getMotor().setDutyCycle(0)); + } + + public Command getTurretSysIdCommand() { + return turret.sysId(Volts.of(4), Volts.of(0.5).per(Seconds), Seconds.of(8)); + } + + /** Characterizes the turret */ + public Command getTurretSysIdCommand(GenericSubsystem launcher) { + return SystemIdentification.getSysIdFullCommand( + SystemIdentification.angleSysIdRoutine( + turret.getMotorController(), turret.getName(), launcher), + 5, + 3.5, + 3, + () -> + turret + .isNear( + turret.getMotorController().getConfig().getMechanismUpperLimit().get(), + Degrees.of(10)) + .getAsBoolean(), + () -> + turret + .isNear( + turret.getMotorController().getConfig().getMechanismLowerLimit().get(), + Degrees.of(10)) + .getAsBoolean(), + () -> turret.getMotor().setDutyCycle(0)); + } + + /** Applies voltage and measures hood velocity to characterize the feed forward */ + public Command getHoodCharacterizationCommand(GenericSubsystem launcher) { + return SystemIdentification.feedforwardCharacterization( + launcher, + (Voltage voltage) -> hood.getMotor().setVoltage(voltage), + () -> hood.getMotorController().getMechanismVelocity().in(RadiansPerSecond)); + } + + /** Applies voltage and measures turret velocity to characterize the feedfoward */ + public Command getTurretCharacterizationCommand(GenericSubsystem launcher) { + return SystemIdentification.feedforwardCharacterization( + launcher, + (Voltage voltage) -> turret.getMotor().setVoltage(voltage), + () -> turret.getMotorController().getMechanismVelocity().in(RadiansPerSecond)); + } + + /** sets the flywheel, hood, and turret motor duty cycles to 0, which stops the motors */ + public void stopAllMotors() { + flyWheel.getMotor().setDutyCycle(0); + hood.getMotor().setDutyCycle(0); + turret.getMotor().setDutyCycle(0); + } + + public Command getFlyWheelSysIdCommand() { + return flyWheel.sysId(Volts.of(8), Volts.of(0.5).per(Seconds), Seconds.of(8)); + } + + public boolean isNearTrench() { + Pose2d current = drivetrain.getPoseEstimator().getCurrentPose(); + double currentX = current.getX(); + double currentY = current.getY(); + + return FieldRegions.isNearTrench(currentX, currentY); + } + + public Optional determineTarget() { + Pose2d current = drivetrain.getPoseEstimator().getCurrentPose(); + return FieldRegions.determineTargetPose(current); + } + + private TargetProfile getTargetProfile(Translation2d targetPose) { + Translation2d fieldTarget = AllianceFlipUtil.apply(targetPose); + Translation2d hubTarget = + AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()); + if (fieldTarget.getDistance(hubTarget) < 1e-6) { + return TargetProfile.HUB; + } + return TargetProfile.SHUTTLE; + } + + private ShotCalculator.ShotProfile getShotProfile(Translation2d targetPose) { + return getTargetProfile(targetPose) == TargetProfile.SHUTTLE + ? ShotCalculator.ShotProfile.SHUTTLE + : ShotCalculator.ShotProfile.NORMAL; + } + + private double[] getTurretAngleToleranceDegrees( + Pose2d currentPose, + Angle desiredTurretAngle, + Translation2d targetPose, + Translation2d SOTMOffset, + Distance distanceToVirtualTarget, + TargetProfile targetProfile) { + if (targetPose == null || targetProfile == TargetProfile.NONE) { + return new double[] { + -Constants.Launcher.TURRET_ANGLE_TOLERANCE_DEGREES, + Constants.Launcher.TURRET_ANGLE_TOLERANCE_DEGREES + }; + } + + Translation2d turretFieldPosition = getTurretFieldPosition(currentPose); + Rotation2d desiredFieldHeading = + currentPose.getRotation().plus(Rotation2d.fromRadians(desiredTurretAngle.in(Radians))); + + if (targetProfile == TargetProfile.HUB) { + return getHubTurretAngleToleranceDegrees( + turretFieldPosition, + desiredFieldHeading, + SOTMOffset, + Meters.of( + AllianceFlipUtil.apply(targetPose) + .minus(currentPose.getTranslation()) + .plus(SOTMOffset) + .getNorm())); + } + + return getShuttleTurretAngleToleranceDegrees( + turretFieldPosition, desiredFieldHeading, targetPose, SOTMOffset); + } + + private double[] getHubTurretAngleToleranceDegrees( + Translation2d turretFieldPosition, + Rotation2d desiredFieldHeading, + Translation2d SOTMOffset, + Distance distanceToVirtualTarget) { + Translation2d adjustedNearLeftCorner = + AllianceFlipUtil.apply(FieldConstants.Hub.nearLeftCorner).plus(SOTMOffset); + Translation2d adjustedNearRightCorner = + AllianceFlipUtil.apply(FieldConstants.Hub.nearRightCorner).plus(SOTMOffset); + Logger.recordOutput("Launcher/Adjusted Near Left Corner", adjustedNearLeftCorner); + Logger.recordOutput("Launcher/Adjusted Near Right Corner", adjustedNearRightCorner); + double toleranceDegrees = + Math.max( + Math.toDegrees( + Math.atan( + (FieldConstants.Hub.innerWidth / 2 - 0.075) + / distanceToVirtualTarget.in(Meters))), + MIN_DYNAMIC_TURRET_TOLERANCE_DEGREES); + return new double[] {-toleranceDegrees, toleranceDegrees}; + } + + private double[] getShuttleTurretAngleToleranceDegrees( + Translation2d turretFieldPosition, + Rotation2d desiredFieldHeading, + Translation2d targetPose, + Translation2d SOTMOffset) { + double allianceZoneFarX = + FieldConstants.TrenchZoneBottom.nearAlliance.getX() - 0.5 * FieldConstants.LeftTrench.depth; + Translation2d upperFieldEdge = + AllianceFlipUtil.apply(new Translation2d(allianceZoneFarX, FieldConstants.fieldWidth)); + Translation2d lowerFieldEdge = AllianceFlipUtil.apply(new Translation2d(allianceZoneFarX, 0.0)); + Translation2d upperLaneEdge = + AllianceFlipUtil.apply( + new Translation2d(allianceZoneFarX, FieldConstants.Hub.nearLeftCorner.getY())); + Translation2d lowerLaneEdge = + AllianceFlipUtil.apply( + new Translation2d(allianceZoneFarX, FieldConstants.Hub.nearRightCorner.getY())); + + if (AllianceFlipUtil.applyY(turretFieldPosition.getY()) >= FieldConstants.fieldWidth / 2.0) { + + Translation2d adjustedUpperFieldEdge = upperFieldEdge.plus(SOTMOffset); + Translation2d adjustedUpperLaneEdge = upperLaneEdge.plus(SOTMOffset); + Logger.recordOutput("Launcher/Adjusted Upper Field Edge", adjustedUpperFieldEdge); + Logger.recordOutput("Launcher/Adjusted Upper Lane Edge", adjustedUpperLaneEdge); + return getAngularMarginDegrees( + turretFieldPosition, desiredFieldHeading, adjustedUpperFieldEdge, adjustedUpperLaneEdge); + } + Translation2d adjustedLowerFieldEdge = lowerFieldEdge.plus(SOTMOffset); + Translation2d adjustedLowerLaneEdge = lowerLaneEdge.plus(SOTMOffset); + Logger.recordOutput("Launcher/Adjusted Lower Field Edge", adjustedLowerFieldEdge); + Logger.recordOutput("Launcher/Adjusted Lower Lane Edge", adjustedLowerLaneEdge); + return getAngularMarginDegrees( + turretFieldPosition, desiredFieldHeading, adjustedLowerFieldEdge, adjustedLowerLaneEdge); + } + + private double[] getAngularMarginDegrees( + Translation2d origin, + Rotation2d desiredFieldHeading, + Translation2d boundaryA, + Translation2d boundaryB) { + + double marginA = boundaryA.minus(origin).getAngle().minus(desiredFieldHeading).getDegrees(); + double marginB = boundaryB.minus(origin).getAngle().minus(desiredFieldHeading).getDegrees(); + + double lowerBound = Math.min(marginA, marginB); + double upperBound = Math.max(marginA, marginB); + lowerBound = + Math.max( + Math.min(lowerBound, -MIN_DYNAMIC_TURRET_SHUTTLE_TOLERANCE_DEGREES), + -MAX_DYNAMIC_TURRET_SHUTTLE_TOLERANCE_DEGREES); + upperBound = + Math.min( + Math.max(upperBound, MIN_DYNAMIC_TURRET_SHUTTLE_TOLERANCE_DEGREES), + MAX_DYNAMIC_TURRET_SHUTTLE_TOLERANCE_DEGREES); + + return new double[] {lowerBound, upperBound}; + } + + private Translation2d getTurretFieldPosition(Pose2d robotPose) { + return robotPose.getTranslation().plus(robotToTurret.rotateBy(robotPose.getRotation())); + } + + public Command getFlyWheelSysIdCommand(GenericSubsystem launcher) { + return SystemIdentification.getSysIdFullCommand( + SystemIdentification.rpmSysIdRoutine( + flyWheel.getMotorController(), flyWheel.getName(), launcher), + 8, + 3, + 3); + } + + @Override + public Command getTurretQuasistaticCommand(GenericSubsystem launcher) { + if (smartTurretController == null) return Commands.none(); + return new frc.robot.rebuilt.commands.TurretQuasistaticCommand(smartTurretController, launcher); + } + + @Override + public Command getTurretDynamicCommand(GenericSubsystem launcher) { + if (smartTurretController == null) return Commands.none(); + return new frc.robot.rebuilt.commands.TurretDynamicCommand(smartTurretController, launcher); + } + + @Override + public Command getTurretKsMapCommand(GenericSubsystem launcher) { + if (smartTurretController == null) return Commands.none(); + return new frc.robot.rebuilt.commands.TurretKsMapCommand( + smartTurretController, turretLowLimit, turretHighLimit, launcher); + } + + @Override + public Command getTurretTrackingTuneCommand(GenericSubsystem launcher) { + if (smartTurretController == null) return Commands.none(); + return new frc.robot.rebuilt.commands.TurretTrackingTuneCommand( + smartTurretController, launcher); + } + + @Override + public Command getTurretSeekingTuneCommand(GenericSubsystem launcher) { + if (smartTurretController == null) return Commands.none(); + return new frc.robot.rebuilt.commands.TurretSeekingTuneCommand(smartTurretController, launcher); + } + + @Override + public void zeroTurret() { + turret.getMotor().setEncoderPosition(HARD_STOP); + } + + @Override + public boolean isTurretAtZero() { + return Math.abs(turret.getAngle().in(Degrees) - HARD_STOP.in(Degrees)) < 2.0; + } + + @Override + public BooleanSupplier getTurretZeroButtonSupplier() { + return turretZeroButton::get; + } + + @Override + public AngularVelocity getFlywheelUpperLimit() { + return flyWheel.getShooterConfig().getUpperSoftLimit().orElse(RPM.of(6000.0)); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOSim.java new file mode 100644 index 00000000..c4513097 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/LauncherIOSim.java @@ -0,0 +1,118 @@ +// 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.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.rebuilt.FieldConstants; +import frc.robot.rebuilt.Rebuilt; +import frc.robot.rebuilt.commands.IndexerCommands.IndexerState; +import frc.robot.rebuilt.subsystems.Indexer.Indexer; +import frc.robot.rebuilt.subsystems.intake.IntakeIOSim; +import java.util.Map; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.Logger; +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; + +/** Add your docs here. */ +public class LauncherIOSim extends LauncherIOReal { + protected GamePieceProjectile gamePieceProjectile; + protected Map devices; + + public LauncherIOSim(Map devices, Map subsystems) { + super(devices, subsystems); + IntakeIOSim.intakeSimulation.addGamePiecesToIntake(8); + // Start with 8 gamepieces in the + // intake + } + + @Override + /** Configures the shot calculator and calculates measurements for parts of the lancher */ + public void configureShotCalculator(ShotCalculator shotCalculator) { + super.configureShotCalculator(shotCalculator); + double circumferenceMeters = flyWheel.getShooterConfig().getCircumference().in(Meters); + double wheelRadiusMeters = circumferenceMeters / (2.0 * Math.PI); + double minFlywheelRadPerSec = + flyWheel.getShooterConfig().getLowerSoftLimit().orElse(RPM.of(0.0)).in(RadiansPerSecond); + double maxFlywheelRadPerSec = + flyWheel.getShooterConfig().getUpperSoftLimit().orElse(RPM.of(5000.0)).in(RadiansPerSecond); + /** Reads the hood angle limits */ + Rotation2d minHoodAngle = + Rotation2d.fromDegrees( + hood.getMotorController().getConfig().getMechanismLowerLimit().get().in(Degrees)); + Rotation2d maxHoodAngle = + Rotation2d.fromDegrees( + hood.getMotorController().getConfig().getMechanismUpperLimit().get().in(Degrees)); + Rotation2d hoodStep = Rotation2d.fromDegrees(0.5); + + double launchHeight = flyWheel.getRelativeMechanismPosition().getZ(); + double targetHeight = FieldConstants.Hub.height; + /** Creates ballistic configuration for the shot calculator */ + ShotCalculator.BallisticConfig config = + new ShotCalculator.BallisticConfig( + 1.0, + 6.0, + 0.1, + minHoodAngle, + maxHoodAngle, + hoodStep, + minFlywheelRadPerSec, + maxFlywheelRadPerSec, + wheelRadiusMeters, + launchHeight, + targetHeight, + 0.0, + 9.80665, + Math.toRadians(90.0)); + + shotCalculator.setBallisticConfig(config); + ShotCalculator.ShotTables simTables = ShotCalculator.createBallisticTables(config); + shotCalculator.setShotTables(simTables); + shotCalculator.setShuttleShotTables(ShotCalculator.copyShotTables(simTables)); + } + + @Override + public void updateSimulation(Launcher launcher, Indexer indexer) { + int amount = IntakeIOSim.intakeSimulation.getGamePiecesAmount(); + // Update simulated mechanism states here + // We should simulate a shot rate of about 10-15 gamepieces per second + // Every other time this is called, determine a randome number and if > 0.5, shoot a gamepiece. + // This would mean we try to shoot 25 times per second, and on average shoot about 12-13 + // gamepieces per second. + if (Math.random() > 0.5 && amount > 0) { + if ((indexer.isCurrent(IndexerState.FEED) && launcher.isShooting()) + || (indexer.isCurrent(IndexerState.FORCE))) { + if (IntakeIOSim.intakeSimulation.obtainGamePieceFromIntake()) { + Pose2d worldPose = Rebuilt.drivetrain.getPoseEstimator().getCurrentPose(); + gamePieceProjectile = + new RebuiltFuelOnFly( + worldPose.getTranslation(), + flyWheel.getRelativeMechanismPosition().toTranslation2d(), + Rebuilt.drivetrain.getFieldVelocity(), + Rotation2d.fromDegrees( + worldPose.getRotation().getMeasure().plus(turret.getAngle()).in(Degrees)), + flyWheel.getRelativeMechanismPosition().getMeasureZ(), + getFlyWheelExitSpeed(flyWheel.getSpeed()), + Degrees.of(90.0).minus(hood.getAngle())) + .withProjectileTrajectoryDisplayCallBack( + (pose3ds) -> { + Logger.recordOutput( + "Launcher/GamePieceTrajectory", pose3ds.toArray(Pose3d[]::new)); + }); + SimulatedArena.getInstance().addGamePieceProjectile(gamePieceProjectile); + // Create a new gamepiece on-the-fly and add it to the field simulation + } + } + } + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/ShotCalculator.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/ShotCalculator.java new file mode 100644 index 00000000..9533d4cc --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/ShotCalculator.java @@ -0,0 +1,712 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.filter.LinearFilter; +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.geometry.Twist2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Distance; +import frc.robot.rebuilt.Rebuilt; +import frc.robot.rebuilt.subsystems.Launcher.TurretControlPhysics.AimingSolution; +import java.util.Map; +import java.util.TreeMap; +import java.util.function.BiFunction; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import lombok.experimental.ExtensionMethod; +import org.frc5010.common.constants.Constants; +import org.frc5010.common.utils.geometry.AllianceFlipUtil; +import org.frc5010.common.utils.geometry.GeomUtil; +import org.littletonrobotics.junction.Logger; + +@ExtensionMethod({GeomUtil.class}) +/** Calculates the turret and hood angle, and the flywheel speed for shooting */ +public class ShotCalculator { + private static ShotCalculator instance; + + private final LinearFilter turretAngleFilter = + LinearFilter.movingAverage((int) (0.1 / Constants.loopPeriodSecs)); + private final LinearFilter hoodAngleFilter = + LinearFilter.movingAverage((int) (0.1 / Constants.loopPeriodSecs)); + + private Rotation2d lastTurretAngle; + private double lastHoodAngle; + private Rotation2d turretAngle; + private double hoodAngle = Double.NaN; + private double turretVelocity; + private double hoodVelocity; + private TurretControlPhysics turretControlPhysics; + private Translation2d cachedTurretOffset; + private Rotation2d minTurretAngle = Rotation2d.fromDegrees(-165.0); + private Rotation2d maxTurretAngle = Rotation2d.fromDegrees(165.0); + private Rotation2d feedforwardPaddingAngle = Rotation2d.fromDegrees(10.0); + private double settlingGain = 0.00; + // Default turret motion constraints (overridden via setTurretMotionConstraints). + // These represent the practical maximum velocity (360 °/s) and acceleration (720 °/s²) + // until real SysId values are provided. + private double turretMaxVelocityRadPerSec = Math.toRadians(360.0); + private double turretMaxAccelRadPerSecSq = Math.toRadians(720.0); + private BiFunction settlingTimeFunction = + TurretControlPhysics.velocityAwareSettlingTimeFunction( + turretMaxVelocityRadPerSec, turretMaxAccelRadPerSecSq); + private DoubleSupplier turretVelocitySupplier = () -> 0.0; + private final String targetName = "Target"; + private final String lookAhead = "Lookahead"; + private final String virtualTarget = "VirtualTarget"; + private final String turret = "Turret"; + + public enum ShotProfile { + NORMAL, + SHUTTLE + } + + public static ShotCalculator getInstance() { + if (instance == null) instance = new ShotCalculator(); + return instance; + } + /** Stores calculated shooting parameters */ + public record ShootingParameters( + boolean isValid, + Rotation2d turretAngle, + double turretVelocity, + double hoodAngle, + double hoodVelocity, + double flywheelSpeed, + Distance distanceToVirtualTarget, + AimingSolution solution) {} + + // Cache parameters + private ShootingParameters latestParameters = null; + public static double flywheelMultiplier = 1.05; + + private static ShotTables normalShotTables = createDefaultTables(); + private static ShotTables shuttleShotTables = copyShotTables(normalShotTables); + private ShotProfile activeShotProfile = ShotProfile.NORMAL; + + private static double minDistance; + private static double maxDistance; + private static double phaseDelay; + private static final InterpolatingTreeMap shotHoodAngleMap = + new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); + private static final InterpolatingDoubleTreeMap shotFlywheelSpeedMap = + new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap timeOfFlightMap = + new InterpolatingDoubleTreeMap(); + + public record ShotTables( + Map hoodAngles, + Map flywheelSpeeds, + Map timeOfFlightSeconds, + double minDistanceMeters, + double maxDistanceMeters, + double phaseDelaySeconds) { + /** returns a copy of ShotTables with updated phase delay */ + public ShotTables withPhaseDelaySeconds(double newPhaseDelaySeconds) { + return new ShotTables( + hoodAngles, + flywheelSpeeds, + timeOfFlightSeconds, + minDistanceMeters, + maxDistanceMeters, + newPhaseDelaySeconds); + } + } + + public static void incrementFlywheelMultiplier(double amount) { + ShotCalculator.flywheelMultiplier += amount; + } + + public static double getFlywheelMultiplier() { + return ShotCalculator.flywheelMultiplier; + } + /** Stores configuration values for generating ballistic shot tables */ + public record BallisticConfig( + double minDistanceMeters, + double maxDistanceMeters, + double distanceStepMeters, + Rotation2d minHoodAngle, + Rotation2d maxHoodAngle, + Rotation2d hoodAngleStep, + double minFlywheelRadPerSec, + double maxFlywheelRadPerSec, + double wheelRadiusMeters, + double launchHeightMeters, + double targetHeightMeters, + double phaseDelaySeconds, + double gravityMetersPerSecondSquared, + double hoodAngleReferenceRadians) {} + + static { + applyShotTables(normalShotTables); + } + + private static Rotation2d legacyHoodAngle(double legacyAngleDegrees) { + return Rotation2d.fromDegrees( + frc.robot.rebuilt.Constants.Launcher.offsetLegacyHoodAngleDegrees(legacyAngleDegrees)); + } + + /** Creates default hood angle, flywheel speeds, and time of light tables */ + public static ShotTables createDefaultTables() { + return new ShotTables( + Map.ofEntries( + Map.entry(1.4156, legacyHoodAngle(33.00)), + Map.entry(2.0796, legacyHoodAngle(35.03)), + Map.entry(2.3645, legacyHoodAngle(36.91)), + Map.entry(2.7649, legacyHoodAngle(39.13)), + Map.entry(3.0481, legacyHoodAngle(41.87)), + Map.entry(3.2195, legacyHoodAngle(43.25)), + Map.entry(3.5309, legacyHoodAngle(44.69)), + Map.entry(3.7474, legacyHoodAngle(45.55)), + Map.entry(3.9269, legacyHoodAngle(46.17)), + Map.entry(4.3173, legacyHoodAngle(48.64)), + Map.entry(4.5403, legacyHoodAngle(49.80)), + Map.entry(4.8099, legacyHoodAngle(50.44)), + Map.entry(5.2494, legacyHoodAngle(51.14)), + Map.entry(5.2859, legacyHoodAngle(51.20)), + Map.entry(5.7789, legacyHoodAngle(51.55)), + Map.entry(6.3521, legacyHoodAngle(53.04)), + Map.entry(10.9907, legacyHoodAngle(51.84)), + Map.entry(13.0240, legacyHoodAngle(55.00))), + Map.ofEntries( + Map.entry(1.4156, 88.00), + Map.entry(2.0796, 88.41), + Map.entry(2.3645, 94.58), + Map.entry(2.7649, 96.83), + Map.entry(3.0481, 98.25), + Map.entry(3.2195, 99.25), + Map.entry(3.5309, 102.69), + Map.entry(3.7474, 104.64), + Map.entry(3.9269, 106.00), + Map.entry(4.3173, 108.00), + Map.entry(4.5403, 110.00), + Map.entry(4.8099, 112.18), + Map.entry(5.2494, 115.97), + Map.entry(5.2859, 114.77), + Map.entry(5.7789, 119.21), + Map.entry(6.3521, 125.75), + Map.entry(10.9907, 156.09), + Map.entry(13.0240, 173.00)), + Map.ofEntries( + Map.entry(1.5090, 0.8768), + Map.entry(1.7908, 0.8932), + Map.entry(2.8011, 0.9517), + Map.entry(2.9721, 0.9616), + Map.entry(3.6098, 0.9985), + Map.entry(3.6990, 1.0037), + Map.entry(4.1380, 1.0291), + Map.entry(4.4120, 1.0450), + Map.entry(5.0046, 1.0793), + Map.entry(5.3802, 1.1011), + Map.entry(5.4947, 1.1077), + Map.entry(6.7420, 1.1800)), + 0.7, + 100.0, + 0.03); + } + + public static ShotTables copyShotTables(ShotTables source) { + if (source == null) { + return createDefaultTables(); + } + + return new ShotTables( + new TreeMap<>(source.hoodAngles()), + new TreeMap<>(source.flywheelSpeeds()), + new TreeMap<>(source.timeOfFlightSeconds()), + source.minDistanceMeters(), + source.maxDistanceMeters(), + source.phaseDelaySeconds()); + } + + public static ShotTables createBallisticTables(BallisticConfig config) { + if (config == null || config.wheelRadiusMeters() <= 0.0) { + return createDefaultTables(); + } + + Map hoodAngles = new TreeMap<>(); + Map flywheelSpeeds = new TreeMap<>(); + Map timeOfFlight = new TreeMap<>(); + + double minValid = Double.POSITIVE_INFINITY; + double maxValid = 0.0; + double minDistance = config.minDistanceMeters(); + double maxDistance = config.maxDistanceMeters(); + double distanceStep = Math.max(config.distanceStepMeters(), 0.05); + double angleStep = Math.max(config.hoodAngleStep().getRadians(), Math.toRadians(0.25)); + double hoodReference = config.hoodAngleReferenceRadians(); + double minAngle = hoodReference - config.maxHoodAngle().getRadians(); + double maxAngle = hoodReference - config.minHoodAngle().getRadians(); + double gravity = config.gravityMetersPerSecondSquared(); + double heightDelta = config.targetHeightMeters() - config.launchHeightMeters(); + + for (double distance = minDistance; distance <= maxDistance + 1e-6; distance += distanceStep) { + BallisticSolution solution = + solveBallistic( + distance, + heightDelta, + gravity, + minAngle, + maxAngle, + angleStep, + config.wheelRadiusMeters(), + config.minFlywheelRadPerSec(), + config.maxFlywheelRadPerSec()); + if (solution == null) { + continue; + } + hoodAngles.put(distance, Rotation2d.fromRadians(hoodReference - solution.angleRadians())); + flywheelSpeeds.put(distance, solution.flywheelRadPerSec()); + timeOfFlight.put(distance, solution.timeOfFlightSeconds()); + minValid = Math.min(minValid, distance); + maxValid = Math.max(maxValid, distance); + } + + if (hoodAngles.isEmpty()) { + return createDefaultTables(); + } + + double minRange = Double.isFinite(minValid) ? minValid : minDistance; + double maxRange = maxValid > 0.0 ? maxValid : maxDistance; + + return new ShotTables( + hoodAngles, flywheelSpeeds, timeOfFlight, minRange, maxRange, config.phaseDelaySeconds()); + } + + private record BallisticSolution( + double angleRadians, double flywheelRadPerSec, double timeOfFlightSeconds) {} + + private static BallisticSolution solveBallistic( + double distanceMeters, + double heightDeltaMeters, + double gravityMetersPerSecondSquared, + double minAngleRadians, + double maxAngleRadians, + double angleStepRadians, + double wheelRadiusMeters, + double minFlywheelRadPerSec, + double maxFlywheelRadPerSec) { + double bestFlywheel = Double.POSITIVE_INFINITY; + double bestAngle = 0.0; + double bestTime = 0.0; + + for (double angle = minAngleRadians; + angle <= maxAngleRadians + 1e-6; + angle += angleStepRadians) { + double cos = Math.cos(angle); + double tan = Math.tan(angle); + double denominator = distanceMeters * tan - heightDeltaMeters; + if (denominator <= 0.0 || Math.abs(cos) < 1e-6) { + continue; + } + + double velocitySquared = + gravityMetersPerSecondSquared + * distanceMeters + * distanceMeters + / (2.0 * cos * cos * denominator); + if (velocitySquared <= 0.0) { + continue; + } + + double velocity = Math.sqrt(velocitySquared); + double flywheelRadPerSec = velocity / wheelRadiusMeters; + if (flywheelRadPerSec < minFlywheelRadPerSec || flywheelRadPerSec > maxFlywheelRadPerSec) { + continue; + } + + if (flywheelRadPerSec < bestFlywheel) { + bestFlywheel = flywheelRadPerSec; + bestAngle = angle; + bestTime = distanceMeters / (velocity * cos); + } + } + + if (!Double.isFinite(bestFlywheel)) { + return null; + } + + return new BallisticSolution(bestAngle, bestFlywheel, bestTime); + } + + /** + * Get the interpolated hood angle (degrees) from the current lookup table for a given distance. + * + * @param distanceMeters distance to target in meters + * @return hood angle in degrees from the lookup table, or NaN if unavailable + */ + public double getLookupHoodAngleDegrees(double distanceMeters) { + Rotation2d value = shotHoodAngleMap.get(distanceMeters); + return value != null ? value.getDegrees() : Double.NaN; + } + + public boolean hasValidShot() { + if (latestParameters != null) { + return latestParameters.solution.isPossible(); + } + return false; + } + + /** + * Get the interpolated flywheel speed from the current lookup table for a given distance. + * + * @param distanceMeters distance to target in meters + * @return flywheel speed from the lookup table, or NaN if unavailable + */ + public double getLookupFlywheelSpeed(double distanceMeters) { + Double value = shotFlywheelSpeedMap.get(distanceMeters); + return value != null ? value : Double.NaN; + } + + /** + * Compute a ballistic guess for the given distance using the stored ballistic config. Returns + * null if no ballistic config has been provided. + * + * @param distanceMeters distance to target in meters + * @return a double[] of {hoodAngleDegrees, flywheelSpeed, timeOfFlightSeconds}, or null + */ + public double[] getBallisticGuess(double distanceMeters) { + if (ballisticConfig == null) { + return null; + } + double hoodReference = ballisticConfig.hoodAngleReferenceRadians(); + double minAngle = hoodReference - ballisticConfig.maxHoodAngle().getRadians(); + double maxAngle = hoodReference - ballisticConfig.minHoodAngle().getRadians(); + double angleStep = Math.max(ballisticConfig.hoodAngleStep().getRadians(), Math.toRadians(0.25)); + double heightDelta = + ballisticConfig.targetHeightMeters() - ballisticConfig.launchHeightMeters(); + BallisticSolution solution = + solveBallistic( + distanceMeters, + heightDelta, + ballisticConfig.gravityMetersPerSecondSquared(), + minAngle, + maxAngle, + angleStep, + ballisticConfig.wheelRadiusMeters(), + ballisticConfig.minFlywheelRadPerSec(), + ballisticConfig.maxFlywheelRadPerSec()); + if (solution == null) { + return null; + } + double hoodAngleDegrees = Math.toDegrees(hoodReference - solution.angleRadians()); + return new double[] { + hoodAngleDegrees, solution.flywheelRadPerSec(), solution.timeOfFlightSeconds() + }; + } + + /** + * Add or update a single data point in the live lookup tables. + * + * @param distanceMeters the distance key + * @param hoodAngleDegrees hood angle in degrees + * @param flywheelSpeed flywheel speed value + * @param timeOfFlightSeconds estimated time-of-flight in seconds + */ + public void addDataPoint( + double distanceMeters, + double hoodAngleDegrees, + double flywheelSpeed, + double timeOfFlightSeconds) { + shotHoodAngleMap.put(distanceMeters, Rotation2d.fromDegrees(hoodAngleDegrees)); + shotFlywheelSpeedMap.put(distanceMeters, flywheelSpeed); + timeOfFlightMap.put(distanceMeters, timeOfFlightSeconds); + if (distanceMeters < minDistance) { + minDistance = distanceMeters; + } + if (distanceMeters > maxDistance) { + maxDistance = distanceMeters; + } + latestParameters = null; + turretControlPhysics = null; + } + + /** Store a ballistic config so that ballistic guesses can be computed on demand. */ + private BallisticConfig ballisticConfig; + + public void setBallisticConfig(BallisticConfig config) { + this.ballisticConfig = config; + } + + public BallisticConfig getBallisticConfig() { + return ballisticConfig; + } + + public void setShotTables(ShotTables tables) { + normalShotTables = tables != null ? tables : createDefaultTables(); + if (activeShotProfile == ShotProfile.NORMAL) { + applyShotTables(normalShotTables); + } + latestParameters = null; + turretControlPhysics = null; + } + + public void setShuttleShotTables(ShotTables tables) { + shuttleShotTables = tables != null ? tables : copyShotTables(normalShotTables); + if (activeShotProfile == ShotProfile.SHUTTLE) { + applyShotTables(shuttleShotTables); + } + latestParameters = null; + turretControlPhysics = null; + } + + public void useShotProfile(ShotProfile shotProfile) { + ShotProfile requestedProfile = shotProfile != null ? shotProfile : ShotProfile.NORMAL; + if (activeShotProfile == requestedProfile) { + return; + } + + activeShotProfile = requestedProfile; + applyShotTables( + activeShotProfile == ShotProfile.SHUTTLE ? shuttleShotTables : normalShotTables); + latestParameters = null; + turretControlPhysics = null; + } + + private static void applyShotTables(ShotTables tables) { + if (tables == null) { + return; + } + shotHoodAngleMap.clear(); + tables.hoodAngles().forEach(shotHoodAngleMap::put); + shotFlywheelSpeedMap.clear(); + tables.flywheelSpeeds().forEach(shotFlywheelSpeedMap::put); + timeOfFlightMap.clear(); + tables.timeOfFlightSeconds().forEach(timeOfFlightMap::put); + minDistance = tables.minDistanceMeters(); + maxDistance = tables.maxDistanceMeters(); + phaseDelay = tables.phaseDelaySeconds(); + } + + public void setTurretConstraints( + Rotation2d minAngle, Rotation2d maxAngle, Rotation2d paddingAngle) { + if (minAngle != null) { + minTurretAngle = minAngle; + } + if (maxAngle != null) { + maxTurretAngle = maxAngle; + } + if (paddingAngle != null) { + feedforwardPaddingAngle = paddingAngle; + } + turretControlPhysics = null; + } + + public void setSettlingTimeFunction( + BiFunction function, double newSettlingGain) { + if (function != null) { + settlingTimeFunction = function; + } + settlingGain = newSettlingGain; + turretControlPhysics = null; + } + + /** + * Configures the turret settling-time function using the closed-form trapezoidal motion profile. + * + *

This replaces any previously supplied {@link #setSettlingTimeFunction} with the analytical + * result derived from the given peak velocity and acceleration. Call this once during robot init + * after measuring the turret's actual motion profile constraints via SysId. + * + * @param maxVelocityRadPerSec peak turret velocity in rad/s + * @param maxAccelRadPerSecSq peak turret acceleration in rad/s² + * @param newSettlingGain multiplier applied to the computed time (use ≤ 1.0 to avoid oscillation) + */ + public void setTurretMotionConstraints( + double maxVelocityRadPerSec, double maxAccelRadPerSecSq, double newSettlingGain) { + turretMaxVelocityRadPerSec = maxVelocityRadPerSec; + turretMaxAccelRadPerSecSq = maxAccelRadPerSecSq; + settlingTimeFunction = + TurretControlPhysics.velocityAwareSettlingTimeFunction( + turretMaxVelocityRadPerSec, turretMaxAccelRadPerSecSq); + settlingGain = newSettlingGain; + turretControlPhysics = null; + } + + /** + * Sets the supplier for the current turret angular velocity. This is used by the velocity-aware + * settling time function to account for turret momentum when predicting time-to-arrival. + * + * @param supplier a {@link DoubleSupplier} returning the current turret velocity in rad/s + */ + public void setTurretVelocitySupplier(DoubleSupplier supplier) { + if (supplier != null) { + turretVelocitySupplier = supplier; + } + } + + public ShootingParameters getParameters( + Translation2d turretRelativePosition, + Rotation2d turretRelativeAngle, + Supplier robotPoseSupplier, + Supplier targetPositionSupplier) { + if (latestParameters != null) { + return latestParameters; + } + + // Snapshot current pose, field-relative velocity, and field-relative acceleration. + // Field velocity is used for linear extrapolation: x += vx*dt, y += vy*dt, heading += omega*dt. + // This matches the actual drivetrain behavior since the drive code already accounts for + // curvature (discretize) when commanding inputs to drive straight in field frame. + Pose2d estimatedPose = robotPoseSupplier.get(); + ChassisSpeeds fieldVelocity = Rebuilt.drivetrain.getFieldVelocity(); + ChassisSpeeds fieldAcceleration = Rebuilt.drivetrain.getFieldAcceleration(); + + // Apply phase delay using linear field-frame extrapolation. + Pose2d phaseDelayedPose = linearExtrapolatePose(estimatedPose, fieldVelocity, phaseDelay); + + Translation2d target = AllianceFlipUtil.apply(targetPositionSupplier.get()); + Pose2d turretPosition = + phaseDelayedPose.transformBy( + new Transform2d( + turretRelativePosition.getMeasureX(), + turretRelativePosition.getMeasureY(), + turretRelativeAngle)); + + TurretControlPhysics physics = getTurretControlPhysics(turretRelativePosition); + double currentTurretVelocityRadPerSec = turretVelocitySupplier.getAsDouble(); + TurretControlPhysics.AimingSolution solution = + physics.solve( + target, + turretRelativeAngle, + currentTurretVelocityRadPerSec, + (timeSinceStartSeconds, lookaheadSeconds) -> { + // Linear field-frame pose extrapolation: no twist, just straight-line translation + // in the field frame plus proportional heading change. + Pose2d predictedPose = + linearExtrapolatePose(phaseDelayedPose, fieldVelocity, lookaheadSeconds); + return new TurretControlPhysics.RobotState( + predictedPose, fieldVelocity, fieldAcceleration); + }); + + double distanceToVirtualTarget = solution.effectiveDistanceMeters(); + Rotation2d hoodSetpoint = shotHoodAngleMap.get(distanceToVirtualTarget); + Double flywheelSpeed = shotFlywheelSpeedMap.get(distanceToVirtualTarget); + + turretAngle = solution.turretLocalHeading(); + // When the distance is outside the shot table range, keep the last valid hood angle rather + // than commanding 0 radians (0°), which is below the physical lower hard stop (~12°) and + // could damage the mechanism. + if (hoodSetpoint != null) { + hoodAngle = hoodSetpoint.getRadians(); + } else if (Double.isNaN(lastHoodAngle)) { + // No previous valid angle and no table entry — leave hoodAngle as NaN so callers can + // detect the invalid state rather than silently driving to 0°. + } + // else: keep hoodAngle at its last valid value. + if (lastTurretAngle == null) lastTurretAngle = turretAngle; + if (Double.isNaN(lastHoodAngle)) lastHoodAngle = hoodAngle; + turretVelocity = + turretAngleFilter.calculate( + turretAngle.minus(lastTurretAngle).getRadians() / Constants.loopPeriodSecs); + hoodVelocity = + hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.loopPeriodSecs); + lastTurretAngle = turretAngle; + lastHoodAngle = hoodAngle; + latestParameters = + new ShootingParameters( + solution.isPossible(), + turretAngle, + turretVelocity, + hoodAngle, + hoodVelocity, + flywheelSpeed != null ? flywheelSpeed : 0.0, + Meters.of(distanceToVirtualTarget), + solution); + + // Log calculated values + Logger.recordOutput("ShotCalculator/AimingStatus", solution.status().toString()); + Logger.recordOutput( + "ShotCalculator/TurretToTargetDistance", solution.effectiveDistanceMeters()); + Logger.recordOutput( + "ShotCalculator/VirtualTargetFieldPosition", + new Pose2d(solution.finalSolverState().virtualTargetFieldPos(), turretAngle)); + Logger.recordOutput("ShotCalculator/FieldVelocity", fieldVelocity); + Logger.recordOutput("ShotCalculator/FieldAcceleration", fieldAcceleration); + + Rebuilt.drivetrain + .getField2d() + .getObject(targetName) + .setPose(new Pose2d(target, target.getAngle())); + // Lookahead visualisation uses the same linear extrapolation + Pose2d lookaheadRobotPose = + linearExtrapolatePose(phaseDelayedPose, fieldVelocity, solution.estimatedTimeOfFlight()); + Pose2d lookaheadTurretPose = + lookaheadRobotPose.transformBy( + new Transform2d( + turretRelativePosition.getMeasureX(), + turretRelativePosition.getMeasureY(), + turretRelativeAngle)); + Rebuilt.drivetrain.getField2d().getObject(lookAhead).setPose(lookaheadTurretPose); + Pose2d virtualTargetPose = new Pose2d(solution.virtualTargetFieldPos(), turretAngle); + Rebuilt.drivetrain.getField2d().getObject(virtualTarget).setPose(virtualTargetPose); + Rebuilt.drivetrain.getField2d().getObject(turret).setPose(turretPosition); + + return latestParameters; + } + + /** + * Extrapolates a robot pose forward in time using linear field-frame velocity components. + * + *

The robot's field-frame position advances by {@code vx * dt} and {@code vy * dt}. Heading + * advances by {@code omega * dt}. This avoids the curvature error introduced by {@link + * Pose2d#exp(Twist2d)} because the drive code already compensates for curvature when generating + * robot-relative motor commands (via {@code ChassisSpeeds.discretize}). + */ + private static Pose2d linearExtrapolatePose( + Pose2d currentPose, ChassisSpeeds fieldVelocity, double dt) { + Translation2d newTranslation = + currentPose + .getTranslation() + .plus( + new Translation2d( + fieldVelocity.vxMetersPerSecond * dt, fieldVelocity.vyMetersPerSecond * dt)); + Rotation2d newRotation = + currentPose + .getRotation() + .plus(Rotation2d.fromRadians(fieldVelocity.omegaRadiansPerSecond * dt)); + return new Pose2d(newTranslation, newRotation); + } + + public void clearShootingParameters() { + latestParameters = null; + } + + private TurretControlPhysics getTurretControlPhysics(Translation2d turretOffset) { + if (turretControlPhysics == null + || cachedTurretOffset == null + || !cachedTurretOffset.equals(turretOffset)) { + cachedTurretOffset = turretOffset; + turretControlPhysics = + new TurretControlPhysics( + turretOffset, + minTurretAngle, + maxTurretAngle, + feedforwardPaddingAngle, + settlingGain, + this::getTimeOfFlightSeconds, + settlingTimeFunction, + minDistance, + maxDistance); + } + return turretControlPhysics; + } + + private double getTimeOfFlightSeconds(double distanceMeters) { + Double time = timeOfFlightMap.get(distanceMeters); + return time != null ? time : 0.0; + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretConfig.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretConfig.java new file mode 100644 index 00000000..0b600948 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretConfig.java @@ -0,0 +1,322 @@ +package frc.robot.rebuilt.subsystems.Launcher; + +import com.ctre.phoenix6.hardware.TalonFX; +import yams.motorcontrollers.SmartMotorController; + +/** + * Configuration for the {@link SmartTurretController} 2-state turret control system. + * + *

All slot feedforward values (kS, kV, kA) are in Amps for use with TorqueCurrentFOC + * control. The expo profile parameters (expoKV, expoKA) are always in Volts regardless of + * control mode. All positional values are in mechanism rotations (post-gear-reduction). + */ +public class SmartTurretConfig { + + // Hardware + private final TalonFX talonFX; + /** + * YAMS SmartMotorController — its closed-loop Notifier will be stopped when SmartTurretController + * takes over. + */ + private final SmartMotorController yamsController; + + private final double gearRatio; + + // Motion constraints (mechanism rot/s and rot/s^2) + private final double maxVelocityMechRotPerSec; + private final double maxAccelMechRotPerSecSq; + + // Seeking PID (Slot0 — MotionMagicTorqueCurrentFOC) + private final double seekingKP; + private final double seekingKI; + private final double seekingKD; + + // Tracking PID (Slot1 — PositionTorqueCurrentFOC) + private final double trackingKP; + private final double trackingKI; + private final double trackingKD; + + // Feedforward in Amps (TorqueCurrentFOC units) + private final double kS; + private final double kV; + private final double kA; + + // MotionMagicExpo plant model — ALWAYS in Volts regardless of control mode. + // These define the mechanism's voltage-domain model for the expo velocity profile. + private final double expoKV; // V/(mechanism rot/s) + private final double expoKA; // V/(mechanism rot/s^2) + + // State transition thresholds (mechanism rotations) + private final double seekingThresholdRotations; + private final double hysteresisBufferRotations; + + // Soft limits (mechanism rotations) + private final double lowerLimitRotations; + private final double upperLimitRotations; + + // Peak torque current limit (Amps) + private final double peakTorqueCurrentAmps; + + // Feedforward safety padding near limits (mechanism rotations) + private final double feedforwardPaddingRotations; + + // Tracking-mode kS deadband (mechanism rotations). + // When |positionError| < this value, external kS feedforward is zeroed to prevent chattering. + private final double trackingDeadbandRotations; + + private SmartTurretConfig(Builder builder) { + this.talonFX = builder.talonFX; + this.yamsController = builder.yamsController; + this.gearRatio = builder.gearRatio; + this.maxVelocityMechRotPerSec = builder.maxVelocityMechRotPerSec; + this.maxAccelMechRotPerSecSq = builder.maxAccelMechRotPerSecSq; + this.seekingKP = builder.seekingKP; + this.seekingKI = builder.seekingKI; + this.seekingKD = builder.seekingKD; + this.trackingKP = builder.trackingKP; + this.trackingKI = builder.trackingKI; + this.trackingKD = builder.trackingKD; + this.kS = builder.kS; + this.kV = builder.kV; + this.kA = builder.kA; + this.expoKV = builder.expoKV; + this.expoKA = builder.expoKA; + this.seekingThresholdRotations = builder.seekingThresholdRotations; + this.hysteresisBufferRotations = builder.hysteresisBufferRotations; + this.lowerLimitRotations = builder.lowerLimitRotations; + this.upperLimitRotations = builder.upperLimitRotations; + this.peakTorqueCurrentAmps = builder.peakTorqueCurrentAmps; + this.feedforwardPaddingRotations = builder.feedforwardPaddingRotations; + this.trackingDeadbandRotations = builder.trackingDeadbandRotations; + } + + public TalonFX getTalonFX() { + return talonFX; + } + + /** Returns the YAMS SmartMotorController, or {@code null} if not configured. */ + public SmartMotorController getYamsController() { + return yamsController; + } + + public double getGearRatio() { + return gearRatio; + } + + public double getMaxVelocityMechRotPerSec() { + return maxVelocityMechRotPerSec; + } + + public double getMaxAccelMechRotPerSecSq() { + return maxAccelMechRotPerSecSq; + } + + public double getSeekingKP() { + return seekingKP; + } + + public double getSeekingKI() { + return seekingKI; + } + + public double getSeekingKD() { + return seekingKD; + } + + public double getTrackingKP() { + return trackingKP; + } + + public double getTrackingKI() { + return trackingKI; + } + + public double getTrackingKD() { + return trackingKD; + } + + public double getKS() { + return kS; + } + + public double getKV() { + return kV; + } + + public double getKA() { + return kA; + } + + /** MotionMagicExpo kV in V/(mechanism rot/s). Always in Volts regardless of control mode. */ + public double getExpoKV() { + return expoKV; + } + + /** MotionMagicExpo kA in V/(mechanism rot/s^2). Always in Volts regardless of control mode. */ + public double getExpoKA() { + return expoKA; + } + + public double getSeekingThresholdRotations() { + return seekingThresholdRotations; + } + + public double getHysteresisBufferRotations() { + return hysteresisBufferRotations; + } + + public double getLowerLimitRotations() { + return lowerLimitRotations; + } + + public double getUpperLimitRotations() { + return upperLimitRotations; + } + + public double getPeakTorqueCurrentAmps() { + return peakTorqueCurrentAmps; + } + + public double getFeedforwardPaddingRotations() { + return feedforwardPaddingRotations; + } + + public double getTrackingDeadbandRotations() { + return trackingDeadbandRotations; + } + + public static class Builder { + private TalonFX talonFX; + private SmartMotorController yamsController = null; + private double gearRatio = 30.0; + private double maxVelocityMechRotPerSec = 3.0; + private double maxAccelMechRotPerSecSq = 2.78; + private double seekingKP = 225; + private double seekingKI = 0; + private double seekingKD = 50; + private double trackingKP = 225; + private double trackingKI = 0; + private double trackingKD = 50; + private double kS = 10.0; + private double kV = 0.0; + private double kA = 5.0; + // Expo defaults: computed from motion constraints in build() if not explicitly set. + // -1 signals "auto-compute from maxVelocity/maxAccel". + private double expoKV = -1; + private double expoKA = -1; + private double seekingThresholdRotations = 10.0 / 360.0; + private double hysteresisBufferRotations = 3.0 / 360.0; + private double lowerLimitRotations = -150.0 / 360.0; + private double upperLimitRotations = 150.0 / 360.0; + private double peakTorqueCurrentAmps = 240.0; + private double feedforwardPaddingRotations = 10.0 / 360.0; // 10 degrees + private double trackingDeadbandRotations = 0.25 / 360.0; // 0.25 degrees + + public Builder withTalonFX(TalonFX talonFX) { + this.talonFX = talonFX; + return this; + } + + /** + * Provides the YAMS {@link SmartMotorController} so {@link SmartTurretController} can stop its + * background closed-loop Notifier thread on construction, preventing interference with our + * direct TorqueCurrentFOC control. + */ + public Builder withYAMSController(SmartMotorController yamsController) { + this.yamsController = yamsController; + return this; + } + + public Builder withGearRatio(double gearRatio) { + this.gearRatio = gearRatio; + return this; + } + + public Builder withMotionConstraints( + double maxVelocityMechRotPerSec, double maxAccelMechRotPerSecSq) { + this.maxVelocityMechRotPerSec = maxVelocityMechRotPerSec; + this.maxAccelMechRotPerSecSq = maxAccelMechRotPerSecSq; + return this; + } + + public Builder withSeekingPID(double kP, double kI, double kD) { + this.seekingKP = kP; + this.seekingKI = kI; + this.seekingKD = kD; + return this; + } + + public Builder withTrackingPID(double kP, double kI, double kD) { + this.trackingKP = kP; + this.trackingKI = kI; + this.trackingKD = kD; + return this; + } + + public Builder withFeedforward(double kS, double kV, double kA) { + this.kS = kS; + this.kV = kV; + this.kA = kA; + return this; + } + + /** + * Sets the MotionMagicExpo plant model parameters in Volts. These are always in V/rps and + * V/rps^2 regardless of control mode (even when using TorqueCurrentFOC). + * + *

If not called, defaults are auto-computed from motion constraints as {@code 12.0 / + * maxVelocity} and {@code 12.0 / maxAcceleration}. + */ + public Builder withExpoConstraints(double expoKV, double expoKA) { + this.expoKV = expoKV; + this.expoKA = expoKA; + return this; + } + + public Builder withSeekingThreshold(double seekingThresholdRotations) { + this.seekingThresholdRotations = seekingThresholdRotations; + return this; + } + + public Builder withHysteresisBuffer(double hysteresisBufferRotations) { + this.hysteresisBufferRotations = hysteresisBufferRotations; + return this; + } + + public Builder withSoftLimits(double lowerLimitRotations, double upperLimitRotations) { + this.lowerLimitRotations = lowerLimitRotations; + this.upperLimitRotations = upperLimitRotations; + return this; + } + + public Builder withPeakTorqueCurrent(double peakAmps) { + this.peakTorqueCurrentAmps = peakAmps; + return this; + } + + public Builder withFeedforwardPadding(double feedforwardPaddingRotations) { + this.feedforwardPaddingRotations = feedforwardPaddingRotations; + return this; + } + + public Builder withTrackingDeadband(double trackingDeadbandRotations) { + this.trackingDeadbandRotations = trackingDeadbandRotations; + return this; + } + + public SmartTurretConfig build() { + if (talonFX == null) { + throw new IllegalStateException("TalonFX must be set"); + } + // Auto-compute expo plant model from motion constraints if not explicitly set. + // V_max / kV_expo = max achievable velocity; V_max / kA_expo = max achievable acceleration. + if (expoKV < 0) { + expoKV = 12.0 / maxVelocityMechRotPerSec; + } + if (expoKA < 0) { + expoKA = 12.0 / maxAccelMechRotPerSecSq; + } + return new SmartTurretConfig(this); + } + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretController.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretController.java new file mode 100644 index 00000000..7fd6469b --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/SmartTurretController.java @@ -0,0 +1,407 @@ +package frc.robot.rebuilt.subsystems.Launcher; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import java.util.concurrent.atomic.AtomicReference; +import org.littletonrobotics.junction.Logger; + +/** + * 2-state turret controller that switches between SEEKING and TRACKING modes. + * + *

SEEKING: Uses {@link MotionMagicExpoTorqueCurrentFOC} (TalonFX exponential profiling) + * for smooth, physically-optimal travel over large distances. The exponential profile adapts to the + * mechanism's motor model (kV/kA) for crisp arrivals without overshoot. Velocity is capped by + * {@code MotionMagicCruiseVelocity}. + * + *

TRACKING: Uses {@link PositionTorqueCurrentFOC} with explicit velocity and acceleration + * feedforward to smoothly track a moving target at close range. + * + *

State transitions use hysteresis to prevent jitter: + * + *

    + *
  • SEEKING → TRACKING when |error| < seekingThreshold + *
  • TRACKING → SEEKING when |error| > seekingThreshold + hysteresisBuffer + *
+ * + *

The {@link #step(double)} method is called at 200 Hz via Notifier, while {@link #setTarget} is + * called from the 20 ms robot loop. Thread safety is ensured via {@link AtomicReference} over an + * immutable {@link TurretTarget} record. + */ +public class SmartTurretController { + + /** The two operational states of the turret controller. */ + public enum TurretState { + SEEKING, + TRACKING + } + + /** Immutable target data published from the 20 ms loop and consumed by the 200 Hz step loop. */ + public record TurretTarget( + double positionMechRot, double velocityRadPerSec, double accelerationRadPerSecSq) {} + + /** Signal update frequency for turret telemetry on CANivore (Hz). */ + private static final double SIGNAL_UPDATE_FREQUENCY_HZ = 250.0; + + private final TalonFX talonFX; + private final SmartTurretConfig config; + + // High-frequency status signals — cached references for refreshAll() in characterization. + private final StatusSignal positionSignal; + private final StatusSignal velocitySignal; + private final StatusSignal accelerationSignal; + private final StatusSignal torqueCurrentSignal; + + // Pre-allocated control requests (reused each cycle to avoid allocation). + private final MotionMagicExpoTorqueCurrentFOC seekingRequest = + new MotionMagicExpoTorqueCurrentFOC(0).withSlot(0); + private final PositionTorqueCurrentFOC trackingRequest = + new PositionTorqueCurrentFOC(0).withSlot(1); + + // Thread-safe target from 20 ms loop to 200 Hz step loop. + private final AtomicReference target = + new AtomicReference<>(new TurretTarget(0, 0, 0)); + + private volatile TurretState currentState = TurretState.SEEKING; + private volatile boolean enabled = false; + private volatile boolean forceDisabled = false; + + /** + * Constructs a SmartTurretController and configures the TalonFX with two PID slots, MotionMagic + * parameters, and torque current limits. + * + *

Uses read-modify-write on the TalonFX configuration to preserve YAMS-set fields (inversion, + * neutral mode, sensor ratio, etc.). + */ + public SmartTurretController(SmartTurretConfig config) { + this.config = config; + this.talonFX = config.getTalonFX(); + + // Read existing config to preserve YAMS-applied settings. + TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + talonFX.getConfigurator().refresh(fxConfig); + + // Slot0: Seeking (MotionMagicExpoTorqueCurrentFOC) + fxConfig.Slot0.kP = config.getSeekingKP(); + fxConfig.Slot0.kI = config.getSeekingKI(); + fxConfig.Slot0.kD = config.getSeekingKD(); + fxConfig.Slot0.kS = config.getKS(); + fxConfig.Slot0.kV = config.getKV(); + fxConfig.Slot0.kA = config.getKA(); + + // Slot1: Tracking (PositionTorqueCurrentFOC) + // kV and kA run at firmware frequency (1 kHz) for the best temporal alignment with PID. + // The Velocity field on the control request provides the reference for kV. + // kS is set to 0 in the slot because it is position-dependent and injected externally + // via withFeedForward() each cycle. This prevents chattering from velocity-sign-based kS + // flipping at near-zero velocity. + fxConfig.Slot1.kP = config.getTrackingKP(); + fxConfig.Slot1.kI = config.getTrackingKI(); + fxConfig.Slot1.kD = config.getTrackingKD(); + fxConfig.Slot1.kS = 0; + fxConfig.Slot1.kV = config.getKV(); + fxConfig.Slot1.kA = config.getKA(); + + // MotionMagicExpo: voltage-domain plant model for the exponential velocity profile. + // These are ALWAYS in Volts (V/rps and V/rps²) regardless of control output type. + // They define the mechanism's physical limits so the expo profile shapes appropriately. + // CruiseVelocity provides an additional hard cap on profile velocity. + fxConfig.MotionMagic.MotionMagicExpo_kV = config.getExpoKV(); + fxConfig.MotionMagic.MotionMagicExpo_kA = config.getExpoKA(); + fxConfig.MotionMagic.MotionMagicCruiseVelocity = config.getMaxVelocityMechRotPerSec(); + + // Torque current peak limits. + fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = config.getPeakTorqueCurrentAmps(); + fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = -config.getPeakTorqueCurrentAmps(); + fxConfig.CurrentLimits.StatorCurrentLimitEnable = false; + + // Firmware-level soft limits — last line of defense against hard stop contact. + fxConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + fxConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = config.getUpperLimitRotations(); + fxConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + fxConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = config.getLowerLimitRotations(); + + talonFX.getConfigurator().apply(fxConfig); + + // Cache high-frequency status signals for position, velocity, and torque current. + // Set them to 250 Hz on CANivore for smooth characterization data. + positionSignal = talonFX.getPosition(); + velocitySignal = talonFX.getVelocity(); + accelerationSignal = talonFX.getAcceleration(); + torqueCurrentSignal = talonFX.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + SIGNAL_UPDATE_FREQUENCY_HZ, + positionSignal, + velocitySignal, + accelerationSignal, + torqueCurrentSignal); + ParentDevice.optimizeBusUtilizationForAll(talonFX); + + // Stop the YAMS SmartMotorController's background closed-loop Notifier. YAMS runs its own + // 20ms update loop that continuously sends position setpoints to the TalonFX. Since + // SmartTurretController now owns this motor via direct TorqueCurrentFOC commands, that loop + // must be permanently disabled to prevent interference. + if (config.getYamsController() != null) { + config.getYamsController().stopClosedLoopController(); + } + } + + /** + * Sets the turret target from the 20 ms robot loop. + * + * @param position desired turret mechanism angle + * @param velocityRadPerSec angular velocity feedforward in rad/s (mechanism units) + * @param accelerationRadPerSecSq angular acceleration feedforward in rad/s^2 (mechanism units) + */ + public void setTarget(Angle position, double velocityRadPerSec, double accelerationRadPerSecSq) { + double positionMechRot = position.in(edu.wpi.first.units.Units.Rotations); + positionMechRot = + MathUtil.clamp( + positionMechRot, config.getLowerLimitRotations(), config.getUpperLimitRotations()); + target.set(new TurretTarget(positionMechRot, velocityRadPerSec, accelerationRadPerSecSq)); + enabled = true; + + Logger.recordOutput("SmartTurret/GoalPosition", positionMechRot); + } + + /** + * Steps the controller at 200 Hz. Determines the current state and applies the appropriate + * control mode to the TalonFX. + * + * @param dtSeconds the time step (unused in onboard-profiling mode, retained for interface + * compatibility) + */ + public void step(double dtSeconds) { + if (!enabled || forceDisabled) { + return; + } + + TurretTarget currentTarget = target.get(); + double actualPositionMechRot = talonFX.getPosition().getValueAsDouble(); + double positionError = Math.abs(currentTarget.positionMechRot() - actualPositionMechRot); + + // State transition with hysteresis. + if (currentState == TurretState.SEEKING) { + if (positionError < config.getSeekingThresholdRotations()) { + currentState = TurretState.TRACKING; + } + } else { // TRACKING + if (positionError + > config.getSeekingThresholdRotations() + config.getHysteresisBufferRotations()) { + currentState = TurretState.SEEKING; + } + } + + switch (currentState) { + case SEEKING: + talonFX.setControl(seekingRequest.withPosition(currentTarget.positionMechRot())); + break; + + case TRACKING: + // Compute position-error-directed kS feedforward externally. + // Unlike firmware kS (which keys on velocity sign and chatters at zero), + // this keys on position-error sign so it always pushes toward the target. + // Inside the deadband, kS is zeroed to let the turret settle without oscillation. + double signedError = currentTarget.positionMechRot() - actualPositionMechRot; + double ksFeedforward; + if (positionError < config.getTrackingDeadbandRotations()) { + ksFeedforward = 0.0; + } else { + ksFeedforward = Math.signum(signedError) * config.getKS(); + } + + // Inject acceleration feedforward manually. The firmware kA in Slot1 operates on + // the derivative of the velocity reference, which is nearly zero since the reference + // only changes at 20 ms boundaries. Explicitly adding kA * accel here fills that gap. + // kA is in Amps/(rot/s²), so convert rad/s² → rot/s² by dividing by 2π. + double accelFeedforward = + config.getKA() * (currentTarget.accelerationRadPerSecSq() / (2.0 * Math.PI)); + + double totalFeedforward = + applyFeedforwardSafetyPadding(actualPositionMechRot, ksFeedforward + accelFeedforward); + + talonFX.setControl( + trackingRequest + .withPosition(currentTarget.positionMechRot()) + .withVelocity(RadiansPerSecond.of(currentTarget.velocityRadPerSec())) + .withFeedForward(totalFeedforward)); + + break; + } + + // Logging. + Logger.recordOutput("SmartTurret/State", currentState.name()); + Logger.recordOutput("SmartTurret/PositionErrorRot", positionError); + Logger.recordOutput("SmartTurret/ActualPositionMechRot", actualPositionMechRot); + Logger.recordOutput("SmartTurret/TargetPositionMechRot", currentTarget.positionMechRot()); + } + + /** + * Scales feedforward toward zero when the turret is near a soft limit to prevent hard stop + * contact. The feedforward is linearly ramped down within the padding zone. + */ + private double applyFeedforwardSafetyPadding(double positionMechRot, double feedforward) { + double padding = config.getFeedforwardPaddingRotations(); + double upper = config.getUpperLimitRotations(); + double lower = config.getLowerLimitRotations(); + + if (feedforward > 0 && positionMechRot > (upper - padding)) { + double scale = MathUtil.clamp((upper - positionMechRot) / padding, 0.0, 1.0); + return feedforward * scale; + } + if (feedforward < 0 && positionMechRot < (lower + padding)) { + double scale = MathUtil.clamp((positionMechRot - lower) / padding, 0.0, 1.0); + return feedforward * scale; + } + return feedforward; + } + + /** + * Estimates the time for the turret to arrive at the given goal position from its current state. + * + *

If the error is within the tracking threshold, returns a small constant. Otherwise, uses the + * closed-form trapezoidal settling time for the seeking portion plus tracking settle time. + * + * @param goalPositionMechRot goal position in mechanism rotations + * @return estimated time to arrival in seconds + */ + public double getEstimatedTimeToArrival(double goalPositionMechRot) { + double actualMechRot = talonFX.getPosition().getValueAsDouble(); + double errorRad = Math.abs(goalPositionMechRot - actualMechRot) * 2.0 * Math.PI; + double velocityRadPerSec = getActualVelocityRadPerSec(); + double vMaxRad = config.getMaxVelocityMechRotPerSec() * 2.0 * Math.PI; + double aMaxRad = config.getMaxAccelMechRotPerSecSq() * 2.0 * Math.PI; + double seekingThresholdRad = config.getSeekingThresholdRotations() * 2.0 * Math.PI; + + // Small constant for tracking-mode settling time. + double trackingSettleTime = 0.05; + + if (errorRad <= seekingThresholdRad) { + return trackingSettleTime; + } + + // Seeking time for the distance beyond the tracking threshold. + double seekError = errorRad - seekingThresholdRad; + double seekTime = + TurretControlPhysics.trapezoidSettlingTime(seekError, velocityRadPerSec, vMaxRad, aMaxRad); + return seekTime + trackingSettleTime; + } + + /** + * Returns the actual motor encoder velocity in mechanism rad/s. + * + * @return actual motor velocity in rad/s (mechanism units) + */ + public double getActualVelocityRadPerSec() { + return talonFX.getVelocity().getValueAsDouble() * 2.0 * Math.PI; + } + + /** + * Returns the actual motor encoder position in mechanism rotations. + * + * @return actual motor position in mechanism rotations + */ + public double getActualPositionMechRot() { + return talonFX.getPosition().getValueAsDouble(); + } + + /** + * Returns the current goal position in mechanism rotations. + * + * @return goal mechanism position in rotations + */ + public double getGoalPositionMechRot() { + return target.get().positionMechRot(); + } + + /** + * Returns the current turret state (SEEKING or TRACKING). + * + * @return current {@link TurretState} + */ + public TurretState getCurrentTurretState() { + return currentState; + } + + /** Stops the turret by disabling the controller. The TalonFX will hold its last command. */ + public void stop() { + enabled = false; + double actualMechRot = talonFX.getPosition().getValueAsDouble(); + target.set(new TurretTarget(actualMechRot, 0, 0)); + currentState = TurretState.SEEKING; + } + + /** + * Resets the controller state. Use during robot init before encoder readings are available. + * + * @param currentPositionMechRot mechanism position in rotations to seed + * @param currentVelocityMechRotPerSec mechanism velocity in rot/s to seed (unused, retained for + * API compatibility) + */ + public void reset(double currentPositionMechRot, double currentVelocityMechRotPerSec) { + target.set(new TurretTarget(currentPositionMechRot, 0, 0)); + currentState = TurretState.SEEKING; + enabled = false; + } + + /** + * Returns the underlying TalonFX for use by tuning commands. + * + * @return the TalonFX hardware reference + */ + public TalonFX getTalonFX() { + return talonFX; + } + + /** + * Returns the high-frequency position status signal (250 Hz on CANivore). Use with {@link + * BaseStatusSignal#refreshAll} for latency-compensated reads. + */ + public StatusSignal getPositionSignal() { + return positionSignal; + } + + /** + * Returns the high-frequency velocity status signal (250 Hz on CANivore). Use with {@link + * BaseStatusSignal#refreshAll} for latency-compensated reads. + */ + public StatusSignal getVelocitySignal() { + return velocitySignal; + } + + /** + * Returns the high-frequency torque current status signal (250 Hz on CANivore). Use with {@link + * BaseStatusSignal#refreshAll} for latency-compensated reads. + */ + public StatusSignal getTorqueCurrentSignal() { + return torqueCurrentSignal; + } + + /** + * Returns the high-frequency acceleration status signal (250 Hz on CANivore). Use with {@link + * BaseStatusSignal#refreshAll} for latency-compensated reads. + */ + public StatusSignal getAccelerationSignal() { + return accelerationSignal; + } + + /** + * Returns the configuration used to construct this controller. + * + * @return the {@link SmartTurretConfig} + */ + public SmartTurretConfig getConfig() { + return config; + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/Launcher/TurretControlPhysics.java b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/TurretControlPhysics.java new file mode 100644 index 00000000..beba1e49 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/Launcher/TurretControlPhysics.java @@ -0,0 +1,543 @@ +package frc.robot.rebuilt.subsystems.Launcher; + +import edu.wpi.first.math.MathUtil; +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.wpilibj.smartdashboard.SmartDashboard; +import java.util.function.BiFunction; +import java.util.function.DoubleFunction; + +public class TurretControlPhysics { + /** Defines configuration parameters and physical constraints for turret aiming calculations */ + private final Translation2d turretOffsetRobotFrame; + + private final Rotation2d minTurretAngle; + private final Rotation2d maxTurretAngle; + private final Rotation2d feedforwardPaddingAngle; + private final double settlingTimeGain; // Alpha filter gain (0.0 to 1.0) + + private static final double DERIVATIVE_PROBE_TIME_DELTA = 0.005; // 5ms + private static final int MAX_SOLVER_ITERATIONS = 4; + private static final double CONVERGENCE_THRESHOLD_SECONDS = 0.001; + + private final DoubleFunction timeOfFlightFunction; + private final BiFunction settlingTimeFunction; + private final double minEffectiveRangeMeters; + private final double maxEffectiveRangeMeters; + + /** + * Builds a settling-time function from trapezoidal motion-profile constraints. + * + *

Given angle error {@code |Δθ|} in radians the function returns the time (seconds) for the + * turret to reach its goal under a trapezoidal velocity profile with peak velocity {@code vMax} + * (rad/s) and peak acceleration {@code aMax} (rad/s²). + * + *

    + *
  • Triangle phase: {@code t = 2 * sqrt(|Δθ| / aMax)} when {@code |Δθ| < vMax²/aMax} + *
  • Trapezoid phase: {@code t = vMax/aMax + |Δθ|/vMax} otherwise + *
+ * + * @param maxVelocityRadPerSec peak turret velocity (rad/s) + * @param maxAccelRadPerSecSq peak turret acceleration (rad/s²) + * @return a {@link DoubleFunction} mapping |angleErrorRadians| → settlingTimeSeconds + */ + public static DoubleFunction trapezoidalSettlingTimeFunction( + double maxVelocityRadPerSec, double maxAccelRadPerSecSq) { + double vMax = Math.abs(maxVelocityRadPerSec); + double aMax = Math.abs(maxAccelRadPerSecSq); + if (vMax < 1e-6 || aMax < 1e-6) { + return (err) -> 0.0; + } + double triangleThreshold = (vMax * vMax) / aMax; + return (angleErrorRad) -> { + double err = Math.abs(angleErrorRad); + if (err < triangleThreshold) { + return 2.0 * Math.sqrt(err / aMax); + } else { + return vMax / aMax + err / vMax; + } + }; + } + + /** + * Builds a velocity-aware settling-time function from trapezoidal motion-profile constraints. + * + *

Unlike {@link #trapezoidalSettlingTimeFunction}, this version accounts for the turret's + * current velocity when estimating time-to-arrival. Uses a closed-form analytical formula — O(1) + * cost with a single sqrt — derived from the three-phase trapezoidal profile: + * + *

    + *
  • Moving toward target without overshoot: normal accel-cruise-decel formula + *
  • Moving toward target but overshooting: decelerate past goal, then come back from rest + *
  • Moving away from target: decelerate to stop (going backward), then move forward + *
+ * + *

The returned estimate is accurate to within the settlingTimeGain that the caller applies; + * the overshoot case uses a symmetric-return path which very slightly undershoots reality. + * + * @param maxVelocityRadPerSec peak turret velocity (rad/s) + * @param maxAccelRadPerSecSq peak turret acceleration (rad/s²) + * @return a {@link BiFunction} mapping (|angleErrorRadians|, currentVelocityRadPerSec) → + * settlingTimeSeconds + */ + public static BiFunction velocityAwareSettlingTimeFunction( + double maxVelocityRadPerSec, double maxAccelRadPerSecSq) { + double vMax = Math.abs(maxVelocityRadPerSec); + double aMax = Math.abs(maxAccelRadPerSecSq); + if (vMax < 1e-6 || aMax < 1e-6) { + return (err, vel) -> 0.0; + } + return (angleErrorRad, currentVelocityRadPerSec) -> + trapezoidSettlingTime(Math.abs(angleErrorRad), currentVelocityRadPerSec, vMax, aMax); + } + + /** + * Builds a settling-time function that accounts for the 2-state SmartTurretController behavior. + * + *

When the turret is already within the seeking threshold (close to target), only a small + * constant tracking settle time is needed. When farther away, the trapezoidal settling time is + * used for the seeking portion beyond the threshold, plus the tracking settle time. + * + * @param seekingThresholdRad the SEEKING -> TRACKING transition threshold in radians + * @param trackingSettleTimeSeconds constant settle time for the tracking-mode PID (~0.05s) + * @param maxVelocityRadPerSec peak turret velocity (rad/s) + * @param maxAccelRadPerSecSq peak turret acceleration (rad/s^2) + * @return a {@link BiFunction} mapping (|angleErrorRadians|, currentVelocityRadPerSec) -> + * settlingTimeSeconds + */ + public static BiFunction twoStateSettlingTimeFunction( + double seekingThresholdRad, + double trackingSettleTimeSeconds, + double maxVelocityRadPerSec, + double maxAccelRadPerSecSq) { + double vMax = Math.abs(maxVelocityRadPerSec); + double aMax = Math.abs(maxAccelRadPerSecSq); + if (vMax < 1e-6 || aMax < 1e-6) { + return (err, vel) -> 0.0; + } + return (angleErrorRad, currentVelocityRadPerSec) -> { + double absError = Math.abs(angleErrorRad); + if (absError <= seekingThresholdRad) { + return trackingSettleTimeSeconds; + } + double seekError = absError - seekingThresholdRad; + double seekTime = trapezoidSettlingTime(seekError, currentVelocityRadPerSec, vMax, aMax); + return seekTime + trackingSettleTimeSeconds; + }; + } + + /** + * Closed-form trapezoidal settling-time estimate. + * + * @param d absolute angle error in rad (must be ≥ 0) + * @param v0 current velocity in rad/s (positive = toward target) + * @param vMax peak velocity in rad/s + * @param aMax peak acceleration in rad/s² + * @return estimated settling time in seconds + */ + static double trapezoidSettlingTime(double d, double v0, double vMax, double aMax) { + if (d < 1e-9) return 0.0; + // Clamp to reachable velocity range. + v0 = Math.max(-vMax, Math.min(vMax, v0)); + + if (v0 < 0.0) { + // Moving away from the target. First decelerate to rest (while traveling backward), + // then cover the original distance plus the backward overshoot from rest. + double tDecel = -v0 / aMax; // > 0 + double dBack = v0 * v0 / (2.0 * aMax); // distance traveled backward + return tDecel + fromRest(d + dBack, vMax, aMax); + } + + // v0 >= 0: moving toward the target. + double dStop = v0 * v0 / (2.0 * aMax); // braking distance from v0 to 0 + if (dStop > d) { + // Would overshoot. Decelerate past the target, stop, then return from rest. + double tDecel = v0 / aMax; + double dOvershoot = dStop - d; // how far past the target before stopping + return tDecel + fromRest(dOvershoot, vMax, aMax); + } + + // Normal approach: won't overshoot. Check trapezoid vs triangle. + double d1 = (vMax * vMax - v0 * v0) / (2.0 * aMax); // to accelerate from v0 to vMax + double d3 = vMax * vMax / (2.0 * aMax); // to decelerate from vMax to 0 + if (d >= d1 + d3) { + // Trapezoid: accelerate to vMax, cruise, decelerate to 0. + double t1 = (vMax - v0) / aMax; + double t2 = (d - d1 - d3) / vMax; + double t3 = vMax / aMax; + return t1 + t2 + t3; + } else { + // Triangle: accelerate to v_peak, decelerate to 0. v_peak < vMax. + double vPeak = Math.sqrt(aMax * d + v0 * v0 / 2.0); + return (vPeak - v0) / aMax + vPeak / aMax; + } + } + + /** Settling time from rest (v=0) to cover distance d under a trapezoidal profile. */ + private static double fromRest(double d, double vMax, double aMax) { + double threshold = vMax * vMax / aMax; + if (d < threshold) { + return 2.0 * Math.sqrt(d / aMax); // triangle + } else { + return vMax / aMax + d / vMax; // trapezoid + } + } + + public record RobotState(Pose2d pose, ChassisSpeeds velocity, ChassisSpeeds acceleration) {} + + @FunctionalInterface + public interface RobotPredictor { + RobotState predict(double timeSinceStartSeconds, double lookaheadSeconds); + } + + /** + * @param turretOffsetRobotFrame Vector from robot center to turret center (Robot Frame). + * @param minTurretAngle Minimum physical rotation limit (e.g. -165 deg). + * @param maxTurretAngle Maximum physical rotation limit (e.g. +165 deg). + * @param feedforwardPaddingAngle Buffer zone near limits where velocity is ramped down. + * @param settlingGain Gain for settling time estimation (<1.0 underestimates to prevent + * oscillation). + * @param timeOfFlightFunc Function returning projectile flight time (s) given distance (m). + * @param settlingTimeFunc Function returning turret settling time (s) given angle error (rad) and + * current velocity (rad/s). + * @param minRangeMeters Minimum effective shot range. + * @param maxRangeMeters Maximum effective shot range. + */ + public TurretControlPhysics( + Translation2d turretOffsetRobotFrame, + Rotation2d minTurretAngle, + Rotation2d maxTurretAngle, + Rotation2d feedforwardPaddingAngle, + double settlingGain, + DoubleFunction timeOfFlightFunc, + BiFunction settlingTimeFunc, + double minRangeMeters, + double maxRangeMeters) { + this.turretOffsetRobotFrame = turretOffsetRobotFrame; + SmartDashboard.putNumber("Turret Offset X", turretOffsetRobotFrame.getX()); + SmartDashboard.putNumber("Turret Offset Y", turretOffsetRobotFrame.getY()); + SmartDashboard.putNumber("Turret Offset Angle", turretOffsetRobotFrame.getAngle().getDegrees()); + + this.minTurretAngle = minTurretAngle; + this.maxTurretAngle = maxTurretAngle; + this.feedforwardPaddingAngle = feedforwardPaddingAngle; + this.settlingTimeGain = settlingGain; + this.timeOfFlightFunction = timeOfFlightFunc; + this.settlingTimeFunction = settlingTimeFunc; + this.minEffectiveRangeMeters = minRangeMeters; + this.maxEffectiveRangeMeters = maxRangeMeters; + } + /** Defines possible states for the aiming status */ + public enum AimingStatus { + READY_TO_FIRE, + TARGET_TOO_CLOSE, + TARGET_TOO_FAR, + IN_DEADZONE, + SOLVER_FAILED + } + /** Defines data representing for the solver result */ + public record AimingSolution( + Translation2d virtualTargetFieldPos, + Rotation2d turretFieldHeading, + Rotation2d turretLocalHeading, + double turretFeedforwardRadPerSec, + double effectiveDistanceMeters, + double estimatedTimeOfFlight, + AimingStatus status, + SolverState finalSolverState) { + public boolean isPossible() { + return status == AimingStatus.READY_TO_FIRE; + } + } + + /** + * Solves for the optimal turret angle and feedforward velocity. + * + * @param targetFieldPos The field-relative position of the target. + * @param currentTurretAngle The current robot-relative angle of the turret. + * @param currentTurretVelocityRadPerSec The current turret angular velocity in rad/s. + * @param predictor The prediction logic to estimate future robot states. + * @return A complete aiming solution including setpoints and status. + */ + public AimingSolution solve( + Translation2d targetFieldPos, + Rotation2d currentTurretAngle, + double currentTurretVelocityRadPerSec, + RobotPredictor predictor) { + + SolverState finalState = + runNewtonSolver( + targetFieldPos, currentTurretAngle, currentTurretVelocityRadPerSec, predictor); + + Rotation2d fieldHeading = getAngleFromVector(finalState.vectorToVirtualTarget); + Rotation2d localHeading = fieldHeading.minus(finalState.robotStateAtFire.pose().getRotation()); + + double feedforwardRadPerSec = calculateKinematicFeedforward(finalState); + + AimingStatus status = AimingStatus.READY_TO_FIRE; + double distanceToTarget = finalState.vectorToVirtualTarget.getNorm(); + /** Checks the distance to the target for effective shooting range */ + if (distanceToTarget < minEffectiveRangeMeters) { + status = AimingStatus.TARGET_TOO_CLOSE; + } else if (distanceToTarget > maxEffectiveRangeMeters) { + status = AimingStatus.TARGET_TOO_FAR; + } else if (!finalState.hasConverged) { + status = AimingStatus.SOLVER_FAILED; + } + + double localHeadingRadians = MathUtil.angleModulus(localHeading.getRadians()); + double minLimitRadians = minTurretAngle.getRadians(); + double maxLimitRadians = maxTurretAngle.getRadians(); + + if (localHeadingRadians < minLimitRadians || localHeadingRadians > maxLimitRadians) { + status = AimingStatus.IN_DEADZONE; + /** + * If the angle is outside limits, then it clamps to a valid limit based on motion direction + */ + if (feedforwardRadPerSec > 0.1) { + localHeading = minTurretAngle; + } else if (feedforwardRadPerSec < -0.1) { + localHeading = maxTurretAngle; + } else { + /** Sets the turret closest angle limit if the feed forward is near zero */ + double distanceToMin = + Math.abs(MathUtil.angleModulus(localHeadingRadians - minLimitRadians)); + double distanceToMax = + Math.abs(MathUtil.angleModulus(localHeadingRadians - maxLimitRadians)); + localHeading = (distanceToMin < distanceToMax) ? minTurretAngle : maxTurretAngle; + } + + feedforwardRadPerSec = 0.0; + + } else { + feedforwardRadPerSec = + applyFeedforwardSafetyPadding(localHeadingRadians, feedforwardRadPerSec); + } + + return new AimingSolution( + finalState.virtualTargetFieldPos, + fieldHeading, + localHeading, + feedforwardRadPerSec, + distanceToTarget, + finalState.requiredTimeOfFlight, + status, + finalState); + } + /** Scales feedforward when the turret is near mechanical limits */ + private double applyFeedforwardSafetyPadding( + double currentAngleRadians, double commandedFeedforward) { + double minLimitRadians = minTurretAngle.getRadians(); + double maxLimitRadians = maxTurretAngle.getRadians(); + double paddingRadians = feedforwardPaddingAngle.getRadians(); + + if (commandedFeedforward > 0 && currentAngleRadians > (maxLimitRadians - paddingRadians)) { + double distanceToLimit = maxLimitRadians - currentAngleRadians; + double scaleFactor = MathUtil.clamp(distanceToLimit / paddingRadians, 0.0, 1.0); + return commandedFeedforward * scaleFactor; + } + + if (commandedFeedforward < 0 && currentAngleRadians < (minLimitRadians + paddingRadians)) { + double distanceToLimit = currentAngleRadians - minLimitRadians; + double scaleFactor = MathUtil.clamp(distanceToLimit / paddingRadians, 0.0, 1.0); + return commandedFeedforward * scaleFactor; + } + + return commandedFeedforward; + } + /** + * Runs the Newton solver to converge the right time of flight so the launcher can shoot when + * moving + */ + private SolverState runNewtonSolver( + Translation2d targetFieldPos, + Rotation2d currentTurretAngle, + double currentTurretVelocityRadPerSec, + RobotPredictor predictor) { + + double timeFlightGuess = 0.5; + SolverState bestState = null; + /** Computes the solver state for the current guess for the current time of flight guess */ + for (int i = 0; i < MAX_SOLVER_ITERATIONS; i++) { + SolverState stateCurrent = + computePhysicsState( + timeFlightGuess, + targetFieldPos, + currentTurretAngle, + currentTurretVelocityRadPerSec, + predictor); + + if (Math.abs(stateCurrent.errorSeconds) < CONVERGENCE_THRESHOLD_SECONDS) { + return stateCurrent.markConverged(); + } + /** Computes the solver state for the derivative probe */ + SolverState stateProbe = + computePhysicsState( + timeFlightGuess + DERIVATIVE_PROBE_TIME_DELTA, + targetFieldPos, + currentTurretAngle, + currentTurretVelocityRadPerSec, + predictor); + + double slope = + (stateProbe.errorSeconds - stateCurrent.errorSeconds) / DERIVATIVE_PROBE_TIME_DELTA; + + if (Math.abs(slope) < 1e-5) slope = Math.signum(slope) * 1e-5; + + double newGuess = timeFlightGuess - (stateCurrent.errorSeconds / slope); + timeFlightGuess = Math.max(0.01, newGuess); + + bestState = stateCurrent; + } + return bestState; + } + /** Computes and returns a solver state */ + private SolverState computePhysicsState( + double timeFlightGuess, + Translation2d targetFieldPos, + Rotation2d currentTurretAngle, + double currentTurretVelocityRadPerSec, + RobotPredictor predictor) { + + RobotState stateNow = predictor.predict(0.0, 0.0); + + Translation2d estimatedVirtualTarget = + targetFieldPos.minus( + stateNow.velocity() != null + ? new Translation2d( + stateNow.velocity().vxMetersPerSecond, + stateNow.velocity().vyMetersPerSecond) + .times(timeFlightGuess) + : new Translation2d()); + /** + * Computes the robot heading, turret offset, and vector from the turret to the estimated target + */ + Rotation2d robotHeadingNow = stateNow.pose().getRotation(); + Translation2d turretOffsetNow = turretOffsetRobotFrame.rotateBy(robotHeadingNow); + Translation2d vectorToEstimatedTarget = + estimatedVirtualTarget.minus(stateNow.pose().getTranslation().plus(turretOffsetNow)); + /** Computes the angle to the estimated target */ + Rotation2d goalAngleLocal = getAngleFromVector(vectorToEstimatedTarget).minus(robotHeadingNow); + double angleErrorRadians = + Math.abs(MathUtil.angleModulus(goalAngleLocal.minus(currentTurretAngle).getRadians())); + + double estimatedSettlingTime = + settlingTimeFunction.apply(angleErrorRadians, currentTurretVelocityRadPerSec) + * settlingTimeGain; + + // Cap prediction lookahead to prevent heading over-extrapolation during fast rotation. + // Linear heading prediction (heading += omega * dt) becomes unreliable beyond ~0.15s + // and causes the solved turret goal to jump discontinuously when robot angular velocity + // changes. + double predictedSettlingTime = Math.min(estimatedSettlingTime, 0.15); + + RobotState stateAtFire = predictor.predict(0.0, predictedSettlingTime); + Rotation2d headingAtFire = stateAtFire.pose().getRotation(); + Translation2d turretOffsetAtFire = turretOffsetRobotFrame.rotateBy(headingAtFire); + + double robotAngularVelocity = stateAtFire.velocity().omegaRadiansPerSecond; + + Translation2d tangentialVelocity = + new Translation2d( + -robotAngularVelocity * turretOffsetAtFire.getY(), + robotAngularVelocity * turretOffsetAtFire.getX()); + + Translation2d robotLinearVelocity = + new Translation2d( + stateAtFire.velocity().vxMetersPerSecond, stateAtFire.velocity().vyMetersPerSecond); + + Translation2d inheritedMuzzleVelocity = robotLinearVelocity.plus(tangentialVelocity); + + Translation2d virtualTargetPos = + targetFieldPos.minus(inheritedMuzzleVelocity.times(timeFlightGuess)); + + Translation2d gunPositionAtFire = stateAtFire.pose().getTranslation().plus(turretOffsetAtFire); + Translation2d vectorToVirtualTarget = virtualTargetPos.minus(gunPositionAtFire); + + double distanceToVirtualTarget = vectorToVirtualTarget.getNorm(); + double requiredTimeOfFlight = timeOfFlightFunction.apply(distanceToVirtualTarget); + + double errorSeconds = timeFlightGuess - requiredTimeOfFlight; + /** Returns a fully constructed solver state */ + return new SolverState( + errorSeconds, + requiredTimeOfFlight, + virtualTargetPos, + vectorToVirtualTarget, + inheritedMuzzleVelocity, + stateAtFire, + false); + } + /** Computes the kinematic feed forward caused by robot rotation and acceleration */ + private double calculateKinematicFeedforward(SolverState state) { + RobotState robotState = state.robotStateAtFire; + Rotation2d robotHeading = robotState.pose().getRotation(); + Translation2d turretOffsetRotated = turretOffsetRobotFrame.rotateBy(robotHeading); + + double robotOmega = robotState.velocity().omegaRadiansPerSecond; + double robotAlpha = + robotState.acceleration() != null ? robotState.acceleration().omegaRadiansPerSecond : 0.0; + + Translation2d accelTangential = + new Translation2d( + -robotAlpha * turretOffsetRotated.getY(), robotAlpha * turretOffsetRotated.getX()); + + Translation2d accelCentripetal = turretOffsetRotated.times(-(robotOmega * robotOmega)); + /** + * Converts the robot's linear acceleration into a 2D vector and defaults it to 0 if no data is + * available + */ + Translation2d accelRobotLinear = + robotState.acceleration() != null + ? new Translation2d( + robotState.acceleration().vxMetersPerSecond, + robotState.acceleration().vyMetersPerSecond) + : new Translation2d(); + + Translation2d accelTurretMount = accelRobotLinear.plus(accelTangential).plus(accelCentripetal); + + Translation2d velocityVirtualTargetDrift = accelTurretMount.times(-state.requiredTimeOfFlight); + + Translation2d velocityRelative = + velocityVirtualTargetDrift.minus(state.inheritedMuzzleVelocity); + + double distanceSquared = Math.pow(state.vectorToVirtualTarget.getNorm(), 2); + + if (distanceSquared < 1e-4) return 0.0; + + double crossProduct = + (state.vectorToVirtualTarget.getX() * velocityRelative.getY()) + - (state.vectorToVirtualTarget.getY() * velocityRelative.getX()); + + double omegaFieldRelative = crossProduct / distanceSquared; + + return omegaFieldRelative - robotOmega; + } + + private Rotation2d getAngleFromVector(Translation2d vec) { + return new Rotation2d(vec.getX(), vec.getY()); + } + /** Packages physics data from the newton solver into one immutable object */ + public record SolverState( + double errorSeconds, + double requiredTimeOfFlight, + Translation2d virtualTargetFieldPos, + Translation2d vectorToVirtualTarget, + Translation2d inheritedMuzzleVelocity, + RobotState robotStateAtFire, + boolean hasConverged) { + /** Returns the new solver state that has converged */ + public SolverState markConverged() { + return new SolverState( + errorSeconds, + requiredTimeOfFlight, + virtualTargetFieldPos, + vectorToVirtualTarget, + inheritedMuzzleVelocity, + robotStateAtFire, + true); + } + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/Intake.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/Intake.java new file mode 100644 index 00000000..7def1c67 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/Intake.java @@ -0,0 +1,196 @@ +// 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.rebuilt.subsystems.intake; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.commands.IntakeCommands; +import frc.robot.rebuilt.commands.IntakeCommands.IntakeState; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.sensors.Controller; +import org.littletonrobotics.junction.Logger; + +public class Intake extends GenericSubsystem { + private IntakeIO io; + private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + /** Creates a new Intake and selects the IO */ + public Intake() { + super("intake.json"); + if (RobotBase.isSimulation()) { + io = new IntakeIOSim(devices); + } else { + io = new IntakeIOReal(devices); + } + } + + public void runSpintake(double speed) { + io.runSpintake(speed); + } + + public void runSpintakes(double outerSpeed, double innerSpeed) { + io.runSpintakes(innerSpeed, outerSpeed); + } + + /** Creates a command that runs the spintake at the given speed and stops when done */ + public Command spintakeCommand(double speed) { + return Commands.run( + () -> { + runSpintake(speed); + }) + .finallyDo( + () -> { + runSpintake(0); + }); + } + + public Command setDesiredHopperAngle(Angle angle) { + return io.setHopperAngle(angle); + } + + public Angle getHopperAngle() { + return inputs.hopperAngleActual; + } + + public boolean isRetracted() { + return io.isRetracted(); + } + + public boolean isDeployed() { + return io.isDeployed(); + } + + public void runHopper(double speed) { + io.runHopper(speed); + } + /** Configures test controller bindings for the spintake, hopper control, and sysid */ + public void configTestController(Controller controller) { + controller.createRightBumper().whileTrue(spintakeCommand(0.5)); + controller.createYButton().whileTrue(getHopperSysIdCommand()); + controller.setRightYAxis(controller.createRightYAxis()); + Trigger rightYAxis = new Trigger(() -> controller.getRightYAxis() > 0.01); + rightYAxis.whileTrue(Commands.run(() -> runHopper(controller.getRightYAxis()))); + } + /** Updates intake inputs from the io periodically and logs them each robot cycle */ + @Override + public void periodic() { + super.periodic(); + io.updateInputs(inputs); + + boolean autoRezeroTriggered = shouldAutoRezeroAtDeployHardStop() || shouldAutoRezeroPastStop(); + if (autoRezeroTriggered) { + io.setHopperPosition(Degrees.of(0)); + } + Logger.recordOutput("Intake/AutoRezeroTriggered", autoRezeroTriggered); + + Logger.processInputs("Intake", inputs); + } + + private boolean shouldAutoRezeroAtDeployHardStop() { + boolean deploySideState = + inputs.stateRequested == IntakeState.INTAKING + || inputs.stateRequested == IntakeState.DEPLOYED + || inputs.stateCurrent == IntakeState.DEPLOYING + || inputs.stateCurrent == IntakeState.INTAKING + || inputs.stateCurrent == IntakeState.DEPLOYED; + + return inputs.hopperZeroed + && deploySideState + && inputs.hopperHardStopDetected + && inputs.hopperAngleActual.in(Degrees) + < Constants.Intake.HOPPER_DEPLOY_STOP_REZERO_MAX_ANGLE + && inputs.hopperAngleActual.in(Degrees) > Constants.Intake.HOPPER_ANGLE_TOLERANCE; + } + + private boolean shouldAutoRezeroPastStop() { + return inputs.hopperZeroed + && inputs.hopperAngleActual.in(Degrees) < Constants.Intake.HOPPER_AUTO_REZERO_THRESHOLD; + } + + public boolean isRequested(IntakeState state) { + return inputs.stateRequested == state; + } + + public boolean isCurrent(IntakeState state) { + return inputs.stateCurrent == state; + } + + public boolean isNearTrench() { + return isCurrent(IntakeState.DEPLOYING) && io.isNearTrench(); + } + + public void setCurrentState(IntakeState state) { + inputs.stateCurrent = state; + } + + public IntakeState getCurrentState() { + return inputs.stateCurrent; + } + + public boolean isHopperStalling() { + return io.isHopperStalling(); + } + + public Command getHopperSysIdCommand() { + return io.getHopperSysIdCommand(); + } + + public Command getHopperCharacterizationCommand() { + return io.getHopperCharacterizationCommand(this); + } + + public void setRequestedState(IntakeState state) { + inputs.stateRequested = state; + } + + public void setHopperDeployed() { + io.setHopperPosition(Degrees.of(0)); + setRequestedState(IntakeCommands.IntakeState.DEPLOYED); + } + + public void setHopperRetracted() { + io.setHopperPosition(Degrees.of(120)); + setRequestedState(IntakeCommands.IntakeState.RETRACTED); + } + + public boolean isHopperMoving() { + return io.isHopperMoving(); + } + + public boolean isHopperHardStopDetected() { + return inputs.hopperHardStopDetected; + } + + public void setHopperPosition(Angle angle) { + io.setHopperPosition(angle); + } + + /** Zeroes the hopper encoder to 0° (the deployed/hard-stop position). */ + public void zeroHopper() { + io.setHopperPosition(Degrees.of(0)); + } + + public boolean isHopperZeroed() { + return inputs.hopperZeroed; + } + + public void setHopperZeroed(boolean zeroed) { + inputs.hopperZeroed = zeroed; + } + + public boolean isHopperAtGoal() { + return inputs.hopperAtGoal; + } + + public boolean isHopperAtPosition(Angle angle) { + return io.isHopperAtLocation(angle); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIO.java new file mode 100644 index 00000000..8fb5b901 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIO.java @@ -0,0 +1,59 @@ +package frc.robot.rebuilt.subsystems.intake; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.rebuilt.commands.IntakeCommands; +import org.frc5010.common.arch.GenericSubsystem; +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public IntakeCommands.IntakeState stateRequested = IntakeCommands.IntakeState.UNKNOWN; + public IntakeCommands.IntakeState stateCurrent = IntakeCommands.IntakeState.UNKNOWN; + public double speed = 0.0; + public Angle hopperAngleActual = Degrees.of(0.0); + public double hopperAngleDegrees = 0.0; + public double hopperVelocityDegreesPerSecond = 0.0; + public double hopperAmps = 0; + public boolean hopperMoving = false; + public boolean hopperStalling = false; + public boolean hopperHardStopDetected = false; + + public Angle hopperAngleDesired = Degrees.of(0); + public double hopperAngleError = 0.0; + public boolean hopperAtGoal = true; + public boolean hopperZeroed = false; + public int simulatedGamepieces = 0; + } + + public void runSpintake(double speed); + + public void runSpintakes(double outerSpeed, double innerSpeed); + + public Command setHopperAngle(Angle angle); + + public void setHopperPosition(Angle angle); + + public boolean isHopperMoving(); + + public boolean isRetracted(); + + public boolean isDeployed(); + + public boolean isHopperStalling(); + + public boolean isHopperAtLocation(Angle location); + + public void runHopper(double speed); + + public boolean isNearTrench(); + + public Command getHopperSysIdCommand(); + + public Command getHopperCharacterizationCommand(GenericSubsystem intake); + + public default void updateInputs(IntakeIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOReal.java new file mode 100644 index 00000000..7f021a06 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOReal.java @@ -0,0 +1,237 @@ +package frc.robot.rebuilt.subsystems.intake; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.rebuilt.Constants; +import frc.robot.rebuilt.FieldConstants; +import frc.robot.rebuilt.commands.IntakeCommands; +import frc.robot.rebuilt.util.TorqueCurrentArmSupport; +import java.util.Map; +import org.frc5010.common.arch.GenericSubsystem; +import org.frc5010.common.drive.GenericDrivetrain; +import org.frc5010.common.motors.SystemIdentification; +import org.littletonrobotics.junction.Logger; +import yams.mechanisms.positional.Arm; +import yams.mechanisms.velocity.FlyWheel; + +public class IntakeIOReal implements IntakeIO { + protected Map devices; + private FlyWheel spintakeInner; + private FlyWheel spintakeOuter; + private Arm intakeHopper; + private TalonFX hopperTalonFX; + private final MotionMagicTorqueCurrentFOC hopperMotionMagicRequest = + new MotionMagicTorqueCurrentFOC(0).withSlot(0); + private Angle hopperAngleSetpoint = Degrees.of(0.0); + private TorqueCurrentArmSupport.Config hopperTorqueCurrentConfig = + TorqueCurrentArmSupport.Config.defaults(false); + protected GenericDrivetrain drivetrain; + private boolean isNearTrench = false; + private IntakeCommands.IntakeState lastState = IntakeCommands.IntakeState.RETRACTED; + + /** initializes the spintake and hopper */ + public IntakeIOReal(Map devices) { + this.devices = devices; + // spintakeLead = (FlyWheel) devices.get("spintake"); + spintakeOuter = (FlyWheel) devices.get("spintake_outer"); + spintakeInner = (FlyWheel) devices.get("spintake_inner"); + intakeHopper = (Arm) devices.get("hopper"); + intakeHopper.getMotorController().setStatorCurrentLimit(Amps.of(100)); + intakeHopper.getMotorController().setSupplyCurrentLimit(Amps.of(30)); + hopperTorqueCurrentConfig = + TorqueCurrentArmSupport.loadConfig("intake/hopper.json", false, "hopper"); + hopperAngleSetpoint = intakeHopper.getAngle(); + + Object rawController = intakeHopper.getMotorController().getMotorController(); + if (!RobotBase.isSimulation() + && hopperTorqueCurrentConfig.useTorqueCurrentFOC() + && rawController instanceof TalonFX talonFX) { + hopperTalonFX = talonFX; + TorqueCurrentArmSupport.syncSlot0Feedforward(intakeHopper, hopperTalonFX); + } + } + + @Override + public void runSpintake(double speed) { + spintakeOuter.getMotor().setDutyCycle(speed); + spintakeInner.getMotor().setDutyCycle(Constants.Intake.INTAKE_INNER_IN); + } + + public void runSpintakes(double outerSpeed, double innerSpeed) { + spintakeOuter.getMotor().setDutyCycle(outerSpeed); + spintakeInner.getMotor().setDutyCycle(innerSpeed); + } + + public Command setHopperAngle(Angle angle) { + return Commands.runOnce(() -> requestHopperAngle(angle)); + } + + public void setHopperPosition(Angle angle) { + intakeHopper.getMotor().setEncoderPosition(angle); + } + + public boolean isHopperMoving() { + return Math.abs( + intakeHopper.getMotorController().getMechanismVelocity().in(Degrees.per(Second))) + > Constants.Intake.HOPPER_MOVING_VELOCITY_THRESHOLD; + } + + public boolean isHopperStalling() { + return Math.abs(intakeHopper.getMotor().getStatorCurrent().in(Amps)) + > Constants.Intake.HOPPER_STALL_CURRENT_THRESHOLD; + } + + public boolean isRetracted() { + return (intakeHopper.getAngle().gte(Constants.Intake.HOPPER_RETRACTED_ANGLE)); + } + + public boolean isDeployed() { + return (intakeHopper.getAngle().lte(Degrees.of(2.0))); + } + + public Command getHopperSysIdCommand() { + return intakeHopper.sysId(Volts.of(4), Volts.of(0.5).per(Seconds), Seconds.of(8)); + } + /** Returns a sysid command for the hopper */ + public Command getHopperSysIdCommand(GenericSubsystem intake) { + return SystemIdentification.getSysIdFullCommand( + SystemIdentification.angleSysIdRoutine( + intakeHopper.getMotorController(), intakeHopper.getName(), intake), + 5, + 5, + 3, + () -> + intakeHopper + .isNear( + intakeHopper.getMotorController().getConfig().getMechanismUpperLimit().get(), + Degrees.of(10)) + .getAsBoolean(), + () -> + intakeHopper + .isNear( + intakeHopper.getMotorController().getConfig().getMechanismLowerLimit().get(), + Degrees.of(10)) + .getAsBoolean(), + () -> intakeHopper.getMotor().setDutyCycle(0)); + } + + public Command getHopperCharacterizationCommand(GenericSubsystem intake) { + return SystemIdentification.feedforwardCharacterization( + intake, + (Voltage voltage) -> intakeHopper.getMotor().setVoltage(voltage), + () -> intakeHopper.getMotorController().getMechanismVelocity().in(Degrees.per(Second))); + } + + public void runHopper(double speed) { + intakeHopper.getMotorController().setDutyCycle(speed); + } + + public boolean isNearTrench() { + Pose2d current = drivetrain.getPoseEstimator().getCurrentPose(); + double currentX = current.getX(); + double currentY = current.getY(); + + double topTrenchLeftX = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getX(); + double topTrenchRightX = FieldConstants.TrenchZoneTop.nearAllianceRightDanger.getX(); + + double topTrenchY = FieldConstants.TrenchZoneTop.nearAllianceLeftDanger.getY(); + + double topOppTrenchLeftX = FieldConstants.TrenchZoneTop.oppAllianceLeftDanger.getX(); + double topOppTrenchRightX = FieldConstants.TrenchZoneTop.oppAllianceRightDanger.getX(); + + double lowerTrenchLeftX = FieldConstants.TrenchZoneBottom.nearAllianceLeftDanger.getX(); + double lowerTrenchRightX = FieldConstants.TrenchZoneBottom.nearAllianceRightDanger.getX(); + + double lowerTrenchY = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getY(); + + double lowerOppTrenchLeftX = FieldConstants.TrenchZoneBottom.oppAllianceLeftDanger.getX(); + double lowerOppTrenchRightX = FieldConstants.TrenchZoneBottom.oppAllianceRightDanger.getX(); + + boolean nearAllianceTop = + ((currentX > topTrenchLeftX && currentX < topTrenchRightX) && currentY > topTrenchY); + + boolean nearOppAllianceTop = + ((currentX > topOppTrenchLeftX && currentX < topOppTrenchRightX) && currentY > topTrenchY); + + boolean nearAllianceBottom = + ((currentX > lowerTrenchLeftX && currentX < lowerTrenchRightX) && currentY < lowerTrenchY); + + boolean nearOppAllianceBottom = + ((currentX > lowerOppTrenchLeftX && currentX < lowerOppTrenchRightX) + && currentY < lowerTrenchY); + + SmartDashboard.putBoolean("Near Top Opp Alliance", nearOppAllianceTop); + SmartDashboard.putBoolean("Near Top Alliance", nearAllianceTop); + SmartDashboard.putBoolean("Near Bottom Opp Alliance", nearOppAllianceBottom); + SmartDashboard.putBoolean("Near Bottom Alliance", nearAllianceBottom); + + if (nearAllianceTop || nearOppAllianceTop || nearAllianceBottom || nearOppAllianceBottom) + return true; + else { + return false; + } + } + + public double getDegreesDifference(Angle angleOne, Angle angleTwo) { + return MathUtil.inputModulus(angleOne.minus(angleTwo).in(Degrees), -180, 180); + } + + public boolean isHopperAtLocation(Angle location) { + return getDegreesDifference(intakeHopper.getMotorController().getMechanismPosition(), location) + < Constants.Intake.HOPPER_ANGLE_TOLERANCE; + } + + private void requestHopperAngle(Angle angle) { + hopperAngleSetpoint = angle; + intakeHopper.getMotorController().setPosition(angle); + } + + /** updates the input structure with the current hopper and intake speed */ + @Override + public void updateInputs(IntakeIOInputs inputs) { + double hopperVelocityDegreesPerSecond = + intakeHopper.getMotorController().getMechanismVelocity().in(Degrees.per(Second)); + double hopperAmps = intakeHopper.getMotor().getStatorCurrent().in(Amps); + + inputs.hopperAngleActual = intakeHopper.getMotorController().getMechanismPosition(); + inputs.hopperAngleDegrees = inputs.hopperAngleActual.in(Degrees); + inputs.hopperVelocityDegreesPerSecond = hopperVelocityDegreesPerSecond; + inputs.hopperAngleDesired = + hopperTalonFX != null + ? hopperAngleSetpoint + : intakeHopper + .getMotorController() + .getMechanismPositionSetpoint() + .orElse(Degrees.of(0)); + inputs.hopperAngleError = inputs.hopperAngleDesired.minus(inputs.hopperAngleActual).in(Degrees); + inputs.hopperAtGoal = + MathUtil.inputModulus(inputs.hopperAngleError, -180, 180) + < Constants.Intake.HOPPER_ANGLE_TOLERANCE; + inputs.speed = spintakeOuter.getMotor().getDutyCycle(); + inputs.hopperAmps = hopperAmps; + inputs.hopperMoving = + Math.abs(hopperVelocityDegreesPerSecond) + > Constants.Intake.HOPPER_MOVING_VELOCITY_THRESHOLD; + inputs.hopperStalling = Math.abs(hopperAmps) > Constants.Intake.HOPPER_STALL_CURRENT_THRESHOLD; + inputs.hopperHardStopDetected = inputs.hopperStalling; + + Logger.recordOutput("Hopper Velocity", hopperVelocityDegreesPerSecond); + Logger.recordOutput("Hopper Moving", inputs.hopperMoving); + Logger.recordOutput("Hopper Hard Stop", inputs.hopperHardStopDetected); + // inputs.speed = spintakeLead.getMotor().getDutyCycle(); + } +} diff --git a/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOSim.java new file mode 100644 index 00000000..dd79c731 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,56 @@ +package frc.robot.rebuilt.subsystems.intake; + +import static edu.wpi.first.units.Units.Inches; + +import frc.robot.rebuilt.Rebuilt; +import java.util.Map; +import org.frc5010.common.drive.GenericDrivetrain; +import swervelib.simulation.ironmaple.simulation.IntakeSimulation; +import swervelib.simulation.ironmaple.simulation.SimulatedArena; +import swervelib.simulation.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation; +import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation; +import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnField; + +/** Simulates the implimentation of IntakeIO */ +public class IntakeIOSim extends IntakeIOReal { + public static IntakeSimulation intakeSimulation; + private AbstractDriveTrainSimulation driveTrainSimulation; + private GamePieceOnFieldSimulation gamePiece; + /** Initializes the mapleSim intake simulation */ + public IntakeIOSim(Map devices) { + super(devices); + driveTrainSimulation = GenericDrivetrain.getMapleSimDrive().get(); + intakeSimulation = + IntakeSimulation.OverTheBumperIntake( + "Fuel", + driveTrainSimulation, + Inches.of(27.25), + Inches.of(11.25), + IntakeSimulation.IntakeSide.FRONT, + 80); + } + /** Runs the intake motor and updates the state of the intake simulation */ + @Override + public void runSpintake(double speed) { + super.runSpintake(speed); + if (speed > 0) { + intakeSimulation.startIntake(); + } else { + intakeSimulation.stopIntake(); + } + } + /** manages simulated collection of game pieces and updates intake inputs */ + @Override + public void updateInputs(IntakeIOInputs inputs) { + super.updateInputs(inputs); + if (inputs.speed < 0) { + if (intakeSimulation.obtainGamePieceFromIntake()) { + gamePiece = + new RebuiltFuelOnField( + Rebuilt.drivetrain.getPoseEstimator().getCurrentPose().getTranslation()); + SimulatedArena.getInstance().addGamePiece(gamePiece); + } + } + inputs.simulatedGamepieces = intakeSimulation.getGamePiecesAmount(); + } +} diff --git a/src/main/java/frc/robot/rebuilt/util/TorqueCurrentArmSupport.java b/src/main/java/frc/robot/rebuilt/util/TorqueCurrentArmSupport.java new file mode 100644 index 00000000..15133f73 --- /dev/null +++ b/src/main/java/frc/robot/rebuilt/util/TorqueCurrentArmSupport.java @@ -0,0 +1,87 @@ +package frc.robot.rebuilt.util; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.hardware.TalonFX; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.rebuilt.Rebuilt; +import java.io.File; +import java.io.IOException; +import org.frc5010.common.config.UnitsParser; +import org.frc5010.common.config.json.devices.YamsArmConfigurationJson; +import yams.mechanisms.positional.Arm; + +public final class TorqueCurrentArmSupport { + private static final ObjectMapper OBJECT_MAPPER = new ObjectMapper(); + private static final double RADIANS_PER_ROTATION = 2.0 * Math.PI; + + private TorqueCurrentArmSupport() {} + + public record Config( + boolean useTorqueCurrentFOC, double gravityFeedforwardAmps, Angle horizontalZero) { + public static Config defaults(boolean useTorqueCurrentFOC) { + return new Config(useTorqueCurrentFOC, 0.0, Degrees.of(0.0)); + } + } + + public static Config loadConfig( + String relativeDevicePath, boolean defaultUseTorqueCurrentFOC, String mechanismName) { + File configFile = + new File( + Filesystem.getDeployDirectory(), + Rebuilt.configDirectory + "/subsystems/" + relativeDevicePath); + if (!configFile.isFile()) { + return Config.defaults(defaultUseTorqueCurrentFOC); + } + + try { + YamsArmConfigurationJson armConfig = + OBJECT_MAPPER.readValue(configFile, YamsArmConfigurationJson.class); + return new Config( + armConfig.useTorqueCurrentFOC, + armConfig.motorSystemId.feedForward.g, + UnitsParser.parseAngle(armConfig.horizontalZero)); + } catch (IOException exception) { + DriverStation.reportWarning( + "Failed to read " + + mechanismName + + " TorqueCurrentFOC config, using safe defaults: " + + exception.getMessage(), + false); + return Config.defaults(defaultUseTorqueCurrentFOC); + } + } + + public static void syncSlot0Feedforward(Arm arm, TalonFX talonFX) { + ArmFeedforward feedforward = + arm.getMotorController().getConfig().getArmFeedforward().orElse(null); + if (feedforward == null) { + return; + } + + Slot0Configs slot0 = new Slot0Configs(); + talonFX.getConfigurator().refresh(slot0); + talonFX + .getConfigurator() + .apply( + slot0 + .withKS(feedforward.getKs()) + .withKV(feedforward.getKv() * RADIANS_PER_ROTATION) + .withKA(feedforward.getKa() * RADIANS_PER_ROTATION)); + } + + public static double calculateGravityFeedforward(Angle targetAngle, Config config) { + if (config.gravityFeedforwardAmps() == 0.0) { + return 0.0; + } + + return config.gravityFeedforwardAmps() + * Math.cos(targetAngle.minus(config.horizontalZero()).in(Radians)); + } +} diff --git a/src/main/java/org/frc5010/common/arch/GenericRobot.java b/src/main/java/org/frc5010/common/arch/GenericRobot.java index ed86f977..347a4dce 100644 --- a/src/main/java/org/frc5010/common/arch/GenericRobot.java +++ b/src/main/java/org/frc5010/common/arch/GenericRobot.java @@ -170,7 +170,8 @@ public LoggedMechanism2d getMechVisual() { @Override protected void initRealOrSim() { if (RobotBase.isReal()) { - WpiDataLogging.start(true); + // WpiDataLogging.start(true); + // TODO: Resolve this, maybe do not double log } else { WpiDataLogging.start(false); // NetworkTableInstance instance = NetworkTableInstance.getDefault(); @@ -264,6 +265,10 @@ public Command getAutonomousCommand() { return generateAutoCommand(selectableCommand.get().asProxy()); } + public static boolean hasEverEnabled() { + return everEnabled; + } + /** Executes periodic behavior when the robot is disabled. */ @Override public void disabledPeriodic() { diff --git a/src/main/java/org/frc5010/common/arch/GenericSubsystem.java b/src/main/java/org/frc5010/common/arch/GenericSubsystem.java index 95a44359..d819021e 100644 --- a/src/main/java/org/frc5010/common/arch/GenericSubsystem.java +++ b/src/main/java/org/frc5010/common/arch/GenericSubsystem.java @@ -4,6 +4,7 @@ package org.frc5010.common.arch; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -124,16 +125,14 @@ public void initSendable(SendableBuilder builder) { @Override public void periodic() { DashBoard.notifyListeners(); - devices.values().stream() - .forEach( - it -> { - if (it instanceof GenericFunctionalMotor) { - ((GenericFunctionalMotor) it).periodicUpdate(); - } - if (it instanceof SmartMechanism) { - ((SmartMechanism) it).updateTelemetry(); - } - }); + for (Object it : devices.values()) { + if (it instanceof GenericFunctionalMotor) { + ((GenericFunctionalMotor) it).periodicUpdate(); + } + if (it instanceof SmartMechanism) { + ((SmartMechanism) it).updateTelemetry(); + } + } } /** @@ -142,16 +141,14 @@ public void periodic() { */ @Override public void simulationPeriodic() { - devices.values().stream() - .forEach( - it -> { - if (it instanceof GenericFunctionalMotor) { - ((GenericFunctionalMotor) it).simulationUpdate(); - } - if (it instanceof SmartMechanism) { - ((SmartMechanism) it).simIterate(); - } - }); + for (Object it : devices.values()) { + if (it instanceof GenericFunctionalMotor) { + ((GenericFunctionalMotor) it).simulationUpdate(); + } + if (it instanceof SmartMechanism) { + ((SmartMechanism) it).simIterate(); + } + } } /** @@ -172,4 +169,9 @@ public DisplayValuesHelper getDisplayValuesHelper() { public void setDisplay(boolean display) { if (display) DashBoard.makeDisplayed(); } + + public static Object setHeight(Distance of) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setHeight'"); + } } diff --git a/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java b/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java index f4a75091..c58f5607 100644 --- a/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java +++ b/src/main/java/org/frc5010/common/commands/AkitDriveCommands.java @@ -25,6 +25,7 @@ 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.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -57,7 +58,7 @@ public class AkitDriveCommands { private static final double FF_RAMP_RATE = 0.1; // Volts/Sec private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 - private static final double PID_TUNING_VOLTAGE = 2.0; // Volts - voltage to apply during tuning + private static final double PID_TUNING_VOLTAGE = 30.0; // Volts - voltage to apply during tuning private static final double PID_TUNING_DELAY = 1.0; // Secs - initial delay before measurements private AkitDriveCommands() {} @@ -258,6 +259,24 @@ public static Command feedforwardCharacterization( })); } + /** + * Measures the velocity feedforward constants for the drive motors using TorqueCurrentFOC. + * + *

This command should only be used in torque current control mode. + * + * @param subsystem the swerve drivetrain subsystem to characterize + * @param characterizer consumer that accepts current values to apply to drive motors + * @param velocitySupplier supplier that returns the current velocity for measurement + * @return a command that performs feedforward characterization and logs results + */ + public static Command torqueCurrentFeedforwardCharacterization( + GenericSubsystem subsystem, + Consumer characterizer, + Supplier velocitySupplier) { + return org.frc5010.common.motors.SystemIdentification.torqueCurrentFeedforwardCharacterization( + subsystem, characterizer, velocitySupplier); + } + /** * Measures the robot's wheel radius by spinning in a circle. * @@ -379,8 +398,7 @@ public static Command drivePIDTuning(GenericSwerveDrivetrain swerveDrive, AkitSw // Transition to new setpoint Commands.runOnce( () -> { - ChassisSpeeds speeds = - new ChassisSpeeds(targetVelocity, 0.0, 0.0); + ChassisSpeeds speeds = new ChassisSpeeds(0, 0.0, targetVelocity); drive.runVelocity(speeds); }), @@ -391,7 +409,7 @@ public static Command drivePIDTuning(GenericSwerveDrivetrain swerveDrive, AkitSw Commands.run( () -> { ChassisSpeeds speeds = - new ChassisSpeeds(targetVelocity, 0.0, 0.0); + new ChassisSpeeds(0, 0.0, targetVelocity); drive.runVelocity(speeds); // Calculate velocity error: difference between setpoint @@ -399,8 +417,7 @@ public static Command drivePIDTuning(GenericSwerveDrivetrain swerveDrive, AkitSw double avgVelocity = 0.0; for (int i = 0; i < 4; i++) { avgVelocity += - drive.getModulesInfo()[i] - .driveVelocityMetersPerSecond(); + drive.getModulesInfo()[i].driveVelocityMetersPerSecond; } avgVelocity /= 4.0; double error = Math.abs(targetVelocity - avgVelocity); @@ -497,7 +514,7 @@ public static Command steerPIDTuning(GenericSwerveDrivetrain swerveDrive, AkitSw double avgAngleError = 0.0; for (int i = 0; i < 4; i++) { double currentAngle = - drive.getModulesInfo()[i].steerAbsoluteDegrees(); + drive.getModulesInfo()[i].steerAbsoluteDegrees; double error = Math.abs(targetDegrees - currentAngle); // Handle angle wrapping (shortest path) if (error > 180.0) { diff --git a/src/main/java/org/frc5010/common/config/RobotParser.java b/src/main/java/org/frc5010/common/config/RobotParser.java index 6d478bf3..2d8bc04d 100644 --- a/src/main/java/org/frc5010/common/config/RobotParser.java +++ b/src/main/java/org/frc5010/common/config/RobotParser.java @@ -19,6 +19,7 @@ import org.frc5010.common.config.json.VisionPropertiesJson; import org.frc5010.common.config.json.YAGSLDrivetrainJson; import org.frc5010.common.config.json.devices.LEDStripParser; +import org.frc5010.common.config.json.devices.OrchestraParser; /** * Parses JSON configuration files to initialize and build a robot's subsystems. @@ -108,6 +109,9 @@ public RobotParser(String robotDirectory, GenericRobot robot) throws IOException // Parse LED strips LEDStripParser.parse(robotDirectory); + // Parse the orchestra configuration (if it exists) + OrchestraParser.parse(robotDirectory); + // Read in the drivetrain switch (robotJson.driveType) { case "YAGSL_SWERVE_DRIVE": diff --git a/src/main/java/org/frc5010/common/config/json/AKitSwerveDrivetrainJson.java b/src/main/java/org/frc5010/common/config/json/AKitSwerveDrivetrainJson.java index fbd61265..d1c9b527 100644 --- a/src/main/java/org/frc5010/common/config/json/AKitSwerveDrivetrainJson.java +++ b/src/main/java/org/frc5010/common/config/json/AKitSwerveDrivetrainJson.java @@ -76,27 +76,26 @@ public void createDriveTrain(GenericRobot robot) { 1), getModuleTranslations(config)); - SwerveDriveFunctions.mapleSimConfig = - DriveTrainSimulationConfig.Default() - .withBumperSize(config.getBumperFrameWidth(), config.getBumperFrameLength()) - .withRobotMass(config.getRobotMass()) - .withCustomModuleTranslations(getModuleTranslations(config)) - .withGyro(COTS.ofPigeon2()) - .withSwerveModule( - new SwerveModuleSimulationConfig( - DeviceConfigReader.getSimulatedMotor( - constants.modules.get("frontLeft").driveMotorSetup.motorType, 1), - DeviceConfigReader.getSimulatedMotor( - constants.modules.get("frontLeft").steerMotorSetup.motorType, 1), - config.getDriveGearRatio(), - config.getSteerGearRatio(), - Volts.of(config.FrontLeft.DriveFrictionVoltage), - Volts.of(config.FrontLeft.SteerFrictionVoltage), - Meters.of(config.FrontLeft.WheelRadius), - KilogramSquareMeters.of(config.FrontLeft.SteerInertia), - constants.wheelCOF)); - if (RobotBase.isSimulation()) { + SwerveDriveFunctions.mapleSimConfig = + DriveTrainSimulationConfig.Default() + .withBumperSize(config.getBumperFrameWidth(), config.getBumperFrameLength()) + .withRobotMass(config.getRobotMass()) + .withCustomModuleTranslations(getModuleTranslations(config)) + .withGyro(COTS.ofPigeon2()) + .withSwerveModule( + new SwerveModuleSimulationConfig( + DeviceConfigReader.getSimulatedMotor( + constants.modules.get("frontLeft").driveMotorSetup.motorType, 1), + DeviceConfigReader.getSimulatedMotor( + constants.modules.get("frontLeft").steerMotorSetup.motorType, 1), + config.getDriveGearRatio(), + config.getSteerGearRatio(), + Volts.of(config.FrontLeft.DriveFrictionVoltage), + Volts.of(config.FrontLeft.SteerFrictionVoltage), + Meters.of(config.FrontLeft.WheelRadius), + KilogramSquareMeters.of(config.FrontLeft.SteerInertia), + constants.wheelCOF)); SwerveDriveFunctions.driveSimulation = new SwerveDriveSimulation( SwerveDriveFunctions.mapleSimConfig, new Pose2d(3, 3, new Rotation2d())); diff --git a/src/main/java/org/frc5010/common/config/json/devices/OrchestraConfigJson.java b/src/main/java/org/frc5010/common/config/json/devices/OrchestraConfigJson.java new file mode 100644 index 00000000..8b49e104 --- /dev/null +++ b/src/main/java/org/frc5010/common/config/json/devices/OrchestraConfigJson.java @@ -0,0 +1,21 @@ +package org.frc5010.common.config.json.devices; + +import org.frc5010.common.utils.OrchestraManager; + +public class OrchestraConfigJson { + public int[] rioIds = {}; + + // TalonFX CAN IDs on the CANivore bus + public int[] canivoreIds = {}; + + public static class MusicEntry { + public String name = ""; + public String path = ""; + } + + public MusicEntry[] music = {}; + + public void configure() { + OrchestraManager.init(this); + } +} diff --git a/src/main/java/org/frc5010/common/config/json/devices/OrchestraParser.java b/src/main/java/org/frc5010/common/config/json/devices/OrchestraParser.java new file mode 100644 index 00000000..bc6a1f55 --- /dev/null +++ b/src/main/java/org/frc5010/common/config/json/devices/OrchestraParser.java @@ -0,0 +1,25 @@ +package org.frc5010.common.config.json.devices; + +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.File; +import java.io.IOException; + +public class OrchestraParser { + public static void parse(String robotDirectory) { + try { + File directory = new File(Filesystem.getDeployDirectory(), robotDirectory + "/subsystems"); + DeviceConfigReader.checkDirectory(directory); + File deviceFile = new File(directory, "orchestra.json"); + if (!deviceFile.exists()) { + return; + } + OrchestraConfigJson orchestraConfig = + new ObjectMapper().readValue(deviceFile, OrchestraConfigJson.class); + orchestraConfig.configure(); + } catch (IOException e) { + System.out.println("Error reading device configuration: " + e.getMessage()); + return; + } + } +} diff --git a/src/main/java/org/frc5010/common/config/json/devices/YamsArmConfigurationJson.java b/src/main/java/org/frc5010/common/config/json/devices/YamsArmConfigurationJson.java index 0c9df9d4..ae0189e3 100644 --- a/src/main/java/org/frc5010/common/config/json/devices/YamsArmConfigurationJson.java +++ b/src/main/java/org/frc5010/common/config/json/devices/YamsArmConfigurationJson.java @@ -37,6 +37,7 @@ public class YamsArmConfigurationJson implements DeviceConfiguration { public UnitValueJson mass = new UnitValueJson(0, MassUnit.POUNDS.toString()); public UnitValueJson voltageCompensation = new UnitValueJson(12, VoltageUnit.VOLTS.toString()); public UnitValueJson horizontalZero = new UnitValueJson(0, AngleUnit.DEGREES.toString()); + public boolean useTorqueCurrentFOC = true; /** * Configure the given GenericSubsystem with an arm using the given json configuration. diff --git a/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java b/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java index 23e54c42..3bf1df18 100644 --- a/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java +++ b/src/main/java/org/frc5010/common/drive/pose/DrivePoseEstimator.java @@ -24,19 +24,95 @@ import java.util.ArrayList; import java.util.List; import java.util.function.Function; -import java.util.stream.Collectors; import org.frc5010.common.arch.GenericSubsystem; import org.frc5010.common.commands.calibration.PoseProviderAutoOffset; import org.frc5010.common.drive.GenericDrivetrain; import org.frc5010.common.drive.pose.PoseProvider.PoseObservation; +import org.frc5010.common.drive.pose.PoseProvider.PoseObservationType; import org.frc5010.common.drive.pose.PoseProvider.ProviderType; import org.frc5010.common.subsystems.LEDStripSegment; import org.frc5010.common.telemetry.DisplayBoolean; import org.frc5010.common.vision.AprilTags; import org.frc5010.common.vision.VisionConstants; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; /** A class to handle estimating the pose of the robot */ public class DrivePoseEstimator extends GenericSubsystem { + + public enum VisionRejectionReason { + NONE, + NO_TAGS, + HIGH_AMBIGUITY, + BAD_Z, + OUT_OF_BOUNDS + } + + public static record VisionObservationDiagnostic( + int cameraIndex, + ProviderType providerType, + PoseObservationType observationType, + double timestamp, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + boolean accepted, + VisionRejectionReason rejectionReason, + boolean acceptorCandidate, + double xStdDev, + double yStdDev, + double thetaStdDev) { + public static final VisionObservationDiagnostic EMPTY = + new VisionObservationDiagnostic( + -1, + ProviderType.NONE, + PoseObservationType.PHOTONVISION, + 0.0, + new Pose3d(), + 0.0, + 0, + 0.0, + false, + VisionRejectionReason.NONE, + false, + Double.NaN, + Double.NaN, + Double.NaN); + } + + @AutoLog + public static class DrivePoseEstimatorInputs { + public Pose2d pose2d = new Pose2d(); + public Pose3d pose3d = new Pose3d(); + public int providerCount = 0; + public int activeProviderCount = 0; + public int observationsReceived = 0; + public int observationsAccepted = 0; + public int observationsRejected = 0; + public int noTagRejectCount = 0; + public int ambiguityRejectCount = 0; + public int zRejectCount = 0; + public int outOfBoundsRejectCount = 0; + public int acceptorCandidateCount = 0; + public boolean visionApplied = false; + public boolean acceptorUpdating = false; + public boolean poseAcceptable = false; + public boolean visionUpdateDisabled = false; + public boolean acceptorUpdatesEnabled = false; + public State estimatorState = State.DISABLED_FIELD; + public double confidenceResetThreshold = 0.0; + public double maxAmbiguity = 0.0; + public double maxZError = 0.0; + public double linearStdDevBaseline = 0.0; + public double angularStdDevBaseline = 0.0; + public double linearStdDevMegatag2Factor = 0.0; + public double angularStdDevMegatag2Factor = 0.0; + public double[] cameraStdDevFactors = new double[0]; + public VisionObservationDiagnostic[] visionObservationDiagnostics = + new VisionObservationDiagnostic[0]; + } + /** The pose tracker */ protected GenericPose poseTracker; /** The field2d object for displaying the pose */ @@ -47,6 +123,17 @@ public class DrivePoseEstimator extends GenericSubsystem { private boolean disableVisionUpdateCommand = false; /** List of PoseProviders */ private List poseProviders = new ArrayList<>(); + /** Reusable filtered list to avoid per-cycle stream/collect allocation */ + private final List activePoseProviders = new ArrayList<>(); + /** Cached current pose in 3D — updated once per periodic() to avoid per-call allocation */ + private Pose3d cachedPose3d = new Pose3d(); + /** Pre-allocated array for Shuffleboard pose3d widget — filled in-place each cycle */ + private final double[] pose3dArray = new double[7]; + /** Reused list for estimator-side observation diagnostics. */ + private final List visionObservationDiagnostics = new ArrayList<>(); + /** Pre-allocated array for estimator-side observation diagnostics. */ + private VisionObservationDiagnostic[] visionObservationDiagnosticsArray = + new VisionObservationDiagnostic[0]; private DisplayBoolean aprilTagVisible = DashBoard.makeDisplayBoolean("AprilTagVisible"); private boolean updatingPoseAcceptor = false; @@ -55,6 +142,8 @@ public class DrivePoseEstimator extends GenericSubsystem { private boolean activateAcceptorUpdates = true; private boolean poseAcceptable = false; + private DrivePoseEstimatorInputsAutoLogged inputs = new DrivePoseEstimatorInputsAutoLogged(); + public static enum State { DISABLED_FIELD(ProviderType.FIELD_BASED), DISABLED_ENV(ProviderType.ENVIRONMENT_BASED), @@ -179,7 +268,7 @@ public Command getCalibrationCommand(GenericDrivetrain drivetrain, int cameraInd */ public Pose2d getCurrentPose() { // return poseProviders.get(0).getRobotPose().get().toPose2d(); - return poseTracker.getCurrentPose(); + return inputs.pose2d; } /** @@ -188,7 +277,7 @@ public Pose2d getCurrentPose() { * @return the current pose with z = 0 */ public Pose3d getCurrentPose3d() { - return new Pose3d(poseTracker.getCurrentPose()); + return cachedPose3d; } /** @@ -197,29 +286,63 @@ public Pose3d getCurrentPose3d() { * @return the current pose */ public double[] getCurrentPose3dArray() { - Pose3d pose = getCurrentPose3d(); - Quaternion rotation = pose.getRotation().getQuaternion(); - return new double[] { - pose.getX(), - pose.getY(), - pose.getZ(), - rotation.getW(), - rotation.getX(), - rotation.getY(), - rotation.getZ() - }; + Quaternion rotation = cachedPose3d.getRotation().getQuaternion(); + pose3dArray[0] = cachedPose3d.getX(); + pose3dArray[1] = cachedPose3d.getY(); + pose3dArray[2] = cachedPose3d.getZ(); + pose3dArray[3] = rotation.getW(); + pose3dArray[4] = rotation.getX(); + pose3dArray[5] = rotation.getY(); + pose3dArray[6] = rotation.getZ(); + return pose3dArray; + } + + private void updateInputs(DrivePoseEstimatorInputsAutoLogged input) { + input.pose3d = getCurrentPose3d(); + input.pose2d = poseTracker.getCurrentPose(); + input.providerCount = poseProviders.size(); + input.activeProviderCount = activePoseProviders.size(); + input.visionUpdateDisabled = disableVisionUpdateCommand; + input.acceptorUpdatesEnabled = activateAcceptorUpdates; + input.estimatorState = state; + input.confidenceResetThreshold = CONFIDENCE_RESET_THRESHOLD; + input.maxAmbiguity = VisionConstants.maxAmbiguity; + input.maxZError = VisionConstants.maxZError; + input.linearStdDevBaseline = VisionConstants.linearStdDevBaseline; + input.angularStdDevBaseline = VisionConstants.angularStdDevBaseline; + input.linearStdDevMegatag2Factor = VisionConstants.linearStdDevMegatag2Factor; + input.angularStdDevMegatag2Factor = VisionConstants.angularStdDevMegatag2Factor; + if (input.cameraStdDevFactors.length != VisionConstants.cameraStdDevFactors.length) { + input.cameraStdDevFactors = new double[VisionConstants.cameraStdDevFactors.length]; + } + System.arraycopy( + VisionConstants.cameraStdDevFactors, + 0, + input.cameraStdDevFactors, + 0, + VisionConstants.cameraStdDevFactors.length); } @Override public void periodic() { - poseProviders.forEach(it -> it.update()); + for (int i = 0; i < poseProviders.size(); i++) { + poseProviders.get(i).update(); + } + // Refresh cached pose BEFORE updatePoseObservationFromProviders() so that getCurrentPose3d() + // returns the current cycle's odometry pose for the acceptor distance check and pose reset. + cachedPose3d = new Pose3d(poseTracker.getCurrentPose()); updatePoseObservationFromProviders(); + // Re-cache after vision fusion so field2d and consumers see the vision-fused pose. + cachedPose3d = new Pose3d(poseTracker.getCurrentPose()); field2d.setRobotPose(getCurrentPose()); + updateInputs(inputs); + Logger.processInputs("PoseEstimator", inputs); } private void resetProviderPoses(Pose2d pose) { - for (PoseProvider provider : poseProviders) { - provider.resetPose(new Pose3d(pose)); + Pose3d pose3d = new Pose3d(pose); + for (int i = 0; i < poseProviders.size(); i++) { + poseProviders.get(i).resetPose(pose3d); } } @@ -237,45 +360,37 @@ protected void updatePoseObservationFromProviders() { poseTracker.updateLocalMeasurements(); boolean visionUpdated = false; boolean accepterUpdating = false; + int observationsReceived = 0; + int observationsAccepted = 0; + int observationsRejected = 0; + int noTagRejectCount = 0; + int ambiguityRejectCount = 0; + int zRejectCount = 0; + int outOfBoundsRejectCount = 0; + int acceptorCandidateCount = 0; poseAcceptable = false; + activePoseProviders.clear(); + visionObservationDiagnostics.clear(); if (!disableVisionUpdateCommand) { - for (PoseProvider provider : - poseProviders.stream() - .filter( - it -> - it.isConnected() - && (state.type == ProviderType.ALL || it.getType() == state.type)) - .collect(Collectors.toList())) { - List observations = provider.getObservations(); - for (PoseObservation observation : observations) { - boolean rejectPose = - (provider.getType() != ProviderType.ENVIRONMENT_BASED) - && observation.tagCount() == 0 // Must have at least one tag - || (observation.tagCount() == 1 - && observation.ambiguity() - > VisionConstants.maxAmbiguity) // Cannot be high ambiguity - || Math.abs(observation.pose().getZ()) - > VisionConstants.maxZError // Must have realistic Z coordinate - - // Must be within the field boundaries - || observation.pose().getX() < 0.0 - || observation.pose().getX() > AprilTags.aprilTagFieldLayout.getFieldLength() - || observation.pose().getY() < 0.0 - || observation.pose().getY() > AprilTags.aprilTagFieldLayout.getFieldWidth(); - + // Build the filtered provider list without stream/collect allocation + for (int i = 0; i < poseProviders.size(); i++) { + PoseProvider p = poseProviders.get(i); + if (p.isConnected() && (state.type == ProviderType.ALL || p.getType() == state.type)) { + activePoseProviders.add(p); + } + } + for (int pi = 0; pi < activePoseProviders.size(); pi++) { + PoseProvider provider = activePoseProviders.get(pi); + PoseObservation[] observations = provider.getObservationsArray(); + // Cache current pose once per provider to avoid repeated calls inside the observation loop + Pose3d cachedCurrentPose3d = getCurrentPose3d(); + for (int oi = 0; oi < observations.length; oi++) { + PoseObservation observation = observations[oi]; + observationsReceived++; + VisionRejectionReason rejectionReason = getVisionRejectionReason(provider, observation); + boolean rejectPose = rejectionReason != VisionRejectionReason.NONE; Pose3d robotPose = observation.pose(); - if (!rejectPose) { - visionUpdated |= true; - poseTracker - .getVisionConsumer() - .accept( - robotPose.toPose2d(), - observation.timestamp(), - provider.getStdDeviations(observation)); - } - - // Decides if pose would be good to update - poseAcceptable |= + boolean acceptorCandidate = activateAcceptorUpdates && provider.getType() == ProviderType.FIELD_BASED && (state == State.ENABLED_FIELD || state == State.ALL) @@ -284,15 +399,73 @@ protected void updatePoseObservationFromProviders() { || (!DriverStation.isDisabled() && robotPose .getTranslation() - .getDistance(getCurrentPose3d().getTranslation()) + .getDistance(cachedCurrentPose3d.getTranslation()) < 0.1)); + + Matrix stdDevs = null; + double xStdDev = Double.NaN; + double yStdDev = Double.NaN; + double thetaStdDev = Double.NaN; + if (!rejectPose) { + stdDevs = provider.getStdDeviations(observation); + xStdDev = stdDevs.get(0, 0); + yStdDev = stdDevs.get(1, 0); + thetaStdDev = stdDevs.get(2, 0); + observationsAccepted++; + visionUpdated |= true; + poseTracker + .getVisionConsumer() + .accept(robotPose.toPose2d(), observation.timestamp(), stdDevs); + } else { + observationsRejected++; + switch (rejectionReason) { + case NO_TAGS: + noTagRejectCount++; + break; + case HIGH_AMBIGUITY: + ambiguityRejectCount++; + break; + case BAD_Z: + zRejectCount++; + break; + case OUT_OF_BOUNDS: + outOfBoundsRejectCount++; + break; + case NONE: + break; + } + } + + // Decides if pose would be good to update + if (acceptorCandidate) { + acceptorCandidateCount++; + } + poseAcceptable |= acceptorCandidate; + + visionObservationDiagnostics.add( + new VisionObservationDiagnostic( + provider.getCameraIndex(), + provider.getType(), + observation.type(), + observation.timestamp(), + robotPose, + observation.ambiguity(), + observation.tagCount(), + observation.averageTagDistance(), + !rejectPose, + rejectionReason, + acceptorCandidate, + xStdDev, + yStdDev, + thetaStdDev)); } } } // Accept poses after estimation integration if (activateAcceptorUpdates && (poseAcceptable || state == State.DISABLED_FIELD)) { - for (PoseProvider provider2 : poseProviders) { + for (int i = 0; i < poseProviders.size(); i++) { + PoseProvider provider2 = poseProviders.get(i); if (provider2.getType() == ProviderType.ENVIRONMENT_BASED) { provider2.resetPose(getCurrentPose3d()); accepterUpdating = true; @@ -302,6 +475,45 @@ protected void updatePoseObservationFromProviders() { aprilTagVisible.setValue(visionUpdated); updatingPoseAcceptor = accepterUpdating; + inputs.observationsReceived = observationsReceived; + inputs.observationsAccepted = observationsAccepted; + inputs.observationsRejected = observationsRejected; + inputs.noTagRejectCount = noTagRejectCount; + inputs.ambiguityRejectCount = ambiguityRejectCount; + inputs.zRejectCount = zRejectCount; + inputs.outOfBoundsRejectCount = outOfBoundsRejectCount; + inputs.acceptorCandidateCount = acceptorCandidateCount; + inputs.visionApplied = visionUpdated; + inputs.acceptorUpdating = accepterUpdating; + inputs.poseAcceptable = poseAcceptable; + if (visionObservationDiagnosticsArray.length != visionObservationDiagnostics.size()) { + visionObservationDiagnosticsArray = + new VisionObservationDiagnostic[visionObservationDiagnostics.size()]; + } + for (int i = 0; i < visionObservationDiagnostics.size(); i++) { + visionObservationDiagnosticsArray[i] = visionObservationDiagnostics.get(i); + } + inputs.visionObservationDiagnostics = visionObservationDiagnosticsArray; + } + + private VisionRejectionReason getVisionRejectionReason( + PoseProvider provider, PoseObservation observation) { + if (provider.getType() != ProviderType.ENVIRONMENT_BASED && observation.tagCount() == 0) { + return VisionRejectionReason.NO_TAGS; + } + if (observation.tagCount() == 1 && observation.ambiguity() > VisionConstants.maxAmbiguity) { + return VisionRejectionReason.HIGH_AMBIGUITY; + } + if (Math.abs(observation.pose().getZ()) > VisionConstants.maxZError) { + return VisionRejectionReason.BAD_Z; + } + if (observation.pose().getX() < 0.0 + || observation.pose().getX() > AprilTags.aprilTagFieldLayout.getFieldLength() + || observation.pose().getY() < 0.0 + || observation.pose().getY() > AprilTags.aprilTagFieldLayout.getFieldWidth()) { + return VisionRejectionReason.OUT_OF_BOUNDS; + } + return VisionRejectionReason.NONE; } public void setState(State type) { diff --git a/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java b/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java index 9d023423..d61e567f 100644 --- a/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java +++ b/src/main/java/org/frc5010/common/drive/pose/PoseProvider.java @@ -8,10 +8,9 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; 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 java.util.Arrays; import java.util.List; import org.frc5010.common.vision.VisionConstants; @@ -20,9 +19,19 @@ public interface PoseProvider { - public VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged(); - public Alert disconnectedAlert = new Alert("PoseProvider", AlertType.kWarning); - public int cameraIndex = 0; + /** + * Returns this provider's own {@link VisionIOInputsAutoLogged} instance. + * + *

Each implementing class must declare and return its own instance. The old pattern of a + * single {@code public VisionIOInputsAutoLogged input} field on the interface was implicitly + * {@code static final} in Java — meaning all cameras shared one object and each camera's {@code + * updateCameraInfo()} overwrote the previous camera's observations. + * + * @return the per-instance inputs object + */ + public VisionIOInputsAutoLogged getInput(); + + public int getCameraIndex(); public enum ProviderType { ALL, @@ -39,6 +48,12 @@ public enum PoseObservationType { ENVIRONMENT_BASED, } + public enum PhotonPoseMethod { + NONE, + MULTITAG, + TRIG + } + @AutoLog public static class VisionIOInputs { public boolean connected = false; @@ -47,12 +62,85 @@ public static class VisionIOInputs { public Pose3d latestTargetPose = new Pose3d(); public double captureTime; public PoseObservation[] poseObservations = new PoseObservation[0]; - public int[] tagIds = new int[0]; + // public int[] tagIds = new int[0]; + /** Total summed distance to all visible tags (meters) */ + public double totalTagDistance = 0.0; + /** Pose ambiguity of the best visible target */ + public double poseAmbiguity = 0.0; + /** Latest estimated robot pose from this camera */ + public Pose3d estimatedRobotPose = new Pose3d(); + /** Count of unread PhotonVision frames returned this cycle */ + public int unreadResultCount = 0; + /** Count of PhotonVision frames processed after capping */ + public int processedResultCount = 0; + /** Count of PhotonVision frames dropped by the per-cycle cap */ + public int droppedResultCount = 0; + /** Per-frame PhotonVision diagnostics for offline pose/filter tuning */ + public PhotonFrameObservation[] photonFrameObservations = new PhotonFrameObservation[0]; } /** Represents the angle to a simple target, not used for pose estimation. */ public static record TargetRotation(Rotation3d rotation) {} + /** Logged raw pose-relevant data for a single tracked PhotonVision target. */ + public static record PhotonTargetObservation( + int fiducialId, + double yaw, + double pitch, + double area, + double skew, + double ambiguity, + Transform3d bestCameraToTarget, + Transform3d altCameraToTarget) { + public static final PhotonTargetObservation EMPTY = + new PhotonTargetObservation( + -1, 0.0, 0.0, 0.0, 0.0, 0.0, new Transform3d(), new Transform3d()); + } + + /** Logged data for one pose-solve method from a PhotonVision frame. */ + public static record PhotonPoseEstimate( + boolean present, + Pose3d pose, + double ambiguity, + int tagCount, + double averageTagDistance, + int[] tagIds) { + public static final PhotonPoseEstimate EMPTY = + new PhotonPoseEstimate(false, new Pose3d(), 0.0, 0, 0.0, new int[0]); + } + + /** Logged raw and solved data for a single PhotonVision frame. */ + public static record PhotonFrameObservation( + double timestamp, + double latencyMillis, + long sequenceId, + long publishTimestampMicros, + int targetCount, + double totalTagDistance, + int[] multitagFiducialIds, + Transform3d rawMultitagBestTransform, + double rawMultitagAmbiguity, + PhotonPoseMethod selectedMethod, + PhotonPoseEstimate multiTagEstimate, + PhotonPoseEstimate trigEstimate, + PhotonTargetObservation[] targets) { + public static final PhotonFrameObservation EMPTY = + new PhotonFrameObservation( + 0.0, + 0.0, + 0L, + 0L, + 0, + 0.0, + new int[0], + new Transform3d(), + 0.0, + PhotonPoseMethod.NONE, + PhotonPoseEstimate.EMPTY, + PhotonPoseEstimate.EMPTY, + new PhotonTargetObservation[0]); + } + /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( double timestamp, @@ -69,7 +157,16 @@ public static record PoseObservation( * @return The current observations of the robot. */ public default List getObservations() { - return Arrays.asList(input.poseObservations); + return Arrays.asList(getInput().poseObservations); + } + + /** + * Returns the raw observations array without wrapping — zero allocation per call. + * + * @return The current observations of the robot as a raw array. + */ + public default PoseObservation[] getObservationsArray() { + return getInput().poseObservations; } /* @@ -79,11 +176,11 @@ public default List getObservations() { * @return Whether the pose provider is currently active. */ public default boolean isConnected() { - return input.connected; + return getInput().connected; } public default double getCaptureTime() { - return input.captureTime; + return getInput().captureTime; } public void update(); @@ -93,7 +190,7 @@ public default void resetPose(Pose3d initPose) {} public ProviderType getType(); public default void logInput(String tableName) { - Logger.processInputs(VisionConstants.SBTabVisionDisplay + "/Camera " + tableName, input); + Logger.processInputs(VisionConstants.SBTabVisionDisplay + "/Camera " + tableName, getInput()); } public default Matrix getStdDeviations(PoseObservation observation) { @@ -105,9 +202,9 @@ public default Matrix getStdDeviations(PoseObservation observation) { linearStdDev *= VisionConstants.linearStdDevMegatag2Factor; angularStdDev *= VisionConstants.angularStdDevMegatag2Factor; } - if (cameraIndex < VisionConstants.cameraStdDevFactors.length) { - linearStdDev *= VisionConstants.cameraStdDevFactors[cameraIndex]; - angularStdDev *= VisionConstants.cameraStdDevFactors[cameraIndex]; + if (getCameraIndex() < VisionConstants.cameraStdDevFactors.length) { + linearStdDev *= VisionConstants.cameraStdDevFactors[getCameraIndex()]; + angularStdDev *= VisionConstants.cameraStdDevFactors[getCameraIndex()]; } return VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev); diff --git a/src/main/java/org/frc5010/common/drive/swerve/AkitSwerveConfig.java b/src/main/java/org/frc5010/common/drive/swerve/AkitSwerveConfig.java index dec8fb73..e50ef874 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/AkitSwerveConfig.java +++ b/src/main/java/org/frc5010/common/drive/swerve/AkitSwerveConfig.java @@ -156,14 +156,17 @@ public AkitTalonFXSwerveConfigBuilder( .withKS(constants.driveMotorControl.feedForward.s) .withKV(constants.driveMotorControl.feedForward.v) .withKA(constants.driveMotorControl.feedForward.a)) - .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) - .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.TorqueCurrentFOC) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.TorqueCurrentFOC) .withSlipCurrent(UnitsParser.parseAmps(constants.slipCurrent)) .withSpeedAt12Volts(UnitsParser.parseVelocity(constants.maxDriveSpeed)) .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) .withFeedbackSource(SteerFeedbackType.FusedCANcoder) - .withDriveMotorInitialConfigs(new TalonFXConfiguration()) + .withDriveMotorInitialConfigs( + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs().withSupplyCurrentLimit(Amps.of(30)))) .withSteerMotorInitialConfigs( new TalonFXConfiguration() .withCurrentLimits( diff --git a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java index b206e86a..cbae6d3d 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java +++ b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java @@ -120,18 +120,25 @@ public void setupPathPlanner() { ppRobotConfigSupplier.get(), // The robot configuration () -> { // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. + // alliance. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = GenericRobot.getAlliance(); - SmartDashboard.putString("YAGSL Alliance", alliance.toString()); - return alliance == DriverStation.Alliance.Red; + return GenericRobot.getAlliance() == DriverStation.Alliance.Red; }, this // Reference to this subsystem to set requirements ); - // Preload PathPlanner Path finding + // Preload PathPlanner Path finding. // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE + warmupPathfinding(); + } + + /** + * Schedules the PathPlanner pathfinding warmup command. Safe to call during robotInit() because + * the command is decorated with ignoringDisable(true), which lets it run while the robot is + * disabled. Calling this early prevents the ~180 ms overrun that occurs when warmup finishes + * mid-match the first time a PathfindingCommand is instantiated. + */ + public void warmupPathfinding() { CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); } @@ -144,18 +151,18 @@ public void addAutoCommands(LoggedDashboardChooser selectableCommand) { public void updateGlassWidget() { GenericSwerveModuleInfo[] modules = swerveDrive.getModulesInfo(); for (int moduleKey = 0; moduleKey < modules.length; moduleKey++) { - double turningDeg = modules[moduleKey].steerRelativeDegrees(); - double absEncDeg = modules[moduleKey].steerAbsoluteDegrees(); + double turningDeg = modules[moduleKey].steerRelativeDegrees; + double absEncDeg = modules[moduleKey].steerAbsoluteDegrees; // This method will be called once per scheduler run absEncDials.get(moduleKey).setAngle(absEncDeg + 90); motorDials.get(moduleKey).setAngle(turningDeg + 90); motorDials .get(moduleKey) - .setLength(0.1 * modules[moduleKey].steerVelocityDegreesPerSecond() + 0.02); + .setLength(0.1 * modules[moduleKey].steerVelocityDegreesPerSecond + 0.02); expectDials .get(moduleKey) - .setLength(0.1 * modules[moduleKey].driveVelocityMetersPerSecond() + 0.02); - expectDials.get(moduleKey).setAngle(modules[moduleKey].expectedSteerDegrees() + 90); + .setLength(0.1 * modules[moduleKey].driveVelocityMetersPerSecond + 0.02); + expectDials.get(moduleKey).setAngle(modules[moduleKey].expectedSteerDegrees + 90); } } @@ -572,9 +579,7 @@ public ChassisSpeeds getFieldVelocitiesFromJoystick( if (Double.isNaN(xInput) || Double.isNaN(yInput) || Double.isNaN(turnSpdFunction.getAsDouble())) { - SmartDashboard.putBoolean("Controller Overide", true); - } else { - SmartDashboard.putBoolean("Controller Overide", false); + // Input is NaN — fall through to previous-value substitution below } xInput = Double.isNaN(xInput) ? previousLeftXInput : xInput; diff --git a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java index fde3b84c..fd371b46 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java +++ b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java @@ -7,32 +7,41 @@ import org.frc5010.common.drive.swerve.akit.Module; import swervelib.SwerveModule; -/** Add your docs here. */ -public record GenericSwerveModuleInfo( - double steerAbsoluteDegrees, - double steerRelativeDegrees, - double driveRelativePositionMeters, - double driveVelocityMetersPerSecond, - double steerVelocityDegreesPerSecond, - double expectedSteerDegrees) { +/** + * Mutable container for swerve module telemetry — reused each cycle to avoid per-call allocation. + * Use {@link #update(Module)} / {@link #update(SwerveModule)} to refresh values in-place. + */ +public class GenericSwerveModuleInfo { + public double steerAbsoluteDegrees; + public double steerRelativeDegrees; + public double driveRelativePositionMeters; + public double driveVelocityMetersPerSecond; + public double steerVelocityDegreesPerSecond; + public double expectedSteerDegrees; - public GenericSwerveModuleInfo(SwerveModule module) { - this( - module.getAbsolutePosition(), - module.getRelativePosition(), - module.getDriveMotor().getPosition(), - module.getDriveMotor().getVelocity(), - module.getAngleMotor().getVelocity(), - module.getState().angle.getDegrees()); + public GenericSwerveModuleInfo() {} + + /** Update all fields from a YAGSL {@link SwerveModule} — zero allocation after first call. */ + public void update(SwerveModule module) { + steerAbsoluteDegrees = module.getAbsolutePosition(); + steerRelativeDegrees = module.getRelativePosition(); + driveRelativePositionMeters = module.getDriveMotor().getPosition(); + driveVelocityMetersPerSecond = module.getDriveMotor().getVelocity(); + steerVelocityDegreesPerSecond = module.getAngleMotor().getVelocity(); + expectedSteerDegrees = module.getState().angle.getDegrees(); } - public GenericSwerveModuleInfo(Module module) { - this( - module.getAngle().getDegrees(), - module.getAngle().getDegrees(), - module.getPosition().distanceMeters, - module.getVelocityMetersPerSec(), - 0.0, - module.getAngle().getDegrees()); + /** + * Update all fields from an AKit {@link Module} — zero allocation after first call. Reads {@code + * inputs} values directly to avoid the allocating {@link Module#getPosition()} call. + */ + public void update(Module module) { + double angleDeg = module.getAngle().getDegrees(); + steerAbsoluteDegrees = angleDeg; + steerRelativeDegrees = angleDeg; + driveRelativePositionMeters = module.getPositionMeters(); + driveVelocityMetersPerSecond = module.getVelocityMetersPerSec(); + steerVelocityDegreesPerSecond = 0.0; + expectedSteerDegrees = angleDeg; } } diff --git a/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java b/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java index 960992a4..d866c679 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java +++ b/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java @@ -605,9 +605,14 @@ public Field2d getField2d() { @Override public GenericSwerveModuleInfo[] getModulesInfo() { SwerveModule[] modules = swerveDrive.getModules(); - if (null == moduleInfos) moduleInfos = new GenericSwerveModuleInfo[modules.length]; + if (null == moduleInfos) { + moduleInfos = new GenericSwerveModuleInfo[modules.length]; + for (int i = 0; i < modules.length; i++) { + moduleInfos[i] = new GenericSwerveModuleInfo(); + } + } for (int i = 0; i < modules.length; i++) { - moduleInfos[i] = new GenericSwerveModuleInfo(modules[i]); + moduleInfos[i].update(modules[i]); } return moduleInfos; } diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java b/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java index 155fc8be..450083c2 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/AkitSwerveDrive.java @@ -7,6 +7,7 @@ package org.frc5010.common.drive.swerve.akit; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; @@ -31,6 +32,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Force; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; @@ -81,6 +83,27 @@ public class AkitSwerveDrive extends SwerveDriveFunctions { private SwerveDrivePoseEstimator poseEstimator; private final Consumer resetSimulationPoseCallBack; + // Pre-allocated odometry buffers — sized for max odometry samples per 20ms loop. + // At 250 Hz odometry the theoretical max is ~5 samples; 16 provides ample headroom. + private static final int MAX_ODOMETRY_SAMPLES = 16; + /** Empty array sentinel for disabled-mode logging — never mutated */ + private static final SwerveModuleState[] EMPTY_MODULE_STATES = new SwerveModuleState[] {}; + + private final SwerveModulePosition[][] odometryModulePositions = + new SwerveModulePosition[MAX_ODOMETRY_SAMPLES][4]; + private final SwerveModulePosition[][] odometryModuleDeltas = + new SwerveModulePosition[MAX_ODOMETRY_SAMPLES][4]; + + { + // Pre-populate every slot so no null checks are needed in the hot loop + for (int s = 0; s < MAX_ODOMETRY_SAMPLES; s++) { + for (int m = 0; m < 4; m++) { + odometryModulePositions[s][m] = new SwerveModulePosition(); + odometryModuleDeltas[s][m] = new SwerveModulePosition(); + } + } + } + public AkitSwerveDrive( AkitSwerveConfig config, GyroIO gyroIO, @@ -145,8 +168,8 @@ public void periodic() { // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/Setpoints", EMPTY_MODULE_STATES); + Logger.recordOutput("SwerveStates/SetpointsOptimized", EMPTY_MODULE_STATES); } getChassisSpeeds(); @@ -154,19 +177,20 @@ public void periodic() { // Update odometry double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together - int sampleCount = sampleTimestamps.length; + int sampleCount = Math.min(sampleTimestamps.length, MAX_ODOMETRY_SAMPLES); for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + // Reuse pre-allocated position / delta arrays for this sample + SwerveModulePosition[] modulePositions = odometryModulePositions[i]; + SwerveModulePosition[] moduleDeltas = odometryModuleDeltas[i]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = - new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + SwerveModulePosition freshPos = modules[moduleIndex].getOdometryPositions()[i]; + modulePositions[moduleIndex].distanceMeters = freshPos.distanceMeters; + modulePositions[moduleIndex].angle = freshPos.angle; + moduleDeltas[moduleIndex].distanceMeters = + freshPos.distanceMeters - lastModulePositions[moduleIndex].distanceMeters; + moduleDeltas[moduleIndex].angle = freshPos.angle; + lastModulePositions[moduleIndex].distanceMeters = freshPos.distanceMeters; + lastModulePositions[moduleIndex].angle = freshPos.angle; } // Update gyro angle @@ -192,7 +216,7 @@ public void periodic() { * * @param speeds Speeds in meters/sec */ - public void runVelocity(ChassisSpeeds speeds) { + public void runVelocity(ChassisSpeeds speeds, Current[] torqueCurrents) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); @@ -204,13 +228,17 @@ public void runVelocity(ChassisSpeeds speeds) { // Send setpoints to modules for (int i = 0; i < 4; i++) { - modules[i].runSetpoint(setpointStates[i]); + modules[i].runSetpoint(setpointStates[i], torqueCurrents[i]); } // Log optimized setpoints (runSetpoint mutates each state) Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } + public void runVelocity(ChassisSpeeds speeds) { + runVelocity(speeds, new Current[] {Amps.zero(), Amps.zero(), Amps.zero(), Amps.zero()}); + } + /** Runs the drive in a straight line with the specified drive output. */ public void runCharacterization(double output) { for (int i = 0; i < 4; i++) { @@ -369,6 +397,7 @@ public ChassisSpeeds getRobotVelocity() { } @Override + @AutoLogOutput(key = "SwerveChassisSpeeds/FieldMeasured") public ChassisSpeeds getFieldVelocity() { return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getRotation()); } @@ -378,6 +407,7 @@ public ChassisSpeeds getFieldVelocity() { * Uses the same kinematics math as velocity, but with per-module acceleration instead of * velocity. */ + @AutoLogOutput(key = "SwerveChassisAcceleration/Robot") public ChassisSpeeds getChassisAcceleration() { SwerveModuleState[] accelStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { @@ -389,13 +419,14 @@ public ChassisSpeeds getChassisAcceleration() { /** * Returns the field-relative chassis acceleration derived from drive motor acceleration signals. */ + @AutoLogOutput(key = "SwerveChassisAcceleration/Field") public ChassisSpeeds getFieldAcceleration() { return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisAcceleration(), getRotation()); } @Override public void drive(ChassisSpeeds velocity, DriveFeedforwards feedforwards) { - runVelocity(velocity); + runVelocity(velocity, feedforwards.torqueCurrents()); } @Override @@ -482,9 +513,14 @@ public AngularVelocity getMaximumModuleAngleVelocity() { @Override public GenericSwerveModuleInfo[] getModulesInfo() { - if (null == moduleInfos) moduleInfos = new GenericSwerveModuleInfo[modules.length]; + if (null == moduleInfos) { + moduleInfos = new GenericSwerveModuleInfo[modules.length]; + for (int i = 0; i < modules.length; i++) { + moduleInfos[i] = new GenericSwerveModuleInfo(); + } + } for (int i = 0; i < modules.length; i++) { - moduleInfos[i] = new GenericSwerveModuleInfo(modules[i]); + moduleInfos[i].update(modules[i]); } return moduleInfos; } @@ -533,6 +569,9 @@ public void addAutoCommands( selectableCommand.addOption( "PRO: Swerve Wheel Radius Characterization", AkitDriveCommands.wheelRadiusCharacterization(drivetrain, this)); + + selectableCommand.addOption( + "PRO: Swerve Drive PID Tuning", AkitDriveCommands.drivePIDTuning(drivetrain, this)); selectableCommand.addOption( "PRO: Swerve Drive Feedforward Characterization", AkitDriveCommands.feedforwardCharacterization( @@ -546,6 +585,20 @@ public void addAutoCommands( (Voltage voltage) -> runSteerCharacterization(voltage.in(Volts)), () -> getSteerFFCharacterizationVelocity())); + selectableCommand.addOption( + "PRO: Swerve Drive TorqueCurrent Characterization", + AkitDriveCommands.torqueCurrentFeedforwardCharacterization( + drivetrain, + (edu.wpi.first.units.measure.Current current) -> runCharacterization(current.in(Amps)), + () -> getDriveFFCharacterizationVelocity())); + selectableCommand.addOption( + "PRO: Swerve Steer TorqueCurrent Characterization", + AkitDriveCommands.torqueCurrentFeedforwardCharacterization( + drivetrain, + (edu.wpi.first.units.measure.Current current) -> + runSteerCharacterization(current.in(Amps)), + () -> getSteerFFCharacterizationVelocity())); + selectableCommand.addOption( "PRO: Swerve Angle PID Tuning", AkitDriveCommands.steerPIDTuning(drivetrain, this)); selectableCommand.addOption( diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java b/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java index b911d5c1..064eb7d3 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/Module.java @@ -7,12 +7,15 @@ package org.frc5010.common.drive.swerve.akit; +import static edu.wpi.first.units.Units.Amps; + 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.units.measure.Current; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import frc.robot.Robot; @@ -31,7 +34,8 @@ public class Module { private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + /** Grow-only array: never shrunk to minimise GC pressure from per-cycle allocation */ + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[0]; public Module( ModuleIO io, @@ -63,11 +67,17 @@ public void periodic() { Math.min( inputs.odometryDrivePositionsRad.length, inputs.odometryTurnPositions.length); // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; + // Grow the array only when needed; never shrink to avoid per-cycle GC pressure + if (odometryPositions.length != sampleCount) { + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + odometryPositions[i] = new SwerveModulePosition(); + } + } for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * constants.WheelRadius; - Rotation2d angle = inputs.odometryTurnPositions[i]; - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + odometryPositions[i].distanceMeters = + inputs.odometryDrivePositionsRad[i] * constants.WheelRadius; + odometryPositions[i].angle = inputs.odometryTurnPositions[i]; } // Update alerts @@ -77,16 +87,20 @@ public void periodic() { } /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ - public void runSetpoint(SwerveModuleState state) { + public void runSetpoint(SwerveModuleState state, Current torqueCurrent) { // Optimize velocity setpoint state.optimize(getAngle()); state.cosineScale(Robot.isSimulation() ? inputs.turnAbsolutePosition : inputs.turnPosition); // Apply setpoints - io.setDriveVelocity(state.speedMetersPerSecond / constants.WheelRadius); + io.setDriveVelocity(state.speedMetersPerSecond / constants.WheelRadius, torqueCurrent); io.setTurnPosition(state.angle); } + public void runSetpoint(SwerveModuleState state) { + runSetpoint(state, Amps.zero()); + } + /** Runs the module with the specified output while controlling to rotation angles. */ public void runCharacterization(double output, AkitSwerveConfig config) { io.setDriveOpenLoop(output); diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIO.java b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIO.java index d4199b10..100cdacd 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIO.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIO.java @@ -8,6 +8,7 @@ package org.frc5010.common.drive.swerve.akit; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Current; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @@ -45,6 +46,11 @@ public default void setTurnOpenLoop(double output) {} /** Run the drive motor at the specified velocity. */ public default void setDriveVelocity(double velocityRadPerSec) {} + /** Run the drive motor at the specified velocity and torque current. */ + public default void setDriveVelocity(double velocityRadPerSec, Current torqueCurrent) { + setDriveVelocity(velocityRadPerSec); + } + /** Run the turn motor to the specified rotation. */ public default void setTurnPosition(Rotation2d rotation) {} } diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFX.java b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFX.java index 19218257..af66eafb 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFX.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFX.java @@ -1,5 +1,6 @@ package org.frc5010.common.drive.swerve.akit; +import static edu.wpi.first.units.Units.Amps; import static org.frc5010.common.drive.swerve.akit.util.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; @@ -57,6 +58,7 @@ public abstract class ModuleIOTalonFX implements ModuleIO { protected final StatusSignal driveAcceleration; protected final StatusSignal driveAppliedVolts; protected final StatusSignal driveCurrent; + protected final StatusSignal driveIsProLicensed; // Inputs from turn motor protected final StatusSignal turnPosition; @@ -141,6 +143,7 @@ protected ModuleIOTalonFX( driveAcceleration = driveTalon.getAcceleration(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); + driveIsProLicensed = driveTalon.getIsProLicensed(); // Create turn status signals turnPosition = turnTalon.getPosition(); @@ -158,6 +161,8 @@ protected ModuleIOTalonFX( driveAcceleration, driveAppliedVolts, driveCurrent, + driveIsProLicensed, + turnAbsolutePosition, turnVelocity, turnAppliedVolts, turnCurrent); @@ -216,15 +221,22 @@ public void setTurnOpenLoop(double output) { } @Override - public void setDriveVelocity(double wheelVelocityRadPerSec) { - double velocityRotPerSec = Units.radiansToRotations(wheelVelocityRadPerSec); + public void setDriveVelocity(double wheelVelocityRadPerSec, Current current) { + double motorVelocityRotPerSec = Units.radiansToRotations(wheelVelocityRadPerSec); driveTalon.setControl( switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); - case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest + .withVelocity(motorVelocityRotPerSec) + .withFeedForward(current); }); } + @Override + public void setDriveVelocity(double velocityRadPerSec) { + setDriveVelocity(velocityRadPerSec, Amps.of(0.0)); + } + @Override public void setTurnPosition(Rotation2d rotation) { turnTalon.setControl( diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFXReal.java b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFXReal.java index 278d410f..0e0987e9 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFXReal.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOTalonFXReal.java @@ -13,6 +13,8 @@ package org.frc5010.common.drive.swerve.akit; +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.util.Units; @@ -31,7 +33,10 @@ public class ModuleIOTalonFXReal extends ModuleIOTalonFX { private final Queue drivePositionQueue; private final Queue turnPositionQueue; - public ModuleIOTalonFXReal(AkitSwerveConfig config, SwerveModuleConstants constants) { + public ModuleIOTalonFXReal( + AkitSwerveConfig config, + SwerveModuleConstants + constants) { super(config, constants); this.timestampQueue = TalonFXOdometryThread.getInstance().makeTimestampQueue(); diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/util/PhoenixUtil.java b/src/main/java/org/frc5010/common/drive/swerve/akit/util/PhoenixUtil.java index 889b351c..d56882a1 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/util/PhoenixUtil.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/util/PhoenixUtil.java @@ -25,6 +25,7 @@ import com.ctre.phoenix6.sim.CANcoderSimState; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; @@ -104,10 +105,20 @@ public static double[] getSimulationOdometryTimeStamps() { /** * Regulates a {@link SwerveModuleConstants} object for simulation. If running on a real robot, - * the input object is returned unchanged. Otherwise, simulation-specific adjustments are made to - * the module constants. The following adjustments are made: - Disable encoder offsets - Disable - * motor inversions for drive and steer motors - Disable CanCoder inversion - Adjust steer motor - * PID gains for simulation - Adjust friction voltages - Adjust steer inertia + * the input object is returned unchanged. Otherwise, simulation-specific adjustments are made: + * disable encoder offsets, disable motor/encoder inversions, switch closed-loop output to + * Voltage, install sim-tuned Voltage PID gains for both steer and drive, and adjust friction + * voltages and steer inertia. + * + *

Real-robot control is {@code TorqueCurrentFOC} with JSON-tuned amps/rotation gains. Those + * gains have the wrong units for Voltage closed-loop and the wrong magnitudes for the maple-sim + * motor model, so sim overrides both the output type and the gains. The AdvantageKit reference + * Voltage steer gains (kP=70 V/rotation, kV=1.91 V·s/rotation) match this code path's lineage and + * track cleanly under the maple-sim physics. Drive uses light Voltage gains where the kV + * feedforward dominates. + * + *

The steer gear ratio is intentionally NOT overridden; it flows through from JSON so the + * controller's mechanism-frame math matches what maple-sim is physically simulating. * * @param moduleConstants module constants to regulate * @return regulated module constants @@ -126,7 +137,11 @@ public static SwerveModuleConstants regulateModuleConstantForSimulation( .withSteerMotorInverted(false) // Disable CanCoder inversion .withEncoderInverted(false) - // Adjust steer motor PID gains for simulation + // Use Voltage closed-loop in sim. JSON gains are TorqueCurrentFOC amps; reusing them under + // Voltage would treat amps as volts and either saturate or wildly under-drive. + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + // Sim-tuned steer Voltage gains (AdvantageKit reference values for sim). .withSteerMotorGains( new Slot0Configs() .withKP(70) @@ -136,7 +151,10 @@ public static SwerveModuleConstants regulateModuleConstantForSimulation( .withKV(1.91) .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)) - .withSteerMotorGearRatio(16.0) + // Sim-tuned drive Voltage gains. kV ≈ 12 V / 16.7 wheel rev/s (Kraken X60 free speed + // 6000 RPM ÷ 6:1 reduction) so the feedforward alone delivers ~85% of max-speed authority. + .withDriveMotorGains( + new Slot0Configs().withKP(0.05).withKI(0).withKD(0).withKS(0.1).withKV(0.72).withKA(0)) // Adjust friction voltages .withDriveFrictionVoltage(Volts.of(0.1)) .withSteerFrictionVoltage(Volts.of(0.05)) diff --git a/src/main/java/org/frc5010/common/motors/SystemIdentification.java b/src/main/java/org/frc5010/common/motors/SystemIdentification.java index 9f9723cf..968296a8 100644 --- a/src/main/java/org/frc5010/common/motors/SystemIdentification.java +++ b/src/main/java/org/frc5010/common/motors/SystemIdentification.java @@ -4,6 +4,7 @@ package org.frc5010.common.motors; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Rotations; @@ -11,6 +12,8 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutVoltage; @@ -38,6 +41,9 @@ public class SystemIdentification { /** Tracks the voltage being applied to a motor */ private static final MutVoltage m_appliedVoltage = new MutVoltage(0, 0, Volts); + + private static final edu.wpi.first.units.measure.MutCurrent m_appliedCurrent = + new edu.wpi.first.units.measure.MutCurrent(0, 0, Amps); /** Tracks the distance travelled of a position motor */ private static final MutAngle m_distance = new MutAngle(0, 0, Rotations); /** Tracks the velocity of a positional motor */ @@ -135,6 +141,35 @@ public static SysIdRoutine angleSysIdRoutine( subsystemBase)); } + public static SysIdRoutine torqueCurrentSysIdRoutine( + SmartMotorController motor, String motorName, SubsystemBase subsystemBase) { + + return new SysIdRoutine( + new Config( + Volts.of(5).div(Seconds.of(1)), + Volts.of(40), + Seconds.of(10)), // Note: Volts measure is interpreted as Amps + new SysIdRoutine.Mechanism( + (Voltage current) -> + ((TalonFX) motor.getMotorController()) + .setControl(new com.ctre.phoenix6.controls.TorqueCurrentFOC(current.in(Volts))), + log -> { + motor.updateTelemetry(); + motor.simIterate(); + if (motor.getMotorController() + instanceof com.ctre.phoenix6.hardware.TalonFX talonFX) { + log.motor(motorName) + .voltage( + m_appliedVoltage.mut_replace( + talonFX.getTorqueCurrent().getValueAsDouble(), + Volts)) // Log as voltage so SysId tool accepts it, but value is Amps + .angularPosition(m_distance.mut_replace(motor.getMechanismPosition())) + .angularVelocity(m_velocity.mut_replace(motor.getMechanismVelocity())); + } + }, + subsystemBase)); + } + public static Command getSysIdQuasistatic( SysIdRoutine routine, SysIdRoutine.Direction direction) { return routine.quasistatic(direction); @@ -232,6 +267,78 @@ public static Command feedforwardCharacterization( })); } + /** + * Measures the velocity feedforward constants for the motors using TorqueCurrentFOC. + * + *

This command should only be used in torque current control mode. + * + * @param subsystem the swerve drivetrain subsystem to characterize + * @param characterizer consumer that accepts current values to apply to motors + * @param velocitySupplier supplier that returns the current velocity for measurement + * @return a command that performs feedforward characterization and logs results + */ + public static Command torqueCurrentFeedforwardCharacterization( + GenericSubsystem subsystem, + Consumer characterizer, + Supplier velocitySupplier) { + List velocitySamples = new LinkedList<>(); + List currentSamples = new LinkedList<>(); + List timeSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce( + () -> { + velocitySamples.clear(); + currentSamples.clear(); + timeSamples.clear(); + }), + + // Allow modules to orient + Commands.run( + () -> { + characterizer.accept(Amps.of(0.0)); + }, + subsystem) + .withTimeout(FF_START_DELAY), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run( + () -> { + double current = + timer.get() * FF_RAMP_RATE * 5.0; // Ramp faster for Amps (e.g. 0.5 A/s) + characterizer.accept(Amps.of(current)); + velocitySamples.add(velocitySupplier.get()); + currentSamples.add(current); + timeSamples.add(timer.get()); + }, + subsystem) + + // When cancelled, calculate and print results + .finallyDo( + () -> { + double[] coefficients = + computeFeedforwardCoefficients(velocitySamples, currentSamples, timeSamples); + double kS = coefficients[0]; + double kV = coefficients[1]; + double kA = coefficients[2]; + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println( + "********** TorqueCurrent FF Characterization Results **********"); + System.out.println("\tkS (Amps): " + formatter.format(kS)); + System.out.println("\tkV (Amps / (rad/s)): " + formatter.format(kV)); + System.out.println("\tkA (Amps / (rad/s^2)): " + formatter.format(kA)); + SmartDashboard.putNumber("Characterization/TorqueCurrentFeedforward/kS", kS); + SmartDashboard.putNumber("Characterization/TorqueCurrentFeedforward/kV", kV); + SmartDashboard.putNumber("Characterization/TorqueCurrentFeedforward/kA", kA); + })); + } + static double[] computeFeedforwardCoefficients( List velocitySamples, List voltageSamples, List timeSamples) { int n = Math.min(velocitySamples.size(), voltageSamples.size()); diff --git a/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java b/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java index e1efcd78..f576b17e 100644 --- a/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java +++ b/src/main/java/org/frc5010/common/sensors/camera/GenericCamera.java @@ -12,10 +12,13 @@ import java.util.ArrayList; import java.util.List; import org.frc5010.common.drive.pose.PoseProvider; +import org.frc5010.common.drive.pose.VisionIOInputsAutoLogged; import org.frc5010.common.vision.VisionConstants; /** A generic camera interface */ public abstract class GenericCamera implements PoseProvider { + /** Per-instance inputs — each camera has its own object so observations are not overwritten */ + protected final VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged(); /** The list of updaters that will be called every time the camera is updated */ protected List updaters = new ArrayList<>(); /** The robot-to-camera transform */ @@ -57,6 +60,18 @@ public GenericCamera(String name, int colIndex, Transform3d robotToCamera) { visionLayout.addDouble("Latency", this::getCaptureTime); } + /** {@inheritDoc} Returns this camera's own per-instance inputs object. */ + @Override + public VisionIOInputsAutoLogged getInput() { + return input; + } + + /** {@inheritDoc} Returns this camera's column index, used for std-dev scaling. */ + @Override + public int getCameraIndex() { + return colIndex; + } + /** * Updates the state of the object by running all registered updaters. * diff --git a/src/main/java/org/frc5010/common/sensors/camera/LimeLightCamera.java b/src/main/java/org/frc5010/common/sensors/camera/LimeLightCamera.java index 4c5468dc..6fce2133 100644 --- a/src/main/java/org/frc5010/common/sensors/camera/LimeLightCamera.java +++ b/src/main/java/org/frc5010/common/sensors/camera/LimeLightCamera.java @@ -194,11 +194,11 @@ public void updateCameraInfo() { } // Save tag IDs to inputs objects - input.tagIds = - Arrays.stream(LimelightHelpers.getRawFiducials(name)) - .mapToInt(fiducial -> fiducial.id) - .distinct() - .toArray(); + // input.tagIds = + // Arrays.stream(LimelightHelpers.getRawFiducials(name)) + // .mapToInt(fiducial -> fiducial.id) + // .distinct() + // .toArray(); } } diff --git a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java index f4e3a61d..879012cf 100644 --- a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java +++ b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionCamera.java @@ -38,11 +38,39 @@ public PhotonVisionCamera(String name, int colIndex, Transform3d cameraToRobot) camera = new PhotonCamera(name); } + /** The empty pipeline result sentinel — avoids allocating a new one each cycle */ + private static final PhotonPipelineResult EMPTY_RESULT = new PhotonPipelineResult(); + + /** + * Maximum number of camera frames to process per 20ms loop cycle. + * + *

PhotonVision buffers every frame received since the NT connection was established. On the + * very first call to {@code getAllUnreadResults()} (typically ~16 s after boot), that buffer may + * contain hundreds of stale frames (16 s × 30 fps = ~480 frames per camera). Processing all of + * them in one cycle causes a 115–400 ms first-periodic overrun. By capping the list to the + * most-recent MAX_FRAMES_PER_CYCLE entries we limit latency while still receiving all frames that + * arrive during a single 20 ms window (≤ 2 at 30 fps). + */ + private static final int MAX_FRAMES_PER_CYCLE = 10; + /** Update the camera and target with the latest result */ @Override public void updateCameraInfo() { - camResults = camera.getAllUnreadResults(); - camResult = camResults.stream().findFirst().orElse(new PhotonPipelineResult()); + List allResults = camera.getAllUnreadResults(); + input.unreadResultCount = allResults.size(); + // Drain stale frames: keep only the most recent MAX_FRAMES_PER_CYCLE results. + // This prevents a multi-hundred-millisecond stall on the first periodic() call when + // the NT subscriber queue has accumulated hundreds of buffered frames since boot. + int size = allResults.size(); + if (size > MAX_FRAMES_PER_CYCLE) { + camResults = allResults.subList(size - MAX_FRAMES_PER_CYCLE, size); + } else { + camResults = allResults; + } + input.processedResultCount = camResults.size(); + input.droppedResultCount = Math.max(0, size - camResults.size()); + // Avoid stream().findFirst() allocation — use direct index access + camResult = camResults.isEmpty() ? EMPTY_RESULT : camResults.get(camResults.size() - 1); input.connected = camera.isConnected(); input.captureTime = camResult.getTimestampSeconds(); } diff --git a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java index 4b543da4..3ad76de3 100644 --- a/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java +++ b/src/main/java/org/frc5010/common/sensors/camera/PhotonVisionPoseCamera.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.HashSet; import java.util.List; @@ -27,6 +26,7 @@ import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator; import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; /** A camera using the PhotonVision library. */ public class PhotonVisionPoseCamera extends PhotonVisionCamera implements FiducialTargetCamera { @@ -36,6 +36,25 @@ public class PhotonVisionPoseCamera extends PhotonVisionCamera implements Fiduci protected Supplier poseSupplier; /** The current list of fiducial IDs */ protected List fiducialIds = new ArrayList<>(); + /** Pre-allocated list reused each cycle to avoid per-cycle GC pressure */ + private final List observations = new ArrayList<>(); + /** Pre-allocated set reused each cycle to avoid per-cycle GC pressure */ + private final Set tagIds = new HashSet<>(); + /** Pre-allocated list reused each cycle to avoid per-cycle GC pressure */ + private final List photonFrameObservations = new ArrayList<>(); + /** + * Pre-allocated output arrays — grown on demand but never shrunk to avoid per-cycle allocation + */ + private PoseObservation[] poseObsArray = new PoseObservation[0]; + + private PhotonFrameObservation[] photonFrameObsArray = new PhotonFrameObservation[0]; + + private int[] tagIdArray = new int[0]; + + /** Sentinel empty Pose3d — returned when no estimate is available; never mutated */ + private static final Pose3d EMPTY_POSE3D = new Pose3d(); + /** Pre-allocated stdDev matrix — mutated in getStdDeviations() to avoid per-call allocation */ + private final Matrix stdDevMatrix = VecBuilder.fill(0.0, 0.0, 0.0); /** * Constructor @@ -84,65 +103,70 @@ public PhotonVisionPoseCamera( public void updateCameraInfo() { poseEstimator.addHeadingData(Timer.getFPGATimestamp(), poseSupplier.get().getRotation()); - List observations = new ArrayList<>(); - SmartDashboard.putBoolean("Camera/" + name() + "/updating", true); + observations.clear(); + tagIds.clear(); + photonFrameObservations.clear(); super.updateCameraInfo(); - Set tagIds = new HashSet<>(); + + double totalTagDistanceAccum = 0.0; + double latestAmbiguity = 0.0; + Pose3d latestRobotPose = null; for (PhotonPipelineResult iCamResult : camResults) { - SmartDashboard.putBoolean("Camera/" + name() + "/resuls", iCamResult.hasTargets()); - Optional estimate = poseEstimator.estimateCoprocMultiTagPose(iCamResult); + Optional multiTagEstimate = + poseEstimator.estimateCoprocMultiTagPose(iCamResult); + Optional trigEstimate = + poseEstimator.estimatePnpDistanceTrigSolvePose(iCamResult); - if (estimate.isEmpty() && !DriverStation.isDisabled()) { - estimate = poseEstimator.estimatePnpDistanceTrigSolvePose(iCamResult); + Optional estimate = Optional.empty(); + PhotonPoseMethod selectedMethod = PhotonPoseMethod.NONE; + if (multiTagEstimate.isPresent()) { + estimate = multiTagEstimate; + selectedMethod = PhotonPoseMethod.MULTITAG; + } else if (trigEstimate.isPresent() && !DriverStation.isDisabled()) { + estimate = trigEstimate; + selectedMethod = PhotonPoseMethod.TRIG; } - if (estimate.isPresent()) { - // if (!DriverStation.isDisabled()) { - // Optional finalEstimate = - // poseEstimator.estimatePnpDistanceTrigSolvePose(iCamResult); - // if (finalEstimate.isPresent()) { - // estimate = finalEstimate; - // } - // } + double totalTagDistance = 0.0; + for (var iTarget : iCamResult.targets) { + totalTagDistance += iTarget.bestCameraToTarget.getTranslation().getNorm(); + } + double averageDistance = + iCamResult.targets.isEmpty() ? 0.0 : totalTagDistance / iCamResult.targets.size(); + + photonFrameObservations.add( + new PhotonFrameObservation( + iCamResult.getTimestampSeconds(), + iCamResult.metadata.getLatencyMillis(), + iCamResult.metadata.sequenceID, + iCamResult.metadata.publishTimestampMicros, + iCamResult.targets.size(), + totalTagDistance, + toMultitagIdArray(iCamResult), + getRawMultitagBestTransform(iCamResult), + getRawMultitagAmbiguity(iCamResult), + selectedMethod, + toPhotonPoseEstimate(multiTagEstimate, iCamResult, totalTagDistance, averageDistance), + toPhotonPoseEstimate(trigEstimate, iCamResult, totalTagDistance, averageDistance), + toPhotonTargetObservations(iCamResult.targets))); + if (estimate.isPresent()) { EstimatedRobotPose estimatedRobotPose = estimate.get(); Pose3d robotPose = estimatedRobotPose.estimatedPose; - - double totalTagDistance = 0.0; - for (var iTarget : iCamResult.targets) { - totalTagDistance += iTarget.bestCameraToTarget.getTranslation().getNorm(); - } // Compute the average tag distance int tagCount = estimatedRobotPose.targetsUsed.size(); - double averageDistance = 0.0; - if (!iCamResult.targets.isEmpty()) { - averageDistance = totalTagDistance / iCamResult.targets.size(); + + // Add tag IDs — use ifPresent to avoid lambda allocation from .map() + if (iCamResult.multitagResult.isPresent()) { + tagIds.addAll(iCamResult.multitagResult.get().fiducialIDsUsed); } - // Add tag IDs - iCamResult.multitagResult.map(it -> tagIds.addAll(it.fiducialIDsUsed)); - - SmartDashboard.putNumber( - "Camera/" + name() + "/Total Distance To Tag " + name, totalTagDistance); - SmartDashboard.putNumber( - "Camera/" + name() + "/Photon Ambiguity " + name, - iCamResult.getBestTarget().poseAmbiguity); - SmartDashboard.putNumberArray( - "Camera/" + name() + "/Photon Camera " + name + " POSE", - new double[] { - robotPose.getX(), - robotPose.getY(), - robotPose.getRotation().toRotation2d().getDegrees() - }); - SmartDashboard.putNumberArray( - "Camera/" + name() + "/Photon Camera " + name + " Robot Offset", - new double[] { - robotToCamera.getX(), - robotToCamera.getY(), - robotToCamera.getRotation().toRotation2d().getDegrees() - }); + // Accumulate telemetry for AK logging + totalTagDistanceAccum += totalTagDistance; + latestAmbiguity = iCamResult.getBestTarget().poseAmbiguity; + latestRobotPose = robotPose; observations.add( new PoseObservation( @@ -155,19 +179,113 @@ public void updateCameraInfo() { PoseObservationType.PHOTONVISION, ProviderType.FIELD_BASED)); } + } - // Save pose observations to inputs object - input.poseObservations = new PoseObservation[observations.size()]; - for (int i = 0; i < observations.size(); i++) { - input.poseObservations[i] = observations.get(i); - } - // Save tag IDs to inputs objects - input.tagIds = new int[tagIds.size()]; - int i = 0; - for (int id : tagIds) { - input.tagIds[i++] = id; - } + int frameObsSize = photonFrameObservations.size(); + if (photonFrameObsArray.length != frameObsSize) { + photonFrameObsArray = new PhotonFrameObservation[frameObsSize]; + } + for (int i = 0; i < frameObsSize; i++) { + photonFrameObsArray[i] = photonFrameObservations.get(i); + } + input.photonFrameObservations = photonFrameObsArray; + + // Save pose observations to inputs object (once, outside the loop). + // Grow the pre-allocated array only when the size increases; never shrink it. + int obsSize = observations.size(); + if (poseObsArray.length != obsSize) { + poseObsArray = new PoseObservation[obsSize]; + } + for (int i = 0; i < obsSize; i++) { + poseObsArray[i] = observations.get(i); + } + input.poseObservations = poseObsArray; + + // Save tag IDs to inputs object (once, outside the loop). + int tagCount2 = tagIds.size(); + if (tagIdArray.length != tagCount2) { + tagIdArray = new int[tagCount2]; + } + int i = 0; + for (int id : tagIds) { + tagIdArray[i++] = id; + } + // input.tagIds = tagIdArray; + // Log telemetry fields via AdvantageKit inputs + input.totalTagDistance = totalTagDistanceAccum; + input.poseAmbiguity = latestAmbiguity; + input.estimatedRobotPose = latestRobotPose != null ? latestRobotPose : EMPTY_POSE3D; + } + + private PhotonTargetObservation[] toPhotonTargetObservations(List targets) { + PhotonTargetObservation[] observations = new PhotonTargetObservation[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + PhotonTrackedTarget target = targets.get(i); + observations[i] = + new PhotonTargetObservation( + target.fiducialId, + target.yaw, + target.pitch, + target.area, + target.skew, + target.poseAmbiguity, + target.bestCameraToTarget, + target.altCameraToTarget); + } + return observations; + } + + private PhotonPoseEstimate toPhotonPoseEstimate( + Optional estimate, + PhotonPipelineResult cameraResult, + double totalTagDistance, + double averageDistance) { + if (estimate.isEmpty()) { + return PhotonPoseEstimate.EMPTY; + } + + EstimatedRobotPose estimatedRobotPose = estimate.get(); + return new PhotonPoseEstimate( + true, + estimatedRobotPose.estimatedPose, + cameraResult.hasTargets() ? cameraResult.getBestTarget().poseAmbiguity : 0.0, + estimatedRobotPose.targetsUsed.size(), + averageDistance, + toTargetIdArray(estimatedRobotPose.targetsUsed)); + } + + private int[] toMultitagIdArray(PhotonPipelineResult cameraResult) { + if (cameraResult.multitagResult.isEmpty()) { + return new int[0]; + } + + int[] ids = new int[cameraResult.multitagResult.get().fiducialIDsUsed.size()]; + for (int i = 0; i < ids.length; i++) { + ids[i] = cameraResult.multitagResult.get().fiducialIDsUsed.get(i); + } + return ids; + } + + private Transform3d getRawMultitagBestTransform(PhotonPipelineResult cameraResult) { + if (cameraResult.multitagResult.isEmpty()) { + return new Transform3d(); + } + return cameraResult.multitagResult.get().estimatedPose.best; + } + + private double getRawMultitagAmbiguity(PhotonPipelineResult cameraResult) { + if (cameraResult.multitagResult.isEmpty()) { + return 0.0; + } + return cameraResult.multitagResult.get().estimatedPose.ambiguity; + } + + private int[] toTargetIdArray(List targets) { + int[] ids = new int[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + ids[i] = targets.get(i).fiducialId; } + return ids; } @Override @@ -192,7 +310,10 @@ public Matrix getStdDeviations(PoseObservation observation) { && observation.tagCount() < 2)) { linearStdDev = 100.0; } - return VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev); + stdDevMatrix.set(0, 0, linearStdDev); + stdDevMatrix.set(1, 0, linearStdDev); + stdDevMatrix.set(2, 0, angularStdDev); + return stdDevMatrix; } /** diff --git a/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java b/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java index 7233a47f..d1e6d6d4 100644 --- a/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java +++ b/src/main/java/org/frc5010/common/sensors/camera/QuestNavInterface.java @@ -26,10 +26,14 @@ import java.util.function.Supplier; import org.frc5010.common.drive.GenericDrivetrain; import org.frc5010.common.drive.pose.PoseProvider; +import org.frc5010.common.drive.pose.VisionIOInputsAutoLogged; /** Add your docs here. */ public class QuestNavInterface implements PoseProvider { + /** Per-instance inputs — each QuestNav has its own object */ + private final VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged(); + private String networkTableRoot = "questnav"; private Supplier robotVelocity = null; private Transform3d robotToQuest; @@ -52,6 +56,13 @@ public class QuestNavInterface implements PoseProvider { private Translation2d _calculatedOffsetToRobotCenter = new Translation2d(); private int _calculatedOffsetToRobotCenterCount = 0; + /** Reusable observation list — cleared each cycle to avoid per-cycle ArrayList allocation */ + private final List questObservations = new ArrayList<>(); + /** Grow-only output array — never shrunk to minimise GC pressure */ + private PoseObservation[] questObsArray = new PoseObservation[0]; + /** Pre-allocated stdDev matrix — mutated in getStdDeviations() to avoid per-call allocation */ + private final Matrix stdDevMatrix = VecBuilder.fill(0.0, 0.0, 0.0); + public QuestNavInterface(Transform3d robotToQuest) { super(); this.robotToQuest = robotToQuest; @@ -65,6 +76,16 @@ public QuestNavInterface(Transform3d robotToQuest, String networkTableRoot) { this.questNav = new QuestNav(); } + @Override + public VisionIOInputsAutoLogged getInput() { + return input; + } + + @Override + public int getCameraIndex() { + return 0; + } + private Pose3d getRobotPoseFromQuestPose(Pose3d questPose) { return questPose.transformBy(robotToQuest.inverse()); } @@ -119,14 +140,14 @@ private void updateObservations() { latestPoseFrame = unreadQuestFrames[unreadQuestFrames.length - 1]; } - List observations = new ArrayList<>(); + questObservations.clear(); if (initializedPosition) { for (PoseFrame frame : unreadQuestFrames) { Pose3d robotPose = getRobotPoseFromQuestPose(frame.questPose3d()).transformBy(softResetTransform); double captureTime = frame.dataTimestamp(); - observations.add( + questObservations.add( new PoseObservation( captureTime, robotPose, @@ -138,11 +159,15 @@ private void updateObservations() { } } input.connected = isActive(); - // Save pose observations to inputs object - input.poseObservations = new PoseObservation[observations.size()]; - for (int i = 0; i < observations.size(); i++) { - input.poseObservations[i] = observations.get(i); + // Save pose observations to inputs object using grow-only pre-allocated array + int obsSize = questObservations.size(); + if (questObsArray.length != obsSize) { + questObsArray = new PoseObservation[obsSize]; + } + for (int i = 0; i < obsSize; i++) { + questObsArray[i] = questObservations.get(i); } + input.poseObservations = questObsArray; } @Override @@ -161,7 +186,10 @@ public Matrix getStdDeviations(PoseObservation observation) { calib = 10; } } - return VecBuilder.fill(calib, calib, calib * 0.2); + stdDevMatrix.set(0, 0, calib); + stdDevMatrix.set(1, 0, calib); + stdDevMatrix.set(2, 0, calib * 0.2); + return stdDevMatrix; } public double getConfidence() { diff --git a/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java b/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java index ca2a5a25..c86278b1 100644 --- a/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java +++ b/src/main/java/org/frc5010/common/sensors/camera/QuestNavOld.java @@ -40,10 +40,17 @@ import org.frc5010.common.drive.pose.DrivePoseEstimator; import org.frc5010.common.drive.pose.DrivePoseEstimator.State; import org.frc5010.common.drive.pose.PoseProvider; +import org.frc5010.common.drive.pose.VisionIOInputsAutoLogged; import org.frc5010.common.drive.swerve.GenericSwerveDrivetrain; /** Add your docs here. */ public class QuestNavOld implements PoseProvider { + /** Per-instance inputs — each QuestNav has its own object */ + private final VisionIOInputsAutoLogged input = new VisionIOInputsAutoLogged(); + /** Per-instance disconnected alert */ + private final edu.wpi.first.wpilibj.Alert disconnectedAlert = + new edu.wpi.first.wpilibj.Alert("QuestNav", edu.wpi.first.wpilibj.Alert.AlertType.kWarning); + private boolean initializedPosition = false; public static boolean isActive = false; private String networkTableRoot = "questnav"; @@ -115,6 +122,16 @@ public QuestNavOld(Transform3d robotToQuest, String networkTableRoot) { setupInitialTimestamp(); } + @Override + public VisionIOInputsAutoLogged getInput() { + return input; + } + + @Override + public int getCameraIndex() { + return 0; + } + private void setupInitialTimestamp() { startTimestamp = timestamp.get(); } diff --git a/src/main/java/org/frc5010/common/subsystems/CameraSystem.java b/src/main/java/org/frc5010/common/subsystems/CameraSystem.java index 2e62c114..0abb367c 100644 --- a/src/main/java/org/frc5010/common/subsystems/CameraSystem.java +++ b/src/main/java/org/frc5010/common/subsystems/CameraSystem.java @@ -54,6 +54,10 @@ public abstract class CameraSystem extends GenericSubsystem { protected TargetModel targetModel = new TargetModel(0.3556); /** Whether to view game pieces in simulation */ protected boolean viewGamePieces = true; + /** Cached game piece A list to detect changes between cycles */ + private List cachedGpas = List.of(); + /** Cached game piece B list to detect changes between cycles */ + private List cachedGpbs = List.of(); /** * Creates a new CameraSystem with the specified camera. @@ -88,25 +92,31 @@ public void simulationPeriodic() { if (!camera.canViewGamePieces()) { return; } - // Update game piece A targets in simulation + // Update game piece A targets only when the list has changed List gpas = SimulatedArena.getInstance().getGamePiecesByType(Constants.Simulation.gamePieceA).stream() .map(it -> it.getPose3d()) .collect(Collectors.toList()); - SimulatedCamera.visionSim.removeVisionTargets("GPA"); - for (Pose3d gpa : gpas) { - VisionTargetSim simTarget = new VisionTargetSim(gpa, targetModel); - SimulatedCamera.visionSim.addVisionTargets("GPA", simTarget); + if (!gpas.equals(cachedGpas)) { + cachedGpas = gpas; + SimulatedCamera.visionSim.removeVisionTargets("GPA"); + for (Pose3d gpa : gpas) { + VisionTargetSim simTarget = new VisionTargetSim(gpa, targetModel); + SimulatedCamera.visionSim.addVisionTargets("GPA", simTarget); + } } - // Update game piece B targets in simulation + // Update game piece B targets only when the list has changed List gpbs = SimulatedArena.getInstance().getGamePiecesByType(Constants.Simulation.gamePieceB).stream() .map(it -> it.getPose3d()) .collect(Collectors.toList()); - SimulatedCamera.visionSim.removeVisionTargets("GPB"); - for (Pose3d gpb : gpbs) { - VisionTargetSim simTarget = new VisionTargetSim(gpb, targetModel); - SimulatedCamera.visionSim.addVisionTargets("GPB", simTarget); + if (!gpbs.equals(cachedGpbs)) { + cachedGpbs = gpbs; + SimulatedCamera.visionSim.removeVisionTargets("GPB"); + for (Pose3d gpb : gpbs) { + VisionTargetSim simTarget = new VisionTargetSim(gpb, targetModel); + SimulatedCamera.visionSim.addVisionTargets("GPB", simTarget); + } } } diff --git a/src/main/java/org/frc5010/common/util/LogSummary.java b/src/main/java/org/frc5010/common/util/LogSummary.java new file mode 100644 index 00000000..1ec27e18 --- /dev/null +++ b/src/main/java/org/frc5010/common/util/LogSummary.java @@ -0,0 +1,256 @@ +package org.frc5010.common.util; + +import edu.wpi.first.util.datalog.DataLogReader; +import edu.wpi.first.util.datalog.DataLogRecord; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.Comparator; +import java.util.HashMap; +import java.util.Map; +import java.util.TreeMap; + +/** + * Command-line utility that reads a .wpilog file and prints a structured summary of key swerve + * drive signals for agent-driven diagnosis and replay validation. + * + *

Usage: + * + * 

{@code
+ * # Analyze the most recent log in logs/
+ * .\gradlew.bat logSummary
+ *
+ * # Analyze a specific log
+ * .\gradlew.bat logSummary -PlogFile=logs/FRC_20260525_143022.wpilog
+ *
+ * # Analyze the most recent replay log
+ * .\gradlew.bat replayValidate
+ * }
+ * + *

Output sections: + * + *

    + *
  1. Header — file path, log type (live/replay), and duration + *
  2. All entries — every signal name and type logged in the file + *
  3. Numeric statistics — min/max for every double-typed signal + *
  4. Anomaly flags — loop overruns, gyro/camera disconnects, excessive currents, vision + * observations all-rejected + *
+ */ +public class LogSummary { + + private static final double CURRENT_OVERLOAD_AMPS = 60.0; + private static final double LOOP_OVERRUN_MS = 25.0; + // Pose3d struct: Translation3d (3×8 bytes) + Rotation3d quaternion (4×8 bytes) = 56 bytes + private static final int POSE3D_BYTES = 56; + + public static void main(String[] args) throws Exception { + String path = (args.length > 0) ? args[0] : findLatestLog(); + if (path == null) { + System.err.println("No .wpilog file found."); + System.err.println("Run the simulation first: .\\gradlew.bat simulateJava"); + System.exit(1); + } + + boolean isReplay = path.endsWith("_sim.wpilog"); + System.out.println("=== Log Summary" + (isReplay ? " [REPLAY]" : "") + ": " + path + " ==="); + System.out.println(); + analyzeLog(path, isReplay); + } + + // --------------------------------------------------------------------------- + // Core analysis + // --------------------------------------------------------------------------- + + private static void analyzeLog(String path, boolean isReplay) throws Exception { + DataLogReader reader = new DataLogReader(path); + + Map names = new HashMap<>(); + Map types = new HashMap<>(); + Map mins = new TreeMap<>(); + Map maxes = new TreeMap<>(); + Map counts = new TreeMap<>(); + // Gyro signal names that went false (disconnected) at any point. + java.util.Set gyroDisconnected = new java.util.LinkedHashSet<>(); + // Vision: max raw bytes seen per Accepted/Rejected pose-array entry (0 = always empty). + Map visionPoseMaxRaw = new TreeMap<>(); + java.util.Set visionCamsDisconnected = new java.util.LinkedHashSet<>(); + + long firstTs = Long.MAX_VALUE; + long lastTs = 0; + + // A log produced by a sim that was killed mid-write ends in a truncated record. + // The iterator throws when it tries to read past EOF; catch it and report what we + // managed to parse rather than failing the whole analysis. + java.util.Iterator it = reader.iterator(); + while (true) { + DataLogRecord rec; + try { + if (!it.hasNext()) break; + rec = it.next(); + } catch (Exception truncated) { + System.out.println( + " (note: log ends in a truncated record — " + + "likely a sim that was killed mid-write; reporting parsed data so far)"); + break; + } + long ts = rec.getTimestamp(); + if (ts < firstTs) firstTs = ts; + if (ts > lastTs) lastTs = ts; + + if (rec.isStart()) { + var d = rec.getStartData(); + names.put(d.entry, d.name); + types.put(d.entry, d.type); + continue; + } + if (rec.isControl()) continue; + + String name = names.getOrDefault(rec.getEntry(), "?entry" + rec.getEntry()); + String type = types.getOrDefault(rec.getEntry(), ""); + + try { + if ("double".equals(type)) { + double v = rec.getDouble(); + mins.merge(name, v, Math::min); + maxes.merge(name, v, Math::max); + counts.merge(name, 1L, Long::sum); + } else if ("boolean".equals(type) && name.contains("Gyro") && name.endsWith("Connected")) { + if (!rec.getBoolean()) gyroDisconnected.add(name); + } else if (isVisionEntry(name)) { + if (isPoseArray(type) && (name.contains("Accepted") || name.contains("Rejected"))) { + // getRaw() returns the raw struct bytes; Pose3d[] has 0 bytes when empty, + // N×56 bytes when non-empty (no length prefix in WPILib struct encoding). + int rawLen = rec.getRaw().length; + visionPoseMaxRaw.merge(name, rawLen, Math::max); + } else if ("boolean".equals(type) && name.endsWith("Connected")) { + if (!rec.getBoolean()) visionCamsDisconnected.add(name); + } + } + } catch (Exception ignored) { + // record type mismatch — skip + } + } + + double durationSec = (lastTs == 0) ? 0 : (lastTs - firstTs) / 1_000_000.0; + System.out.printf("Duration : %.2f s%n", durationSec); + System.out.printf("Entries : %d%n", names.size()); + if (isReplay) System.out.println("Log type : REPLAY (_sim.wpilog)"); + System.out.println(); + + // --- All entries --- + System.out.println("=== All Entries ==="); + new TreeMap<>(names) + .forEach((id, n) -> System.out.printf(" %-60s (%s)%n", n, types.getOrDefault(id, "?"))); + System.out.println(); + + // --- Numeric statistics --- + System.out.println("=== Numeric Statistics (min / max) ==="); + mins.forEach( + (n, min) -> { + double max = maxes.getOrDefault(n, min); + long cnt = counts.getOrDefault(n, 0L); + System.out.printf( + " %-60s %10.4f / %10.4f (n=%d)%n", truncate(n, 60), min, max, cnt); + }); + System.out.println(); + + // --- Anomaly flags --- + System.out.println("=== Anomaly Flags ==="); + boolean[] found = {false}; + + // Loop overrun: AdvantageKit records actual wall time per cycle in FullCycleMS. + double maxCycleMs = maxes.getOrDefault("/RealOutputs/LoggedRobot/FullCycleMS", 0.0); + if (maxCycleMs > LOOP_OVERRUN_MS) { + System.out.printf( + " [WARN] Loop overrun: FullCycleMS max=%.1f ms (threshold %.0f ms)%n", + maxCycleMs, LOOP_OVERRUN_MS); + found[0] = true; + } + + mins.forEach( + (n, min) -> { + double max = maxes.getOrDefault(n, min); + if (n.contains("CurrentAmps") && max > CURRENT_OVERLOAD_AMPS) { + System.out.printf(" [WARN] High current: %s max=%.1f A%n", n, max); + found[0] = true; + } + }); + + gyroDisconnected.forEach( + n -> { + System.out.printf(" [WARN] Gyro disconnected during log: %s%n", n); + found[0] = true; + }); + + // Vision anomalies + if (!visionPoseMaxRaw.isEmpty() || !visionCamsDisconnected.isEmpty()) { + System.out.println(); + System.out.println(" --- Vision ---"); + + visionCamsDisconnected.forEach( + n -> { + System.out.printf(" [WARN] Camera disconnected: %s%n", n); + found[0] = true; + }); + + boolean hasAcceptedKeys = + visionPoseMaxRaw.keySet().stream() + .anyMatch(k -> k.contains("Accepted") && !k.contains("Summary")); + boolean anyAccepted = + visionPoseMaxRaw.entrySet().stream() + .anyMatch( + e -> + e.getKey().contains("Accepted") + && !e.getKey().contains("Summary") + && e.getValue() > 0); + boolean anyRejected = + visionPoseMaxRaw.entrySet().stream() + .anyMatch(e -> e.getKey().contains("Rejected") && e.getValue() > 0); + + if (hasAcceptedKeys && !anyAccepted && anyRejected) { + System.out.println( + " [WARN] Vision: observations detected but all rejected " + + "— check ambiguity/field-bounds filters"); + found[0] = true; + } + + visionPoseMaxRaw.forEach( + (n, maxRaw) -> { + int maxPoses = (maxRaw > 0) ? Math.max(1, maxRaw / POSE3D_BYTES) : 0; + System.out.printf(" [INFO] %-58s max_poses/frame=%d%n", truncate(n, 58), maxPoses); + }); + } + + if (!found[0]) { + System.out.println(" No anomalies detected."); + } + } + + // --------------------------------------------------------------------------- + // Helpers + // --------------------------------------------------------------------------- + + private static boolean isVisionEntry(String name) { + return name.contains("/Vision/"); + } + + private static boolean isPoseArray(String type) { + // AdvantageKit generates "struct:Pose3d[]" for Pose3d[] @AutoLog fields. + return type.contains("Pose3d") && type.contains("[]"); + } + + private static String findLatestLog() throws Exception { + Path dir = Paths.get("logs"); + if (!Files.isDirectory(dir)) return null; + return Files.list(dir) + .filter(p -> p.toString().endsWith(".wpilog")) + .max(Comparator.comparingLong(p -> p.toFile().lastModified())) + .map(Path::toString) + .orElse(null); + } + + private static String truncate(String s, int max) { + return s.length() <= max ? s : "..." + s.substring(s.length() - (max - 3)); + } +} diff --git a/src/main/java/org/frc5010/common/utils/OrchestraManager.java b/src/main/java/org/frc5010/common/utils/OrchestraManager.java new file mode 100644 index 00000000..1ebf3839 --- /dev/null +++ b/src/main/java/org/frc5010/common/utils/OrchestraManager.java @@ -0,0 +1,92 @@ +package org.frc5010.common.utils; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Orchestra; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.configs.AudioConfigs; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.Filesystem; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import org.frc5010.common.config.json.devices.OrchestraConfigJson; + +/** Manages CTRE Orchestra playback through all robot TalonFX motors during disabled mode. */ +public class OrchestraManager { + private static Orchestra orchestra; + private static Map musicMap; + private static OrchestraConfigJson musicConfigJson; + private static List motors = new ArrayList<>(); + + public static void init(OrchestraConfigJson configJson) { + orchestra = new Orchestra(); + musicMap = new HashMap<>(); + musicConfigJson = configJson; + for (OrchestraConfigJson.MusicEntry entry : musicConfigJson.music) { + musicMap.put(entry.name, entry.path); + } + } + + public static void loadMusic(String musicFileName) { + if (null != orchestra) { + motors.clear(); + for (int id : musicConfigJson.rioIds) { + TalonFX motor = new TalonFX(id); + AudioConfigs config = new AudioConfigs().withAllowMusicDurDisable(true); + motor.getConfigurator().apply(config, 0); + orchestra.addInstrument(motor); + motors.add(motor); + } + CANBus canivoreBus = new CANBus("canivore"); + for (int id : musicConfigJson.canivoreIds) { + TalonFX motor = new TalonFX(id, canivoreBus); + AudioConfigs config = new AudioConfigs().withAllowMusicDurDisable(true); + motor.getConfigurator().apply(config, 0); + orchestra.addInstrument(motor, 0); + motors.add(motor); + } + orchestra.loadMusic( + Filesystem.getDeployDirectory().toPath().resolve(musicMap.get(musicFileName)).toString()); + } + } + + /** Start playing the mariachi song. Safe to call repeatedly. */ + public static void play() { + if (null != orchestra) { + StatusCode status = orchestra.play(); + if (!status.isOK()) { + System.err.println("Failed to play music: " + status); + } + } + } + + public static boolean isPlaying() { + return null != orchestra && orchestra.isPlaying(); + } + + /** Stop playback and release motors back to normal control. */ + public static void stop() { + if (null != orchestra) { + orchestra.stop(); + } + } + + public static void playTone(double frequencyHz) { + if (motors != null) { + com.ctre.phoenix6.controls.MusicTone tone = + new com.ctre.phoenix6.controls.MusicTone(frequencyHz); + for (TalonFX motor : motors) { + motor.setControl(tone); + } + } + } + + public static void stopTone() { + if (motors != null) { + for (TalonFX motor : motors) { + motor.setControl(new com.ctre.phoenix6.controls.NeutralOut()); + } + } + } +} diff --git a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnField.java b/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnField.java deleted file mode 100644 index d00e3f82..00000000 --- a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnField.java +++ /dev/null @@ -1,21 +0,0 @@ -// 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 org.frc5010.lobbinloco; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation; - -/** Add your docs here. */ -public class FRC5010BallOnField extends GamePieceOnFieldSimulation { - public FRC5010BallOnField() { - super(LobbinLoco.LOBBINLOCO_BALL_INFO, new Pose2d(0, 0, new Rotation2d())); - } - - public FRC5010BallOnField(Translation2d initialPosition) { - super(LobbinLoco.LOBBINLOCO_BALL_INFO, new Pose2d(initialPosition, new Rotation2d())); - } -} diff --git a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnTheFly.java b/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnTheFly.java deleted file mode 100644 index 3a656088..00000000 --- a/src/main/java/org/frc5010/lobbinloco/FRC5010BallOnTheFly.java +++ /dev/null @@ -1,48 +0,0 @@ -// 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 org.frc5010.lobbinloco; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile; - -/** Add your docs here. */ -public class FRC5010BallOnTheFly extends GamePieceProjectile { - private static Runnable hitBoxCallBack = () -> System.out.println("hit target!"); - - public static void setHitBoxCallBack(Runnable callBack) { - hitBoxCallBack = callBack; - } - - public FRC5010BallOnTheFly( - Translation2d robotPosition, - Translation2d shooterPositionOnRobot, - ChassisSpeeds chassisSpeeds, - Rotation2d shooterFacing, - Distance initialHeight, - LinearVelocity launchingSpeed, - Angle shooterAngle) { - super( - LobbinLoco.LOBBINLOCO_BALL_INFO, - robotPosition, - shooterPositionOnRobot, - chassisSpeeds, - shooterFacing, - initialHeight, - launchingSpeed, - shooterAngle); - super.withTouchGroundHeight(0.8); - super.enableBecomesGamePieceOnFieldAfterTouchGround(); - super.withTargetTolerance( - new Translation3d(LobbinLoco.GOAL_LENGTH, LobbinLoco.GOAL_WIDTH, LobbinLoco.BALL_HEIGHT) - .div(2.0)); - super.withHitTargetCallBack(hitBoxCallBack); - } -} diff --git a/src/main/java/org/frc5010/lobbinloco/LobbinLoco.java b/src/main/java/org/frc5010/lobbinloco/LobbinLoco.java deleted file mode 100644 index 6c6282b2..00000000 --- a/src/main/java/org/frc5010/lobbinloco/LobbinLoco.java +++ /dev/null @@ -1,116 +0,0 @@ -// 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 org.frc5010.lobbinloco; - -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Kilograms; -import static edu.wpi.first.units.Units.Meters; - -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.units.Units; -import org.dyn4j.geometry.Circle; -import swervelib.simulation.ironmaple.simulation.SimulatedArena; -import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation.GamePieceInfo; - -/** Add your docs here. */ -public class LobbinLoco extends SimulatedArena { - public static final double FIELD_LENGTH = Units.Feet.of(16).in(Units.Meters); - public static final double FIELD_OVERALL_LENGTH = Units.Feet.of(50).in(Units.Meters); - public static final double FIELD_WIDTH = Units.Feet.of(20).in(Units.Meters); - public static final double GOAL_LENGTH = Inches.of(36).in(Meters); - public static final double GOAL_WIDTH = Inches.of(36).in(Meters); - public static final double GOAL_HEIGHT = Inches.of(8).in(Meters); - public static final double BALL_HEIGHT = Inches.of(5).in(Meters); - - // Dimensions in meters (50 feet x 20 feet) - private static final Translation2d bottomLeft = new Translation2d(0.0, 0.0); - private static final Translation2d bottomRight = new Translation2d(FIELD_LENGTH, 0); - private static final Translation2d bottomRightOverall = - new Translation2d(FIELD_OVERALL_LENGTH, 0); - private static final Translation2d topLeft = new Translation2d(0.0, FIELD_WIDTH); - private static final Translation2d topRight = new Translation2d(FIELD_LENGTH, FIELD_WIDTH); - private static final Translation2d topRightOverall = - new Translation2d(FIELD_OVERALL_LENGTH, FIELD_WIDTH); - - public static final GamePieceInfo LOBBINLOCO_BALL_INFO = - new GamePieceInfo( - "FRC5010Ball", - new Circle(Inches.of(5).in(Meters)), - Inches.of(5), - Kilograms.of(0.1), - 1.8, - 5, - 0.8); - - private static class LobbinLoboFieldMap extends FieldMap { - public LobbinLoboFieldMap() { - addBorderLine(bottomLeft, bottomRight); // _ - addBorderLine(bottomRight, bottomRightOverall); // __ - addBorderLine(bottomRight, topRight); // _|_ - addBorderLine(bottomRightOverall, topRightOverall); // _|_| - addBorderLine(topRight, topRightOverall); // - addBorderLine(topRight, topLeft); - addBorderLine(topLeft, bottomLeft); - - defineGoal(288, 30); // 18 - defineGoal(288, 170); // 17 - defineGoal(432, 80); // 20 - defineGoal(432, 120); // 19 - defineGoal(528, 10); // 22 - defineGoal(528, 190); // 21 - } - - private void defineGoal(double x, double y) { - double xInMeters = Inches.of(x).in(Meters); - double yInMeters = Inches.of(y).in(Meters); - // Goal Right Wall - // this.addBorderLine( - // new Translation2d(xInMeters, yInMeters), - // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters)); - this.addRectangularObstacle( - GOAL_LENGTH, GOAL_HEIGHT, new Pose2d(xInMeters, yInMeters, new Rotation2d())); - // Goal Front Wall - // this.addBorderLine( - // new Translation2d(xInMeters, yInMeters), - // new Translation2d(xInMeters, yInMeters + GOAL_WIDTH)); - this.addRectangularObstacle( - GOAL_LENGTH, GOAL_HEIGHT, new Pose2d(xInMeters, yInMeters, new Rotation2d(90.0))); - // Goal Left Wall - // this.addBorderLine( - // new Translation2d(xInMeters, yInMeters + GOAL_WIDTH), - // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters + GOAL_WIDTH)); - this.addRectangularObstacle( - GOAL_LENGTH, - GOAL_HEIGHT, - new Pose2d(xInMeters, yInMeters + GOAL_WIDTH, new Rotation2d())); - // Goal Back Wall - // this.addBorderLine( - // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters), - // new Translation2d(xInMeters + GOAL_LENGTH, yInMeters + GOAL_WIDTH)); - this.addRectangularObstacle( - GOAL_LENGTH, - GOAL_HEIGHT, - new Pose2d(xInMeters + GOAL_LENGTH, yInMeters + GOAL_WIDTH, new Rotation2d(-90.0))); - } - } - - private static final LobbinLoboFieldMap fieldMap = new LobbinLoboFieldMap(); - - public LobbinLoco() { - super(fieldMap); - } - - /** - * Places game pieces on the field according to the current game configuration. This method is - * called by the SimulatedArena class when the game starts. The method is responsible for placing - * the game pieces in the correct positions on the simulated field. - */ - @Override - public void placeGamePiecesOnField() { - // no pieces on the field to start - } -} diff --git a/src/main/resources/schemas/yams-arm.schema.json b/src/main/resources/schemas/yams-arm.schema.json index 42ba7318..e1597276 100644 --- a/src/main/resources/schemas/yams-arm.schema.json +++ b/src/main/resources/schemas/yams-arm.schema.json @@ -59,6 +59,10 @@ "horizontalZero": { "$ref": "./unit-value.schema.json", "description": "Angle at which arm is horizontal (degrees)" + }, + "useTorqueCurrentFOC": { + "type": "boolean", + "description": "Whether direct TorqueCurrentFOC requests should be used when a real TalonFX is available" } } } diff --git a/src/test/java/org/frc5010/common/motors/SystemIdentificationTest.java b/src/test/java/org/frc5010/common/motors/SystemIdentificationTest.java new file mode 100644 index 00000000..665b5e17 --- /dev/null +++ b/src/test/java/org/frc5010/common/motors/SystemIdentificationTest.java @@ -0,0 +1,78 @@ +// 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 org.frc5010.common.motors; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; + +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +class SystemIdentificationTest { + + @Test + void testComputeFeedforwardCoefficientsWithAcceleration() { + double kS = 0.2; + double kV = 1.5; + double kA = 0.3; + double dt = 0.02; + + List velocitySamples = new ArrayList<>(); + List voltageSamples = new ArrayList<>(); + List timeSamples = new ArrayList<>(); + + double time = 0.0; + double velocity = 0.0; + for (int i = 0; i < 120; i++) { + double accel = 0.6 + 0.25 * Math.sin(i * 0.15); + velocity += accel * dt; + double voltage = kS + kV * velocity + kA * accel; + velocitySamples.add(velocity); + voltageSamples.add(voltage); + timeSamples.add(time); + time += dt; + } + + double[] coefficients = + SystemIdentification.computeFeedforwardCoefficients( + velocitySamples, voltageSamples, timeSamples); + + assertNotNull(coefficients); + assertEquals(kS, coefficients[0], 1e-2); + assertEquals(kV, coefficients[1], 1e-2); + assertEquals(kA, coefficients[2], 1e-2); + } + + @Test + void testComputeFeedforwardCoefficientsWithoutAccelerationData() { + double kS = 0.1; + double kV = 2.0; + double dt = 0.02; + + List velocitySamples = new ArrayList<>(); + List voltageSamples = new ArrayList<>(); + List timeSamples = new ArrayList<>(); + + double velocity = 0.0; + double time = 0.0; + for (int i = 0; i < 30; i++) { + double voltage = kS + kV * velocity; + velocitySamples.add(velocity); + voltageSamples.add(voltage); + timeSamples.add(time); + velocity += 0.25; + time += dt; + } + + double[] coefficients = + SystemIdentification.computeFeedforwardCoefficients(velocitySamples, voltageSamples, null); + + assertNotNull(coefficients); + assertEquals(kS, coefficients[0], 1e-6); + assertEquals(kV, coefficients[1], 1e-6); + assertEquals(0.0, coefficients[2], 1e-9); + } +} diff --git a/temp_yams/META-INF/MANIFEST.MF b/temp_yams/META-INF/MANIFEST.MF new file mode 100644 index 00000000..58630c02 --- /dev/null +++ b/temp_yams/META-INF/MANIFEST.MF @@ -0,0 +1,2 @@ +Manifest-Version: 1.0 +