diff --git a/.SysId/sysid-window.json b/.SysId/sysid-window.json new file mode 100644 index 00000000..8d0d6d46 --- /dev/null +++ b/.SysId/sysid-window.json @@ -0,0 +1,57 @@ +{ + "MainWindow": { + "GLOBAL": { + "font": "Proggy Dotted", + "fps": "120", + "height": "720", + "maximized": "1", + "style": "0", + "userScale": "2", + "width": "1280", + "xpos": "0", + "ypos": "29" + } + }, + "Window": { + "###Analyzer": { + "Collapsed": "0", + "Pos": "320,25", + "Size": "360,689" + }, + "###Data Selector": { + "Collapsed": "0", + "Pos": "5,480", + "Size": "310,291" + }, + "###Log Loader": { + "Collapsed": "0", + "Pos": "5,25", + "Size": "310,450" + }, + "###Program Log": { + "Collapsed": "0", + "Pos": "327,701", + "Size": "360,135" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Diagnostic Plots": { + "Collapsed": "0", + "Pos": "685,25", + "Size": "1225,779" + }, + "Exception Caught!": { + "Collapsed": "0", + "Pos": "720,431", + "Size": "480,128" + }, + "Override Units": { + "Collapsed": "0", + "Pos": "481,289", + "Size": "320,144" + } + } +} 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/.vscode/settings.json b/.vscode/settings.json index 8328c83c..3b81250d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -29,139 +29,6 @@ null ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.import.gradle.annotationProcessing.enabled": false, - "java.completion.favoriteStaticMembers": [ - "org.junit.Assert.*", - "org.junit.Assume.*", - "org.junit.jupiter.api.Assertions.*", - "org.junit.jupiter.api.Assumptions.*", - "org.junit.jupiter.api.DynamicContainer.*", - "org.junit.jupiter.api.DynamicTest.*", - "org.mockito.Mockito.*", - "org.mockito.ArgumentMatchers.*", - "org.mockito.Answers.*", - "edu.wpi.first.units.Units.*" - ], - "java.completion.filteredTypes": [ - "java.awt.*", - "com.sun.*", - "sun.*", - "jdk.*", - "org.graalvm.*", - "io.micrometer.shaded.*", - "java.beans.*", - "java.util.Base64.*", - "java.util.Timer", - "java.sql.*", - "javax.swing.*", - "javax.management.*", - "javax.smartcardio.*", - "edu.wpi.first.math.proto.*", - "edu.wpi.first.math.**.proto.*", - "edu.wpi.first.math.**.struct.*", - null - ], - "java.dependency.enableDependencyCheckup": false, - "json.schemas": [ - { - "fileMatch": [ - "**/akit_swerve_drivetrain.json" - ], - "url": "./src/main/resources/schemas/akit_swerve_drivetrain.schema.json" - }, - { - "fileMatch": [ - "**/robots.json" - ], - "url": "./src/main/resources/schemas/robots.schema.json" - }, - { - "fileMatch": [ - "**/robot.json" - ], - "url": "./src/main/resources/schemas/robot.schema.json" - }, - { - "fileMatch": [ - "**/competition_mode.json", - "**/demo_mode.json" - ], - "url": "./src/main/resources/schemas/user-mode.schema.json" - }, - { - "fileMatch": [ - "**/cameras.json" - ], - "url": "./src/main/resources/schemas/cameras.schema.json" - }, - { - "fileMatch": [ - "**/cameras/**/*.json", - "!**/cameras.json" - ], - "url": "./src/main/resources/schemas/camera.schema.json" - }, - { - "fileMatch": [ - "**/controllers.json" - ], - "url": "./src/main/resources/schemas/controllers.schema.json" - }, - { - "fileMatch": [ - "**/controllers/*.json", - "!**/controllers/axis/**/*.json" - ], - "url": "./src/main/resources/schemas/driveteam-controller.schema.json" - }, - { - "fileMatch": [ - "**/controllers/axis/*.json" - ], - "url": "./src/main/resources/schemas/driveteam-controller-axis.schema.json" - }, - { - "fileMatch": [ - "**/led_strip.json" - ], - "url": "./src/main/resources/schemas/led-strip.schema.json" - }, - { - "fileMatch": [ - "**/subsystems/example/percent_motor.json" - ], - "url": "./src/main/resources/schemas/percent-motor.schema.json" - }, - { - "fileMatch": [ - "**/subsystems/example/velocity_motor.json" - ], - "url": "./src/main/resources/schemas/velocity-motor.schema.json" - }, - { - "fileMatch": [ - "**/subsystems/example/yams_arm.json" - ], - "url": "./src/main/resources/schemas/yams-arm.schema.json" - }, - { - "fileMatch": [ - "**/subsystems/example/yams_elevator.json" - ], - "url": "./src/main/resources/schemas/yams-elevator.schema.json" - }, - { - "fileMatch": [ - "**/subsystems/example/yams_pivot.json" - ], - "url": "./src/main/resources/schemas/yams-pivot.schema.json" - }, - { - "fileMatch": [ - "**/subsystems/example/yams_shooter.json" - ], - "url": "./src/main/resources/schemas/yams-shooter.schema.json" - } - ], + "java.debug.settings.onBuildFailureProceed": true, "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 00000000..d4102b64 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,62 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## What this is + +FRC team 5010's 2026 robot code (WPILib, Java 17, command-based). It is built on a reusable in-house framework (`org.frc5010.common`, "FRC5010Lib") that is **JSON-configuration-driven**: robots, subsystems, drivetrains, controllers, and cameras are described by deploy-time JSON rather than hardcoded, so one codebase runs multiple physical robots. AdvantageKit (`org.littletonrobotics.junction`) provides logging/replay; the `Robot` class extends `LoggedRobot`. + +## Build / run commands + +The build runs Spotless (`compileJava.dependsOn(spotlessApply)`), and Spotless's `googleJavaFormat()` crashes on newer JDKs in some environments (`NoSuchMethodError ... JCImport.getQualifiedIdentifier`). When that happens, exclude the Spotless tasks: + +```bash +./gradlew compileJava -x spotlessJava -x spotlessJavaCheck # compile only +./gradlew build -x spotlessJava -x spotlessJavaCheck # full build +./gradlew test -x spotlessJava -x spotlessJavaCheck # JUnit 5 tests (src/test/java) +./gradlew test --tests 'org.frc5010.common.arch.GenericRobotTest' # single test class +./gradlew simulateJavaRelease # desktop/maple-sim simulation +./gradlew deploy # deploy to the roboRIO +``` + +Spotless config: `googleJavaFormat`, `removeUnusedImports`, 100-col limit. Run `./gradlew spotlessApply` locally before pushing where the toolchain works; otherwise match the style by hand (2-space indent, ≤100 col code lines — comments/Javadoc are left alone by the formatter). + +Chromium/Playwright are preinstalled in the cloud env; do not run `playwright install`. + +## How a robot is selected and built (read these together) + +1. `frc.robot.Main` → `frc.robot.Robot` (`LoggedRobot`) → `RobotContainer` → `RobotsParser`. +2. `RobotsParser` reads `src/main/deploy/robots.json`, which maps a robot **name → { id (roboRIO MAC), robotClass, simulate, competition }**. The active robot is chosen by matching the RIO's MAC, or in simulation by the `simulate: true` flag. The matching entry's `robotClass` (e.g. `frc.robot.blackteam.BlackRobot`) is instantiated via reflection with its **deploy directory name** (e.g. `buttercup_swerve`). +3. `GenericRobot(String directory)` constructs `RobotParser` + `SubsystemParser`, which read the JSON files under `src/main/deploy//` and build the drivetrain, subsystems, cameras, and controllers. **Exceptions during this phase are caught in `GenericRobot`'s constructor, printed, and swallowed** — a failed parse leaves a half-built robot rather than a crash, so "the drivetrain didn't get created" usually means an exception was thrown and logged during config parsing. + +So robot behavior is determined largely by `src/main/deploy//*.json`, not by Java edits. The two configured robots share `BlackRobot` but have different deploy dirs (`black_robot`, `buttercup_swerve`). + +## Code layout + +- `src/main/java/org/frc5010/common/` — the framework library: + - `arch/` — `GenericRobot`, `GenericSubsystem`, `GenericCommand`, NetworkTables value plumbing. + - `config/` — JSON parsers (`RobotParser`, `SubsystemParser`, `RobotsParser`) and the `config/json/**` POJOs that map 1:1 to the deploy JSON. `UnitsParser` + `config/units/**` parse `{ "val", "uom" }` measurements (units accept aliases, e.g. `m/sec`, `kg*m^2`, `degrees`). + - `drive/` — drivetrain abstractions. See the swerve section below. + - `telemetry/` — `DisplayValuesHelper` + `Display*` types wrap NetworkTables entries; `makeConfig*` values are persistent and dashboard-editable (used for live tuning). + - `sensors/`, `motors/`, `mechanisms/`, `subsystems/`, `vision/`, `auto/`, `commands/`. +- `src/main/java/frc/robot/` — WPILib entrypoint + robot-specific code; `blackteam/BlackRobot` is the concrete `GenericRobot` and its subsystems (flywheels, feeder). +- `src/main/deploy//` — per-robot JSON config (robot.json, drivetrain, subsystems, controllers, cameras, pathplanner). +- `src/main/resources/schemas/` — JSON schemas for the config files (e.g. `akit_swerve_drivetrain.schema.json`); validate config changes against these. + +## Swerve drivetrain architecture (the most complex area) + +`GenericSwerveDrivetrain` (the WPILib `Subsystem`) wraps a `SwerveDriveFunctions` implementation chosen by `robot.json`'s `driveType`: + +- `YAGSL_SWERVE_DRIVE` → `YAGSLSwerveDrivetrain` (the YAGSL/`swervelib` library). Configured by `deploy//yagsl_swerve/**` JSON (modules, `pidfproperties.json`, `physicalproperties.json`, `swervedrive.json`). +- `AKIT_SWERVE_DRIVE` → `AkitSwerveDrive` (AdvantageKit-style IO-layer swerve). Configured by `deploy//akit_swerve_drivetrain.json` (validated by the schema). `type` selects the module IO: `TalonFX`, `SparkTalon` (TalonFX drive + Spark steer), or `Spark` (NEO/SparkMax drive + steer). + +Key AKit-swerve facts that are non-obvious and have caused real bugs: + +- **The `Spark` IO ignores most of the JSON** — `ModuleIOSpark` reads CAN IDs, gear ratios, drive PID, current limits, and per-module `zeroRotation` from the hardcoded `drive/swerve/akit/DriveConstants.java`, **not** from `akit_swerve_drivetrain.json`. The JSON still drives kinematics/PathPlanner/sim. To change Spark hardware behavior, edit `DriveConstants`. (`SparkTalon`/`TalonFX` IOs do read the JSON via `AkitSwerveConfig`.) +- **Two odometry threads.** `ModuleIOSpark`/`ModuleIOSparkTalon` sample on `SparkOdometryThread`; `GyroIOPigeon2` (a CTRE device) samples on `TalonFXOdometryThread`. Both must be created for the gyro to work, and `AkitSwerveDrive.periodic` must tolerate the two queues having different lengths. +- **Steering runs closed-loop on the absolute encoder.** Canandmags are read via the Spark's absolute-encoder API; offsets rely on the encoder's internal zero (config offsets are `0`). `Module.runSetpoint` cosine-scales drive speed by the *measured* steering angle and `optimize()`s against it, so the module IO must populate `inputs.turnPosition` (not just `turnAbsolutePosition`) or strafe/odometry break. A module whose zero leaves it resting at ±180° sits on the wrap seam and will intermittently flip direction. +- Simulation skips the hardware IO entirely (uses `ModuleIOSim`/`GyroIOSim` + maple-sim) and never starts the odometry threads, so hardware-thread/encoder bugs do **not** reproduce in sim. + +## Git / workflow + +Active development branches are created per-task off the team's feature branches (e.g. `claude/...`). The default branch is protected — branch before committing. Do not push to a different branch without explicit permission. GitHub operations go through the `mcp__github__*` tools (scope: `frc5010/teamblack2025`); there is no `gh` CLI. diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/simgui.json b/simgui.json index 2cfd91f7..1e98610d 100644 --- a/simgui.json +++ b/simgui.json @@ -2,20 +2,19 @@ "HALProvider": { "Addressable LEDs": { "0": { - "columns": 15, - "order": 1 + "columns": 17 }, "window": { "visible": true } }, "Other Devices": { - "SPARK MAX [5]": { + "SPARK MAX [12]": { "header": { "open": true } }, - "SPARK MAX [7]": { + "SPARK MAX [12] RELATIVE ENCODER": { "header": { "open": true } @@ -40,6 +39,11 @@ "open": true } }, + "Talon FX (v6)[2]/Rotor Sensor": { + "header": { + "open": true + } + }, "Talon FX (v6)[51]": { "header": { "open": true @@ -84,52 +88,85 @@ "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/Shuffleboard/BabySwerve/Auto Modes": "String Chooser", + "/Shuffleboard/BlackRobot/Auto Modes": "String Chooser", "/Shuffleboard/ExampleRobot/Auto Modes": "String Chooser", "/SmartDashboard/Alerts": "Alerts", "/SmartDashboard/Arm/mechanism": "Mechanism2d", "/SmartDashboard/Auto Modes": "String Chooser", + "/SmartDashboard/BlackRobot/Auto Modes": "String Chooser", "/SmartDashboard/Drive Visual": "Mechanism2d", "/SmartDashboard/DrivePoseEstimator/Pose Field": "Field2d", "/SmartDashboard/DrivePoseEstimator/values/Pose Field": "Field2d", "/SmartDashboard/Elevator/mechanism": "Mechanism2d", "/SmartDashboard/Encoders": "Alerts", "/SmartDashboard/ExampleRobot/Auto Modes": "String Chooser", + "/SmartDashboard/FeederMech/mechanism": "Mechanism2d", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Hood/mechanism": "Mechanism2d", "/SmartDashboard/IMU": "Alerts", "/SmartDashboard/JSON": "Alerts", + "/SmartDashboard/LowerShooterMech/mechanism": "Mechanism2d", + "/SmartDashboard/Mechanisms/Commands/FeederMotor/Down": "Command", + "/SmartDashboard/Mechanisms/Commands/FeederMotor/Up": "Command", + "/SmartDashboard/Mechanisms/Commands/FeederMotor/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/Commands/FeederSubsystem/Live Tuning": "Command", + "/SmartDashboard/Mechanisms/FeederMech/FeederMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/FeederMech/FeederMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/FeederMech/FeederMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/FeederMech/FeederMotor/Commands/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/LowerShooterMech/LowerShooterMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/LowerShooterMech/LowerShooterMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/LowerShooterMech/LowerShooterMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/LowerShooterMech/LowerShooterMotor/Commands/ZeroEncoder": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/Down": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/LiveTuning": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/Up": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/Commands/ZeroEncoder": "Command", "/SmartDashboard/Mechanisms/Shooter/ShooterMotor/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/LowerShooterMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/LowerShooterMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/LowerShooterMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/LowerShooterMotor/Commands/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/ShooterMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/ShooterMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/ShooterMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/ShooterMech/ShooterMotor/Commands/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/LowerShooterMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/LowerShooterMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/LowerShooterMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/LowerShooterMotor/Commands/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/UpperShooterMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/UpperShooterMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/UpperShooterMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/UpperShooterMech/UpperShooterMotor/Commands/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/lowerflywheel/lowerflywheelMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/lowerflywheel/lowerflywheelMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/lowerflywheel/lowerflywheelMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/lowerflywheel/lowerflywheelMotor/Commands/ZeroEncoder": "Command", + "/SmartDashboard/Mechanisms/upperflywheel/upperflywheelMotor/Commands/Down": "Command", + "/SmartDashboard/Mechanisms/upperflywheel/upperflywheelMotor/Commands/LiveTuning": "Command", + "/SmartDashboard/Mechanisms/upperflywheel/upperflywheelMotor/Commands/Up": "Command", + "/SmartDashboard/Mechanisms/upperflywheel/upperflywheelMotor/Commands/ZeroEncoder": "Command", "/SmartDashboard/Motors": "Alerts", "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/PhotonAlerts": "Alerts", "/SmartDashboard/Pigeon 2 (v6) [13]": "Gyro", + "/SmartDashboard/Pigeon 2 (v6) [9]": "Gyro", "/SmartDashboard/Pigeon 2 [13]": "Gyro", "/SmartDashboard/Robot Visual": "Mechanism2d", "/SmartDashboard/SendableChooser[0]": "String Chooser", "/SmartDashboard/Shooter/mechanism": "Mechanism2d", + "/SmartDashboard/ShooterMech/mechanism": "Mechanism2d", "/SmartDashboard/Swerve Drive": "Alerts", "/SmartDashboard/Turret/mechanism": "Mechanism2d", + "/SmartDashboard/UpperShooterMech/mechanism": "Mechanism2d", "/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d", - "/SmartDashboard/hoodmotor/mechanism": "Mechanism2d", + "/SmartDashboard/lowerflywheel/mechanism": "Mechanism2d", "/SmartDashboard/navX-Sensor[1]": "Gyro", - "/SmartDashboard/navX-Sensor[4]": "Gyro" + "/SmartDashboard/navX-Sensor[4]": "Gyro", + "/SmartDashboard/upperflywheel/mechanism": "Mechanism2d" }, "windows": { - "/FMSInfo": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Drive Visual": { - "window": { - "visible": true - } - }, "/SmartDashboard/DrivePoseEstimator/Pose Field": { "CARPET0": { "image": ".\\pictures\\carpet.png", @@ -331,61 +368,11 @@ "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, @@ -418,358 +405,253 @@ }, "GPA0": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA1": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA10": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA11": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA12": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA13": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA14": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA15": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA16": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA17": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA18": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA19": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA2": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA20": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA21": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA22": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA23": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA24": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA25": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA26": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA27": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA28": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA29": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA3": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA30": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA31": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA32": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA33": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA34": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA35": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA36": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA37": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA38": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA39": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA4": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA40": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA41": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA42": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA43": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA44": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA45": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA46": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA47": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA48": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA49": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA5": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 - }, - "GPA50": { - "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA6": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA7": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA8": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPA9": { "image": ".\\pictures\\gpa.png", - "length": 0.15000000596046448, - "width": 0.15000000596046448 + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB0": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB1": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB10": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB11": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB12": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB13": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB14": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB15": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB16": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB17": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB18": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB19": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB2": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB20": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB21": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB22": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB23": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB24": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB3": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB4": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB5": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB6": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB7": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB8": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "GPB9": { - "length": 0.4000000059604645, - "width": 0.4000000059604645 + "image": ".\\pictures\\gpb.png", + "length": 0.12700000405311584, + "width": 0.12700000405311584 }, "OdometryPose": { "arrowColor": [ @@ -801,17 +683,17 @@ "arrowWeight": 3.0, "style": "Hidden" }, - "bottom": 1914, - "height": 8.069275856018066, - "left": 245, - "right": 3942, - "top": 118, - "width": 16.54119300842285, + "bottom": 599, + "builtin": "", + "height": 6.0, + "image": ".\\pictures\\LobbinLoco.png", + "right": 1432, + "width": 14.600000381469727, "window": { "visible": true } }, - "/SmartDashboard/ExampleRobot/Auto Modes": { + "/SmartDashboard/FeederMech/mechanism": { "window": { "visible": true } @@ -841,30 +723,33 @@ "left": 245, "right": 3942, "top": 118, - "width": 16.54119300842285 - }, - "/SmartDashboard/Robot Visual": { + "width": 16.54119300842285, "window": { "visible": true } }, - "/SmartDashboard/Shooter/mechanism": { + "/SmartDashboard/LowerShooterMech/mechanism": { "window": { "visible": true } }, "/SmartDashboard/VisionSystemSim-main/Sim Field": { - "bottom": 1914, - "height": 8.069275856018066, - "left": 245, - "right": 3942, - "top": 118, - "width": 16.54119300842285, + "bottom": 599, + "builtin": "", + "height": 6.0, + "image": ".\\pictures\\LobbinLoco.png", + "right": 1432, + "width": 14.600000381469727, + "window": { + "visible": true + } + }, + "/SmartDashboard/lowerflywheel/mechanism": { "window": { "visible": true } }, - "/SmartDashboard/hoodmotor/mechanism": { + "/SmartDashboard/upperflywheel/mechanism": { "window": { "visible": true } @@ -892,20 +777,15 @@ } }, "transitory": { - "AdvantageKit": { - "Drive": { - "open": true - }, - "Vision": { - "Camera left": { - "open": true - } - }, - "open": true - }, "Mechanisms": { - "Arm": { - "ArmMotor": { + "LowerShooterMech": { + "LowerShooterMotor": { + "closedloop": { + "open": true, + "setpoint": { + "open": true + } + }, "mechanism": { "open": true }, @@ -916,30 +796,9 @@ }, "open": true }, - "Hood": { - "HoodMotor": { - "open": true - } - }, - "Shooter": { - "ShooterMotor": { - "mechanism": { - "open": true - }, - "open": true, - "rotor": { - "open": true - } - } - }, "open": true }, "Shuffleboard": { - "ExampleSubsystem": { - "percent_motor": { - "open": true - } - }, "Vision": { "Camera shooter": { "open": true @@ -947,31 +806,24 @@ } }, "SmartDashboard": { - "Characterization": { - "Steer": { - "open": true - }, - "open": true - }, - "Drive Feedforward": { + "Camera": { "open": true }, "DrivePoseEstimator": { "DrivePoseEstimator": { "open": true }, + "open": true, "values": { "open": true } }, "ExampleRobot": { + "open": true, "values": { "open": true } }, - "Wheel Radius Characterization": { - "open": true - }, "open": true }, "Tuning": { @@ -989,6 +841,7 @@ } }, "photonvision": { + "open": true, "shooter": { "Transform3d##v_/photonvision/shooter/targetPose": { "Translation3d##v_translation": { @@ -999,5 +852,11 @@ } } } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables View": { + "visible": false } } 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/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/basic_robot/akit_swerve_drivetrain.json b/src/main/deploy/basic_robot/akit_swerve_drivetrain.json deleted file mode 100644 index f1a74ad6..00000000 --- a/src/main/deploy/basic_robot/akit_swerve_drivetrain.json +++ /dev/null @@ -1,178 +0,0 @@ -{ - "type": "TalonFX", - "constants": { - "trackWidth": { - "val": 22, - "uom": "inches" - }, - "wheelBase": { - "val": 22, - "uom": "inches" - }, - "wheelDiameter": { - "val": 3.955, - "uom": "inches" - }, - "maxDriveSpeed": { - "val": 4.69, - "uom": "m/sec" - }, - "bumperFrameLength": { - "val": 30, - "uom": "inches" - }, - "bumperFrameWidth": { - "val": 30, - "uom": "inches" - }, - "gyro": { - "type": "pigeon2", - "id": 50, - "inverted": false, - "canbus": "" - }, - "driveGearRatio": "1:6.75", - "steerGearRatio": "1:25", - "driveMotorControl": { - "feedBack": { - "p": 0.00001, - "i": 0.0, - "d": 0.0 - }, - "feedForward": { - "s": 0.0698163127444402, - "v": 0.11987949085405275, - "a": 0.0 - } - }, - "steerMotorControl": { - "feedBack": { - "p": 100, - "i": 0.0, - "d": 0.5 - }, - "feedForward": { - "s": 0.06242725612073021, - "v": 0.4513057312290134, - "a": 0.0 - } - }, - "modules": { - "frontLeft": { - "driveMotorSetup": { - "name": "frontLeftDrive", - "controllerType": "talonfx", - "motorType": "KrakenX60", - "canId": 4 - }, - "steerMotorSetup": { - "name": "frontLeftSteer", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 3 - }, - "encoderId": 13, - "absoluteOffset": { - "val": 0.15234375, - "uom": "rotations" - }, - "encoderInverted": false - }, - "frontRight": { - "driveMotorSetup": { - "name": "frontRightDrive", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 2 - }, - "steerMotorSetup": { - "name": "frontRightSteer", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 1 - }, - "encoderId": 16, - "absoluteOffset": { - "val": -0.4873046875, - "uom": "rotations" - }, - "encoderInverted": false - }, - "backLeft": { - "driveMotorSetup": { - "name": "backLeftDrive", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 6 - }, - "steerMotorSetup": { - "name": "backLeftSteer", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 5 - }, - "encoderId": 14, - "absoluteOffset": { - "val": -0.219482421875, - "uom": "rotations" - }, - "encoderInverted": false - }, - "backRight": { - "driveMotorSetup": { - "name": "backRightDrive", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 8 - }, - "steerMotorSetup": { - "name": "backRightSteer", - "controllerType": "talonfx", - "motorType": "Krakenx60", - "canId": 7 - }, - "encoderId": 15, - "absoluteOffset": { - "val": 0.17236328125, - "uom": "rotations" - }, - "encoderInverted": false - } - }, - "coupleRatio": 3.8181818181818183, - "invertLeftSide": true, - "invertRightSide": true, - "steerInertia": { - "val": 0.04, - "uom": "kg*m^2" - }, - "driveInertia": { - "val": 0.025, - "uom": "kg*m^2" - }, - "robotMass": { - "val": 50, - "uom": "kg" - }, - "wheelCOF": 1.2, - "slipCurrent": { - "val": 120, - "uom": "amps" - }, - "canbus": "", - "startingPose": { - "x": { - "val": 3.42, - "uom": "m" - }, - "y": { - "val": 5.75, - "uom": "m" - }, - "rotation": { - "val": -45, - "uom": "deg" - } - } - } -} 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/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/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/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/robot.json b/src/main/deploy/basic_robot/robot.json deleted file mode 100644 index 2475d0f1..00000000 --- a/src/main/deploy/basic_robot/robot.json +++ /dev/null @@ -1,15 +0,0 @@ -{ - "userConfig": "competition_mode.json", - "driveType": "AKIT_SWERVE_DRIVE", - "trackWidth": 16.8, - "trackWidthUom": "in", - "wheelBase": 18.5, - "wheelBaseUom": "in", - "wheelDiameter": 0.103386904, - "wheelDiameterUom": "m", - "physicalMaxSpeed": 5.93, - "physicalMaxSpeedUom": "m/s", - "driveMotorGearRatio": 1.0, - "loadSimulatedField": true, - "gamePieceA": "Fuel" -} 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/percent_motor.json b/src/main/deploy/basic_robot/subsystems/example/percent_motor.json deleted file mode 100644 index 93be5418..00000000 --- a/src/main/deploy/basic_robot/subsystems/example/percent_motor.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "name": "percent_motor", - "controller": "spark", - "type": "Neo", - "id": 11, - "gearing": 1.0, - "momentOfInertiaKgMSq": 1.0, - "x": 0.5, - "y": -0.092, - "z": 0.2 -} 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/basic_robot/subsystems/example/yams_arm.json b/src/main/deploy/basic_robot/subsystems/example/yams_arm.json deleted file mode 100644 index 2f588f40..00000000 --- a/src/main/deploy/basic_robot/subsystems/example/yams_arm.json +++ /dev/null @@ -1,102 +0,0 @@ -{ - "motorSetup": { - "name": "Hood", - "controllerType": "talonfx", - "motorType": "KrakenX60", - "canId": 13, - "robotToMotor": { - "x": { - "val": -8.2, - "uom": "in" - }, - "y": { - "val": 10.104, - "uom": "in" - }, - "z": { - "val": 15.786, - "uom": "in" - }, - "rotation": { - "val": 0.0, - "uom": "deg" - } - } - }, - "motorSystemId": { - "feedBack": { - "p": 4, - "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" - } - }, - "simSystemId": { - "feedBack": { - "p": 4, - "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": 0.234, - "uom": "m" - }, - "lowerHardLimit": { - "val": 55, - "uom": "deg" - }, - "upperHardLimit": { - "val": 100, - "uom": "deg" - }, - "startingAngle": { - "val": 55, - "uom": "deg" - }, - "lowerSoftLimit": { - "val": 55, - "uom": "deg" - }, - "upperSoftLimit": { - "val": 85, - "uom": "deg" - }, - "gearing": [ - 3, - 4 - ], - "mass": { - "val": 1, - "uom": "lbs" - }, - "horizontalZero": { - "val": 55, - "uom": "deg" - } -} diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json b/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json deleted file mode 100644 index e8cbe550..00000000 --- a/src/main/deploy/basic_robot/subsystems/example/yams_elevator.json +++ /dev/null @@ -1,98 +0,0 @@ -{ - "motorSetup": { - "name": "Elevator", - "controllerType": "spark", - "motorType": "Neo", - "canId": 10, - "robotToMotor": { - "x": { - "val": -10.7, - "uom": "in" - }, - "y": { - "val": -3.669, - "uom": "in" - }, - "z": { - "val": 5.323, - "uom": "in" - }, - "rotation": { - "val": 0.0, - "uom": "deg" - } - } - }, - "motorSystemId": { - "feedBack": { - "p": 2.7739E-10, - "i": 0.0, - "d": 0.0 - }, - "feedForward": { - "s": 0.013799, - "g": 0.24168, - "v": 10.219, - "a": 0.2153 - }, - "maxVelocity": { - "val": 4, - "uom": "m/s" - }, - "maxAcceleration": { - "val": 20, - "uom": "m/s^2" - }, - "controlMode": "CLOSED_LOOP" - }, - "simSystemId": { - "feedBack": { - "p": 2.7739E-10, - "i": 0.0, - "d": 0.0 - }, - "feedForward": { - "s": 0.013799, - "g": 0.24168, - "v": 10.219, - "a": 0.2153 - }, - "maxVelocity": { - "val": 4, - "uom": "m/s" - }, - "maxAcceleration": { - "val": 20, - "uom": "m/s^2" - } - }, - "sprocketTeeth": 22, - "lowerSoftLimit": { - "val": 0.1, - "uom": "m" - }, - "upperSoftLimit": { - "val": 2, - "uom": "m" - }, - "lowerHardLimit": { - "val": 0.1, - "uom": "m" - }, - "upperHardLimit": { - "val": 3, - "uom": "m" - }, - "gearing": [ - 3, - 4 - ], - "startingPosition": { - "val": 0.5, - "uom": "m" - }, - "mass": { - "val": 16, - "uom": "lbs" - } -} diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json b/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json deleted file mode 100644 index d3d3b2ba..00000000 --- a/src/main/deploy/basic_robot/subsystems/example/yams_pivot.json +++ /dev/null @@ -1,96 +0,0 @@ -{ - "motorSetup": { - "name": "Turret", - "controllerType": "talonfx", - "motorType": "KrakenX60", - "canId": 15, - "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" - } - }, - "movementPlane": "XY" - }, - "motorSystemId": { - "feedBack": { - "p": 4, - "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" - } - }, - "simSystemId": { - "feedBack": { - "p": 4, - "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" - } - }, - "lowerHardLimit": { - "val": -165, - "uom": "deg" - }, - "upperHardLimit": { - "val": 165, - "uom": "deg" - }, - "startingAngle": { - "val": 0, - "uom": "deg" - }, - "lowerSoftLimit": { - "val": -160, - "uom": "deg" - }, - "upperSoftLimit": { - "val": 160, - "uom": "deg" - }, - "gearStages": "12:36:40:12:108", - "radius": { - "val": 5, - "uom": "in" - }, - "mass": { - "val": 20, - "uom": "lb" - } -} diff --git a/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json b/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json deleted file mode 100644 index dc9a2c3b..00000000 --- a/src/main/deploy/basic_robot/subsystems/example/yams_shooter.json +++ /dev/null @@ -1,70 +0,0 @@ -{ - "motorSetup": { - "name": "Shooter", - "controllerType": "spark", - "motorType": "KrakenX44", - "canId": 12, - "robotToMotor": { - "x": { - "val": -5.872, - "uom": "in" - }, - "y": { - "val": 4.8, - "uom": "in" - }, - "z": { - "val": 18.72, - "uom": "in" - }, - "rotation": { - "val": 0.0, - "uom": "deg" - } - } - }, - "motorSystemId": { - "feedBack": { - "p": 4, - "i": 0.0, - "d": 0.0 - }, - "feedForward": { - "s": 0.0, - "v": 0.0, - "a": 0.0 - } - }, - "simSystemId": { - "feedBack": { - "p": 4, - "i": 0.0, - "d": 0.0 - }, - "feedForward": { - "s": 0.0, - "v": 0.0, - "a": 0.0 - } - }, - "lowerSoftLimit": { - "val": 0, - "uom": "rpm" - }, - "upperSoftLimit": { - "val": 5000, - "uom": "rpm" - }, - "gearing": [ - 3, - 4 - ], - "mass": { - "val": 2, - "uom": "kg" - }, - "radius": { - "val": 0.05, - "uom": "m" - } -} diff --git a/src/main/deploy/black_robot/akit_swerve_drivetrain.json b/src/main/deploy/black_robot/akit_swerve_drivetrain.json new file mode 100644 index 00000000..df46dc6f --- /dev/null +++ b/src/main/deploy/black_robot/akit_swerve_drivetrain.json @@ -0,0 +1,222 @@ +{ + "type": "Spark", + "constants": { + "trackWidth": { + "val": 18.5625, + "uom": "inches" + }, + "wheelBase": { + "val": 18.5625, + "uom": "inches" + }, + "wheelDiameter": { + "val": 4, + "uom": "inches" + }, + "maxDriveSpeed": { + "val": 5.93, + "uom": "m/sec" + }, + "bumperFrameLength": { + "val": 28, + "uom": "inches" + }, + "bumperFrameWidth": { + "val": 28, + "uom": "inches" + }, + "gyro": { + "type": "pigeon2", + "id": 9, + "inverted": false, + "canbus": "" + }, + "driveGearRatio": "1:6.75", + "steerGearRatio": "1:25", + "driveMotorControl": { + "closedLoopRamp": { + "val": 0.15, + "uom": "seconds" + }, + "feedBack": { + "p": 0.0020645, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.0, + "v": 0.1, + "a": 0.0 + } + }, + "steerMotorControl": { + "closedLoopRamp": { + "val": 0.05, + "uom": "seconds" + }, + "feedBack": { + "p": 2.0, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.0, + "v": 0.0, + "a": 0.0 + } + }, + "modules": { + "frontLeft": { + "driveMotorSetup": { + "name": "frontLeftDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 18, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "frontLeftSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 2, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + }, + "frontRight": { + "driveMotorSetup": { + "name": "frontRightDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 17, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "frontRightSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 6, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + }, + "backLeft": { + "driveMotorSetup": { + "name": "backLeftDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 16, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "backLeftSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 3, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + }, + "backRight": { + "driveMotorSetup": { + "name": "backRightDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 15, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "backRightSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 4, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + } + }, + "coupleRatio": 1.0, + "invertLeftSide": true, + "invertRightSide": true, + "steerInertia": { + "val": 0.04, + "uom": "kg*m^2" + }, + "driveInertia": { + "val": 0.025, + "uom": "kg*m^2" + }, + "robotMass": { + "val": 75, + "uom": "kg" + }, + "wheelCOF": 1.19, + "slipCurrent": { + "val": 40, + "uom": "amps" + }, + "canbus": "", + "startingPose": { + "x": { + "val": 1.0, + "uom": "m" + }, + "y": { + "val": 1.0, + "uom": "m" + }, + "rotation": { + "val": 0.0, + "uom": "deg" + } + } + } +} diff --git a/src/main/deploy/black_robot/cameras.json b/src/main/deploy/black_robot/cameras.json new file mode 100644 index 00000000..10e9384c --- /dev/null +++ b/src/main/deploy/black_robot/cameras.json @@ -0,0 +1,4 @@ +{ + "cameras": [], + "simulatedField": "org.frc5010.lobbinloco.LobbinLoco" +} diff --git a/src/main/deploy/basic_robot/cameras/intake.json b/src/main/deploy/black_robot/cameras/intake.json similarity index 100% rename from src/main/deploy/basic_robot/cameras/intake.json rename to src/main/deploy/black_robot/cameras/intake.json diff --git a/src/main/deploy/basic_robot/cameras/localization.json b/src/main/deploy/black_robot/cameras/localization.json similarity index 83% rename from src/main/deploy/basic_robot/cameras/localization.json rename to src/main/deploy/black_robot/cameras/localization.json index 481de59d..706b561a 100644 --- a/src/main/deploy/basic_robot/cameras/localization.json +++ b/src/main/deploy/black_robot/cameras/localization.json @@ -2,6 +2,7 @@ "name": "localization", "use": "apriltag", "type": "limelight", + "strategy": "LOWEST_AMBIGUITY", "column": 0, "x": -0.35, "y": -0.25, diff --git a/src/main/deploy/basic_robot/cameras/quest.json b/src/main/deploy/black_robot/cameras/quest.json similarity index 100% rename from src/main/deploy/basic_robot/cameras/quest.json rename to src/main/deploy/black_robot/cameras/quest.json diff --git a/src/main/deploy/basic_robot/cameras/shooter.json b/src/main/deploy/black_robot/cameras/shooter.json similarity index 100% rename from src/main/deploy/basic_robot/cameras/shooter.json rename to src/main/deploy/black_robot/cameras/shooter.json diff --git a/src/main/deploy/baby_swerve/competition_mode.json b/src/main/deploy/black_robot/competition_mode.json similarity index 100% rename from src/main/deploy/baby_swerve/competition_mode.json rename to src/main/deploy/black_robot/competition_mode.json diff --git a/src/main/deploy/basic_robot/controllers.json b/src/main/deploy/black_robot/controllers.json similarity index 100% rename from src/main/deploy/basic_robot/controllers.json rename to src/main/deploy/black_robot/controllers.json diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json b/src/main/deploy/black_robot/controllers/axis/driver_left_trigger.json similarity index 82% rename from src/main/deploy/basic_robot/controllers/axis/driver_right_x.json rename to src/main/deploy/black_robot/controllers/axis/driver_left_trigger.json index 8f981e23..007abd6c 100644 --- a/src/main/deploy/basic_robot/controllers/axis/driver_right_x.json +++ b/src/main/deploy/black_robot/controllers/axis/driver_left_trigger.json @@ -1,7 +1,7 @@ { "channel": 4, "deadband": 0.07, - "invert": true, + "invert": false, "scale": 1.0, "curvePower": 3, "limit": 1.0 diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json b/src/main/deploy/black_robot/controllers/axis/driver_left_x.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_x.json rename to src/main/deploy/black_robot/controllers/axis/driver_left_x.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json b/src/main/deploy/black_robot/controllers/axis/driver_left_y.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_y.json rename to src/main/deploy/black_robot/controllers/axis/driver_left_y.json diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json b/src/main/deploy/black_robot/controllers/axis/driver_right_trigger.json similarity index 82% rename from src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json rename to src/main/deploy/black_robot/controllers/axis/driver_right_trigger.json index 47b46917..bd2ec447 100644 --- a/src/main/deploy/basic_robot/controllers/axis/driver_right_trigger.json +++ b/src/main/deploy/black_robot/controllers/axis/driver_right_trigger.json @@ -1,7 +1,7 @@ { "channel": 3, "deadband": 0.07, - "invert": true, + "invert": false, "scale": 1.0, "curvePower": 3, "limit": 1.0 diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_left_trigger.json b/src/main/deploy/black_robot/controllers/axis/driver_right_x.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/axis/driver_left_trigger.json rename to src/main/deploy/black_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/black_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/black_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/black_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/black_robot/controllers/axis/operator_right_y.json diff --git a/src/main/deploy/baby_swerve/controllers/driver.json b/src/main/deploy/black_robot/controllers/driver.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/driver.json rename to src/main/deploy/black_robot/controllers/driver.json diff --git a/src/main/deploy/baby_swerve/controllers/operator.json b/src/main/deploy/black_robot/controllers/operator.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers/operator.json rename to src/main/deploy/black_robot/controllers/operator.json diff --git a/src/main/deploy/basic_robot/demo_mode.json b/src/main/deploy/black_robot/demo_mode.json similarity index 58% rename from src/main/deploy/basic_robot/demo_mode.json rename to src/main/deploy/black_robot/demo_mode.json index f60f73c9..a54e8ef0 100644 --- a/src/main/deploy/basic_robot/demo_mode.json +++ b/src/main/deploy/black_robot/demo_mode.json @@ -1,6 +1,6 @@ { - "maxSpeed": 2.0, - "maxAngularSpeed": 2.0, + "maxSpeed": 1.0, + "maxAngularSpeed": 1.0, "maxAccelleration": 1.0, "maxAngularAccelleration": 9.0 } diff --git a/src/main/deploy/baby_swerve/drive_modules/backleft.json b/src/main/deploy/black_robot/drive_modules/backleft.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/backleft.json rename to src/main/deploy/black_robot/drive_modules/backleft.json diff --git a/src/main/deploy/baby_swerve/drive_modules/backright.json b/src/main/deploy/black_robot/drive_modules/backright.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/backright.json rename to src/main/deploy/black_robot/drive_modules/backright.json diff --git a/src/main/deploy/baby_swerve/drive_modules/frontleft.json b/src/main/deploy/black_robot/drive_modules/frontleft.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/frontleft.json rename to src/main/deploy/black_robot/drive_modules/frontleft.json diff --git a/src/main/deploy/baby_swerve/drive_modules/frontright.json b/src/main/deploy/black_robot/drive_modules/frontright.json similarity index 100% rename from src/main/deploy/baby_swerve/drive_modules/frontright.json rename to src/main/deploy/black_robot/drive_modules/frontright.json diff --git a/src/main/deploy/basic_robot/field/game_pieces.json b/src/main/deploy/black_robot/field/game_pieces.json similarity index 78% rename from src/main/deploy/basic_robot/field/game_pieces.json rename to src/main/deploy/black_robot/field/game_pieces.json index a1e2a1fb..b13b37c1 100644 --- a/src/main/deploy/basic_robot/field/game_pieces.json +++ b/src/main/deploy/black_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/black_robot/robot.json b/src/main/deploy/black_robot/robot.json new file mode 100644 index 00000000..eeac1fa3 --- /dev/null +++ b/src/main/deploy/black_robot/robot.json @@ -0,0 +1,16 @@ +{ + "userConfig": "demo_mode.json", + "driveType": "AKIT_SWERVE_DRIVE", + "trackWidth": 18.5625, + "trackWidthUom": "in", + "wheelBase": 18.5625, + "wheelBaseUom": "in", + "wheelDiameter": 4, + "wheelDiameterUom": "in", + "physicalMaxSpeed": 5.93, + "physicalMaxSpeedUom": "m/s", + "driveMotorGearRatio": 1.0, + "loadSimulatedField": false, + "gamePieceA": "Coral", + "gamePieceB": "Algae" +} diff --git a/src/main/deploy/black_robot/subsystems/feeder.json b/src/main/deploy/black_robot/subsystems/feeder.json new file mode 100644 index 00000000..4a9c79da --- /dev/null +++ b/src/main/deploy/black_robot/subsystems/feeder.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "feeder/flywheel.json" + } + ] +} diff --git a/src/main/deploy/black_robot/subsystems/feeder/flywheel.json b/src/main/deploy/black_robot/subsystems/feeder/flywheel.json new file mode 100644 index 00000000..19f2f47d --- /dev/null +++ b/src/main/deploy/black_robot/subsystems/feeder/flywheel.json @@ -0,0 +1,71 @@ +{ + "motorSetup": { + "name": "feeder", + "logLevel": "HIGH", + "controllerType": "Spark", + "motorType": "Neo", + "canId": 12, + "inverted": true, + "idleMode": "BRAKE", + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "controlAlgorithm": "PROFILED", + "motorSystemId": { + "feedBack": { + "p": 0.00016541, + "i": 0.0, + "d": 0.0 + }, + "maxVelocity": { + "val": 5000, + "uom": "rpm" + }, + "maxAcceleration": { + "val": 2500, + "uom": "rps^2" + }, + "closedLoopRamp": { + "val": 0.25, + "uom": "s" + }, + "controlMode": "CLOSED_LOOP" + }, + "simSystemId": { + "feedBack": { + "p": 0.00016541, + "i": 0.0, + "d": 0.0 + }, + "maxVelocity": { + "val": 5000, + "uom": "rpm" + }, + "maxAcceleration": { + "val": 2500, + "uom": "rps^2" + } + }, + "lowerSoftLimit": { + "val": -500, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 500, + "uom": "rpm" + }, + "gearing": [ + 3, + 4 + ], + "mass": { + "val": 1, + "uom": "lbs" + }, + "radius": { + "val": 1.5, + "uom": "in" + } +} diff --git a/src/main/deploy/basic_robot/subsystems/led_strip.json b/src/main/deploy/black_robot/subsystems/led_strip.json similarity index 54% rename from src/main/deploy/basic_robot/subsystems/led_strip.json rename to src/main/deploy/black_robot/subsystems/led_strip.json index 25c307af..c414c947 100644 --- a/src/main/deploy/basic_robot/subsystems/led_strip.json +++ b/src/main/deploy/black_robot/subsystems/led_strip.json @@ -1,5 +1,5 @@ { - "length": 30, + "length": 34, "dataPin": 0, "segments": [ { @@ -11,6 +11,16 @@ "name": "team_colors", "start": 10, "end": 29 + }, + { + "name": "left_half", + "start": 0, + "end": 16 + }, + { + "name": "right_half", + "start": 17, + "end": 33 } ] } diff --git a/src/main/deploy/black_robot/subsystems/lowerflywheel.json b/src/main/deploy/black_robot/subsystems/lowerflywheel.json new file mode 100644 index 00000000..229cec66 --- /dev/null +++ b/src/main/deploy/black_robot/subsystems/lowerflywheel.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "lowerflywheel/flywheel.json" + } + ] +} diff --git a/src/main/deploy/black_robot/subsystems/lowerflywheel/flywheel.json b/src/main/deploy/black_robot/subsystems/lowerflywheel/flywheel.json new file mode 100644 index 00000000..c26ad551 --- /dev/null +++ b/src/main/deploy/black_robot/subsystems/lowerflywheel/flywheel.json @@ -0,0 +1,50 @@ +{ + "motorSetup": { + "name": "lowerflywheel", + "controllerType": "Spark", + "motorType": "Neo", + "canId": 10, + "inverted": false + }, + "motorSystemId": { + "feedBack": { + "p": 3.0352E-05, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.74358, + "a": 0.098446 + } + }, + "simSystemId": { + "feedBack": { + "p": 0, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.11449, + "a": 0.094235 + } + }, + "lowerSoftLimit": { + "val": -10000, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 10000, + "uom": "rpm" + }, + "gearStages": "52:24", + "mass": { + "val": 2, + "uom": "kg" + }, + "radius": { + "val": 2, + "uom": "in" + } +} diff --git a/src/main/deploy/black_robot/subsystems/upperflywheel.json b/src/main/deploy/black_robot/subsystems/upperflywheel.json new file mode 100644 index 00000000..41742573 --- /dev/null +++ b/src/main/deploy/black_robot/subsystems/upperflywheel.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "upperflywheel/flywheel.json" + } + ] +} diff --git a/src/main/deploy/black_robot/subsystems/upperflywheel/flywheel.json b/src/main/deploy/black_robot/subsystems/upperflywheel/flywheel.json new file mode 100644 index 00000000..618ac0a1 --- /dev/null +++ b/src/main/deploy/black_robot/subsystems/upperflywheel/flywheel.json @@ -0,0 +1,50 @@ +{ + "motorSetup": { + "name": "upperflywheel", + "controllerType": "Spark", + "motorType": "Neo", + "canId": 11, + "inverted": false + }, + "motorSystemId": { + "feedBack": { + "p": 3.0352E-05, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.74358, + "a": 0.098446 + } + }, + "simSystemId": { + "feedBack": { + "p": 0, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.11449, + "a": 0.094235 + } + }, + "lowerSoftLimit": { + "val": -10000, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 10000, + "uom": "rpm" + }, + "gearStages": "52:24", + "mass": { + "val": 2, + "uom": "kg" + }, + "radius": { + "val": 2, + "uom": "in" + } +} diff --git a/src/main/deploy/basic_robot/yagsl_drivetrain.json b/src/main/deploy/black_robot/yagsl_drivetrain.json similarity index 59% rename from src/main/deploy/basic_robot/yagsl_drivetrain.json rename to src/main/deploy/black_robot/yagsl_drivetrain.json index c6beb5d7..6609776c 100644 --- a/src/main/deploy/basic_robot/yagsl_drivetrain.json +++ b/src/main/deploy/black_robot/yagsl_drivetrain.json @@ -1,12 +1,7 @@ { - "directory": "basic_robot/yagsl_swerve", + "directory": "black_robot/yagsl_swerve", "turningMotorGearRatio": 1.0, - "driveModules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ], + "driveModules": [], "startingPose": { "x": { "val": 1.0, diff --git a/src/main/deploy/basic_robot/yagsl_swerve/controllerproperties.json b/src/main/deploy/black_robot/yagsl_swerve/controllerproperties.json similarity index 100% rename from src/main/deploy/basic_robot/yagsl_swerve/controllerproperties.json rename to src/main/deploy/black_robot/yagsl_swerve/controllerproperties.json diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json b/src/main/deploy/black_robot/yagsl_swerve/modules/backleft.json similarity index 71% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json rename to src/main/deploy/black_robot/yagsl_swerve/modules/backleft.json index 6218565f..178cad4d 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontright.json +++ b/src/main/deploy/black_robot/yagsl_swerve/modules/backleft.json @@ -10,17 +10,18 @@ "canbus": null }, "encoder": { - "type": "canandcoder", + "type": "canandmag", "id": 0, "canbus": null }, "inverted": { "drive": true, - "angle": true + "angle": false }, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": 8.37, - "left": -10.88 + "front": -9.2813, + "left": 9.2813 } } diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json b/src/main/deploy/black_robot/yagsl_swerve/modules/backright.json similarity index 71% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json rename to src/main/deploy/black_robot/yagsl_swerve/modules/backright.json index 1c03c3f1..04d8bc55 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/frontleft.json +++ b/src/main/deploy/black_robot/yagsl_swerve/modules/backright.json @@ -10,17 +10,18 @@ "canbus": null }, "encoder": { - "type": "canandcoder", + "type": "canandmag", "id": 0, "canbus": null }, "inverted": { "drive": true, - "angle": true + "angle": false }, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": 8.37, - "left": 10.88 + "front": -9.2813, + "left": -9.2813 } } diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json b/src/main/deploy/black_robot/yagsl_swerve/modules/frontleft.json similarity index 71% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json rename to src/main/deploy/black_robot/yagsl_swerve/modules/frontleft.json index eea317b8..7f0210c3 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/backright.json +++ b/src/main/deploy/black_robot/yagsl_swerve/modules/frontleft.json @@ -10,17 +10,18 @@ "canbus": null }, "encoder": { - "type": "canandcoder", + "type": "canandmag", "id": 0, "canbus": null }, "inverted": { "drive": true, - "angle": true + "angle": false }, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": -8.37, - "left": -10.88 + "front": 9.2813, + "left": 9.2813 } } diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json b/src/main/deploy/black_robot/yagsl_swerve/modules/frontright.json similarity index 71% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json rename to src/main/deploy/black_robot/yagsl_swerve/modules/frontright.json index 2ac2a1ab..85a5bf53 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/backleft.json +++ b/src/main/deploy/black_robot/yagsl_swerve/modules/frontright.json @@ -10,17 +10,18 @@ "canbus": null }, "encoder": { - "type": "canandcoder", + "type": "canandmag", "id": 0, "canbus": null }, "inverted": { "drive": true, - "angle": true + "angle": false }, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": -8.37, - "left": 10.88 + "front": 9.2813, + "left": -9.2813 } } diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json b/src/main/deploy/black_robot/yagsl_swerve/modules/physicalproperties.json similarity index 82% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json rename to src/main/deploy/black_robot/yagsl_swerve/modules/physicalproperties.json index c921b000..66939aba 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/physicalproperties.json +++ b/src/main/deploy/black_robot/yagsl_swerve/modules/physicalproperties.json @@ -1,17 +1,17 @@ { "conversionFactors": { "angle": { - "gearRatio": 12.8, + "gearRatio": 25, "factor": 0 }, "drive": { - "gearRatio": 8.14, + "gearRatio": 6.75, "diameter": 4, "factor": 0 } }, "currentLimit": { - "drive": 100, + "drive": 40, "angle": 20 }, "rampRate": { diff --git a/src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json b/src/main/deploy/black_robot/yagsl_swerve/modules/pidfproperties.json similarity index 64% rename from src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json rename to src/main/deploy/black_robot/yagsl_swerve/modules/pidfproperties.json index 5924fdbb..2644bc19 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/modules/pidfproperties.json +++ b/src/main/deploy/black_robot/yagsl_swerve/modules/pidfproperties.json @@ -1,16 +1,16 @@ { "drive": { - "p": 0.00023, - "i": 0.0000002, - "d": 1, + "p": 0.0020645, + "i": 0, + "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 0.0020645, + "p": 0.025, "i": 0, "d": 0, - "f": 0.001, + "f": 0.0, "iz": 0 } } diff --git a/src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json b/src/main/deploy/black_robot/yagsl_swerve/swervedrive.json similarity index 93% rename from src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json rename to src/main/deploy/black_robot/yagsl_swerve/swervedrive.json index f4876233..993fe290 100644 --- a/src/main/deploy/basic_robot/yagsl_swerve/swervedrive.json +++ b/src/main/deploy/black_robot/yagsl_swerve/swervedrive.json @@ -1,7 +1,7 @@ { "imu": { "type": "pigeon2", - "id": 13, + "id": 9, "canbus": "" }, "invertedIMU": false, diff --git a/src/main/deploy/buttercup_swerve/akit_swerve_drivetrain.json b/src/main/deploy/buttercup_swerve/akit_swerve_drivetrain.json new file mode 100644 index 00000000..02bb00b2 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/akit_swerve_drivetrain.json @@ -0,0 +1,222 @@ +{ + "type": "Spark", + "constants": { + "trackWidth": { + "val": 18.5625, + "uom": "inches" + }, + "wheelBase": { + "val": 18.5625, + "uom": "inches" + }, + "wheelDiameter": { + "val": 4, + "uom": "inches" + }, + "maxDriveSpeed": { + "val": 5.93, + "uom": "m/sec" + }, + "bumperFrameLength": { + "val": 28, + "uom": "inches" + }, + "bumperFrameWidth": { + "val": 28, + "uom": "inches" + }, + "gyro": { + "type": "pigeon2", + "id": 9, + "inverted": false, + "canbus": "" + }, + "driveGearRatio": "1:6.75", + "steerGearRatio": "1:25", + "driveMotorControl": { + "closedLoopRamp": { + "val": 0.15, + "uom": "seconds" + }, + "feedBack": { + "p": 0.0020645, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.0, + "v": 0.1, + "a": 0.0 + } + }, + "steerMotorControl": { + "closedLoopRamp": { + "val": 0.05, + "uom": "seconds" + }, + "feedBack": { + "p": 1.5, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.0, + "v": 0.0, + "a": 0.0 + } + }, + "modules": { + "frontLeft": { + "driveMotorSetup": { + "name": "frontLeftDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 4, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "frontLeftSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 3, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + }, + "frontRight": { + "driveMotorSetup": { + "name": "frontRightDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 2, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "frontRightSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 1, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + }, + "backLeft": { + "driveMotorSetup": { + "name": "backLeftDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 6, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "backLeftSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 5, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + }, + "backRight": { + "driveMotorSetup": { + "name": "backRightDrive", + "controllerType": "spark", + "motorType": "NEO", + "canId": 8, + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "steerMotorSetup": { + "name": "backRightSteer", + "controllerType": "spark", + "motorType": "NEO", + "canId": 7, + "inverted": false, + "currentLimit": { + "val": 20, + "uom": "amps" + } + }, + "encoderId": 0, + "absoluteOffset": { + "val": 0, + "uom": "degrees" + }, + "encoderInverted": true + } + }, + "coupleRatio": 1.0, + "invertLeftSide": false, + "invertRightSide": false, + "steerInertia": { + "val": 0.04, + "uom": "kg*m^2" + }, + "driveInertia": { + "val": 0.025, + "uom": "kg*m^2" + }, + "robotMass": { + "val": 75, + "uom": "kg" + }, + "wheelCOF": 1.19, + "slipCurrent": { + "val": 40, + "uom": "amps" + }, + "canbus": "", + "startingPose": { + "x": { + "val": 1.0, + "uom": "m" + }, + "y": { + "val": 1.0, + "uom": "m" + }, + "rotation": { + "val": 0.0, + "uom": "deg" + } + } + } +} diff --git a/src/main/deploy/buttercup_swerve/cameras.json b/src/main/deploy/buttercup_swerve/cameras.json new file mode 100644 index 00000000..10e9384c --- /dev/null +++ b/src/main/deploy/buttercup_swerve/cameras.json @@ -0,0 +1,4 @@ +{ + "cameras": [], + "simulatedField": "org.frc5010.lobbinloco.LobbinLoco" +} diff --git a/src/main/deploy/buttercup_swerve/cameras/intake.json b/src/main/deploy/buttercup_swerve/cameras/intake.json new file mode 100644 index 00000000..33938add --- /dev/null +++ b/src/main/deploy/buttercup_swerve/cameras/intake.json @@ -0,0 +1,13 @@ +{ + "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/buttercup_swerve/cameras/localization.json b/src/main/deploy/buttercup_swerve/cameras/localization.json new file mode 100644 index 00000000..706b561a --- /dev/null +++ b/src/main/deploy/buttercup_swerve/cameras/localization.json @@ -0,0 +1,13 @@ +{ + "name": "localization", + "use": "apriltag", + "type": "limelight", + "strategy": "LOWEST_AMBIGUITY", + "column": 0, + "x": -0.35, + "y": -0.25, + "z": 0.5, + "roll": 0, + "pitch": -20, + "yaw": 180 +} diff --git a/src/main/deploy/buttercup_swerve/cameras/quest.json b/src/main/deploy/buttercup_swerve/cameras/quest.json new file mode 100644 index 00000000..df412311 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/cameras/quest.json @@ -0,0 +1,11 @@ +{ + "name": "quest", + "use": "quest", + "column": 0, + "x": 0, + "y": 0, + "z": 18, + "roll": 0, + "pitch": 0, + "yaw": 0 +} diff --git a/src/main/deploy/buttercup_swerve/cameras/shooter.json b/src/main/deploy/buttercup_swerve/cameras/shooter.json new file mode 100644 index 00000000..b60f68bc --- /dev/null +++ b/src/main/deploy/buttercup_swerve/cameras/shooter.json @@ -0,0 +1,16 @@ +{ + "name": "shooter", + "use": "target", + "type": "photonvision", + "column": 0, + "x": -0.25, + "y": 0, + "z": 0.25, + "roll": 0, + "pitch": 20, + "yaw": 180, + "targetFiducialIds": [ + 4, + 11 + ] +} diff --git a/src/main/deploy/basic_robot/competition_mode.json b/src/main/deploy/buttercup_swerve/competition_mode.json similarity index 100% rename from src/main/deploy/basic_robot/competition_mode.json rename to src/main/deploy/buttercup_swerve/competition_mode.json diff --git a/src/main/deploy/baby_swerve/controllers.json b/src/main/deploy/buttercup_swerve/controllers.json similarity index 100% rename from src/main/deploy/baby_swerve/controllers.json rename to src/main/deploy/buttercup_swerve/controllers.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json b/src/main/deploy/buttercup_swerve/controllers/axis/driver_left_trigger.json similarity index 54% rename from src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json rename to src/main/deploy/buttercup_swerve/controllers/axis/driver_left_trigger.json index 8f981e23..97720dc5 100644 --- a/src/main/deploy/baby_swerve/controllers/axis/driver_right_x.json +++ b/src/main/deploy/buttercup_swerve/controllers/axis/driver_left_trigger.json @@ -1,8 +1,8 @@ { "channel": 4, "deadband": 0.07, - "invert": true, - "scale": 1.0, + "invert": false, + "scale": 0.5, "curvePower": 3, - "limit": 1.0 + "limit": 0.5 } diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json b/src/main/deploy/buttercup_swerve/controllers/axis/driver_left_x.json similarity index 82% rename from src/main/deploy/basic_robot/controllers/axis/driver_left_x.json rename to src/main/deploy/buttercup_swerve/controllers/axis/driver_left_x.json index d77f0e91..41719d6a 100644 --- a/src/main/deploy/basic_robot/controllers/axis/driver_left_x.json +++ b/src/main/deploy/buttercup_swerve/controllers/axis/driver_left_x.json @@ -3,6 +3,5 @@ "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/buttercup_swerve/controllers/axis/driver_left_y.json similarity index 100% rename from src/main/deploy/basic_robot/controllers/axis/driver_left_y.json rename to src/main/deploy/buttercup_swerve/controllers/axis/driver_left_y.json diff --git a/src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json b/src/main/deploy/buttercup_swerve/controllers/axis/driver_right_trigger.json similarity index 82% rename from src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json rename to src/main/deploy/buttercup_swerve/controllers/axis/driver_right_trigger.json index 47b46917..bd2ec447 100644 --- a/src/main/deploy/baby_swerve/controllers/axis/driver_right_trigger.json +++ b/src/main/deploy/buttercup_swerve/controllers/axis/driver_right_trigger.json @@ -1,7 +1,7 @@ { "channel": 3, "deadband": 0.07, - "invert": true, + "invert": false, "scale": 1.0, "curvePower": 3, "limit": 1.0 diff --git a/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json b/src/main/deploy/buttercup_swerve/controllers/axis/driver_right_x.json similarity index 82% rename from src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json rename to src/main/deploy/buttercup_swerve/controllers/axis/driver_right_x.json index 41b79133..f899fd4b 100644 --- a/src/main/deploy/basic_robot/controllers/axis/driver_left_trigger.json +++ b/src/main/deploy/buttercup_swerve/controllers/axis/driver_right_x.json @@ -1,7 +1,7 @@ { "channel": 2, "deadband": 0.07, - "invert": true, + "invert": false, "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/buttercup_swerve/controllers/axis/operator_left_y.json similarity index 100% rename from src/main/deploy/basic_robot/controllers/axis/operator_left_y.json rename to src/main/deploy/buttercup_swerve/controllers/axis/operator_left_y.json diff --git a/src/main/deploy/basic_robot/controllers/axis/operator_right_y.json b/src/main/deploy/buttercup_swerve/controllers/axis/operator_right_y.json similarity index 100% rename from src/main/deploy/basic_robot/controllers/axis/operator_right_y.json rename to src/main/deploy/buttercup_swerve/controllers/axis/operator_right_y.json diff --git a/src/main/deploy/basic_robot/controllers/driver.json b/src/main/deploy/buttercup_swerve/controllers/driver.json similarity index 100% rename from src/main/deploy/basic_robot/controllers/driver.json rename to src/main/deploy/buttercup_swerve/controllers/driver.json diff --git a/src/main/deploy/basic_robot/controllers/operator.json b/src/main/deploy/buttercup_swerve/controllers/operator.json similarity index 100% rename from src/main/deploy/basic_robot/controllers/operator.json rename to src/main/deploy/buttercup_swerve/controllers/operator.json diff --git a/src/main/deploy/buttercup_swerve/demo_mode.json b/src/main/deploy/buttercup_swerve/demo_mode.json new file mode 100644 index 00000000..a54e8ef0 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/demo_mode.json @@ -0,0 +1,6 @@ +{ + "maxSpeed": 1.0, + "maxAngularSpeed": 1.0, + "maxAccelleration": 1.0, + "maxAngularAccelleration": 9.0 +} diff --git a/src/main/deploy/buttercup_swerve/drive_modules/backleft.json b/src/main/deploy/buttercup_swerve/drive_modules/backleft.json new file mode 100644 index 00000000..dd530f9e --- /dev/null +++ b/src/main/deploy/buttercup_swerve/drive_modules/backleft.json @@ -0,0 +1,5 @@ +{ + "s": 0.0, + "v": 0.0, + "a": 0.0 +} diff --git a/src/main/deploy/buttercup_swerve/drive_modules/backright.json b/src/main/deploy/buttercup_swerve/drive_modules/backright.json new file mode 100644 index 00000000..dd530f9e --- /dev/null +++ b/src/main/deploy/buttercup_swerve/drive_modules/backright.json @@ -0,0 +1,5 @@ +{ + "s": 0.0, + "v": 0.0, + "a": 0.0 +} diff --git a/src/main/deploy/buttercup_swerve/drive_modules/frontleft.json b/src/main/deploy/buttercup_swerve/drive_modules/frontleft.json new file mode 100644 index 00000000..dd530f9e --- /dev/null +++ b/src/main/deploy/buttercup_swerve/drive_modules/frontleft.json @@ -0,0 +1,5 @@ +{ + "s": 0.0, + "v": 0.0, + "a": 0.0 +} diff --git a/src/main/deploy/buttercup_swerve/drive_modules/frontright.json b/src/main/deploy/buttercup_swerve/drive_modules/frontright.json new file mode 100644 index 00000000..dd530f9e --- /dev/null +++ b/src/main/deploy/buttercup_swerve/drive_modules/frontright.json @@ -0,0 +1,5 @@ +{ + "s": 0.0, + "v": 0.0, + "a": 0.0 +} diff --git a/src/main/deploy/buttercup_swerve/field/game_pieces.json b/src/main/deploy/buttercup_swerve/field/game_pieces.json new file mode 100644 index 00000000..b13b37c1 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/field/game_pieces.json @@ -0,0 +1,10 @@ +{ + "gamePieces": [ + { + "x": 2, + "y": 7, + "type": "Coral", + "rotation": 0 + } + ] +} diff --git a/src/main/deploy/buttercup_swerve/robot.json b/src/main/deploy/buttercup_swerve/robot.json new file mode 100644 index 00000000..5973e5f7 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/robot.json @@ -0,0 +1,16 @@ +{ + "userConfig": "demo_mode.json", + "driveType": "AKIT_SWERVE_DRIVE", + "trackWidth": 18.5625, + "trackWidthUom": "in", + "wheelBase": 18.5625, + "wheelBaseUom": "in", + "wheelDiameter": 4, + "wheelDiameterUom": "in", + "physicalMaxSpeed": 5.93, + "physicalMaxSpeedUom": "m/s", + "driveMotorGearRatio": 1.0, + "loadSimulatedField": false, + "gamePieceA": "FRC5010Ball", + "gamePieceB": "FRC5010Ball" +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/feeder.json b/src/main/deploy/buttercup_swerve/subsystems/feeder.json new file mode 100644 index 00000000..4a9c79da --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/feeder.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "feeder/flywheel.json" + } + ] +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/feeder/flywheel.json b/src/main/deploy/buttercup_swerve/subsystems/feeder/flywheel.json new file mode 100644 index 00000000..19f2f47d --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/feeder/flywheel.json @@ -0,0 +1,71 @@ +{ + "motorSetup": { + "name": "feeder", + "logLevel": "HIGH", + "controllerType": "Spark", + "motorType": "Neo", + "canId": 12, + "inverted": true, + "idleMode": "BRAKE", + "currentLimit": { + "val": 40, + "uom": "amps" + } + }, + "controlAlgorithm": "PROFILED", + "motorSystemId": { + "feedBack": { + "p": 0.00016541, + "i": 0.0, + "d": 0.0 + }, + "maxVelocity": { + "val": 5000, + "uom": "rpm" + }, + "maxAcceleration": { + "val": 2500, + "uom": "rps^2" + }, + "closedLoopRamp": { + "val": 0.25, + "uom": "s" + }, + "controlMode": "CLOSED_LOOP" + }, + "simSystemId": { + "feedBack": { + "p": 0.00016541, + "i": 0.0, + "d": 0.0 + }, + "maxVelocity": { + "val": 5000, + "uom": "rpm" + }, + "maxAcceleration": { + "val": 2500, + "uom": "rps^2" + } + }, + "lowerSoftLimit": { + "val": -500, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 500, + "uom": "rpm" + }, + "gearing": [ + 3, + 4 + ], + "mass": { + "val": 1, + "uom": "lbs" + }, + "radius": { + "val": 1.5, + "uom": "in" + } +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/led_strip.json b/src/main/deploy/buttercup_swerve/subsystems/led_strip.json new file mode 100644 index 00000000..c414c947 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/led_strip.json @@ -0,0 +1,26 @@ +{ + "length": 34, + "dataPin": 0, + "segments": [ + { + "name": "status_indicator", + "start": 0, + "end": 9 + }, + { + "name": "team_colors", + "start": 10, + "end": 29 + }, + { + "name": "left_half", + "start": 0, + "end": 16 + }, + { + "name": "right_half", + "start": 17, + "end": 33 + } + ] +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/lowerflywheel.json b/src/main/deploy/buttercup_swerve/subsystems/lowerflywheel.json new file mode 100644 index 00000000..229cec66 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/lowerflywheel.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "lowerflywheel/flywheel.json" + } + ] +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/lowerflywheel/flywheel.json b/src/main/deploy/buttercup_swerve/subsystems/lowerflywheel/flywheel.json new file mode 100644 index 00000000..694665db --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/lowerflywheel/flywheel.json @@ -0,0 +1,50 @@ +{ + "motorSetup": { + "name": "lowerflywheel", + "controllerType": "Spark", + "motorType": "Neo", + "canId": 10, + "inverted": true + }, + "motorSystemId": { + "feedBack": { + "p": 3.0352E-05, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.74358, + "a": 0.098446 + } + }, + "simSystemId": { + "feedBack": { + "p": 0, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.11449, + "a": 0.094235 + } + }, + "lowerSoftLimit": { + "val": -10000, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 10000, + "uom": "rpm" + }, + "gearStages": "52:24", + "mass": { + "val": 2, + "uom": "kg" + }, + "radius": { + "val": 2, + "uom": "in" + } +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/upperflywheel.json b/src/main/deploy/buttercup_swerve/subsystems/upperflywheel.json new file mode 100644 index 00000000..41742573 --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/upperflywheel.json @@ -0,0 +1,8 @@ +{ + "devices": [ + { + "device": "yams_shooter", + "file": "upperflywheel/flywheel.json" + } + ] +} diff --git a/src/main/deploy/buttercup_swerve/subsystems/upperflywheel/flywheel.json b/src/main/deploy/buttercup_swerve/subsystems/upperflywheel/flywheel.json new file mode 100644 index 00000000..8fdf840a --- /dev/null +++ b/src/main/deploy/buttercup_swerve/subsystems/upperflywheel/flywheel.json @@ -0,0 +1,50 @@ +{ + "motorSetup": { + "name": "upperflywheel", + "controllerType": "Spark", + "motorType": "Neo", + "canId": 11, + "inverted": true + }, + "motorSystemId": { + "feedBack": { + "p": 3.0352E-05, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.74358, + "a": 0.098446 + } + }, + "simSystemId": { + "feedBack": { + "p": 0, + "i": 0.0, + "d": 0.0 + }, + "feedForward": { + "s": 0.04234, + "v": 0.11449, + "a": 0.094235 + } + }, + "lowerSoftLimit": { + "val": -10000, + "uom": "rpm" + }, + "upperSoftLimit": { + "val": 10000, + "uom": "rpm" + }, + "gearStages": "52:24", + "mass": { + "val": 2, + "uom": "kg" + }, + "radius": { + "val": 2, + "uom": "in" + } +} diff --git a/src/main/deploy/buttercup_swerve/yagsl_drivetrain.json b/src/main/deploy/buttercup_swerve/yagsl_drivetrain.json new file mode 100644 index 00000000..6da0d30c --- /dev/null +++ b/src/main/deploy/buttercup_swerve/yagsl_drivetrain.json @@ -0,0 +1,19 @@ +{ + "directory": "buttercup_swerve/yagsl_swerve", + "turningMotorGearRatio": 1.0, + "driveModules": [], + "startingPose": { + "x": { + "val": 1.0, + "uom": "m" + }, + "y": { + "val": 1.0, + "uom": "m" + }, + "rotation": { + "val": 0.0, + "uom": "deg" + } + } +} diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/controllerproperties.json similarity index 61% rename from src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/controllerproperties.json index c5ab6446..dd353caf 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/controllerproperties.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/controllerproperties.json @@ -1,5 +1,5 @@ { - "angleJoystickRadiusDeadband": 0.5, + "angleJoystickRadiusDeadband": 0.05, "heading": { "p": 0.4, "i": 0, diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/backleft.json similarity index 63% rename from src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/modules/backleft.json index a985764b..187c0fa9 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backright.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/backleft.json @@ -1,27 +1,27 @@ { "drive": { "type": "neo", - "id": 5, + "id": 6, "canbus": null }, "angle": { - "type": "neo550", - "id": 6, + "type": "neo", + "id": 5, "canbus": null }, "encoder": { - "type": "thrifty", - "id": 2, + "type": "canandmag", + "id": 0, "canbus": null }, "inverted": { "drive": false, - "angle": true + "angle": false }, - "absoluteEncoderOffset": -166.15, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": -3.75, - "left": -3.75 + "front": -9.2813, + "left": 9.2813 } } diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/backright.json similarity index 63% rename from src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/modules/backright.json index 7234a3ef..7356bcc7 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontright.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/backright.json @@ -1,27 +1,27 @@ { "drive": { "type": "neo", - "id": 7, + "id": 8, "canbus": null }, "angle": { - "type": "neo550", - "id": 8, + "type": "neo", + "id": 7, "canbus": null }, "encoder": { - "type": "thrifty", - "id": 3, + "type": "canandmag", + "id": 0, "canbus": null }, "inverted": { "drive": false, - "angle": true + "angle": false }, - "absoluteEncoderOffset": -358.5, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": 3.75, - "left": -3.75 + "front": -9.2813, + "left": -9.2813 } } diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/frontleft.json similarity index 63% rename from src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/modules/frontleft.json index 929f3fc9..7d857f81 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/backleft.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/frontleft.json @@ -1,27 +1,27 @@ { "drive": { "type": "neo", - "id": 3, + "id": 4, "canbus": null }, "angle": { - "type": "neo550", - "id": 4, + "type": "neo", + "id": 3, "canbus": null }, "encoder": { - "type": "thrifty", - "id": 1, + "type": "canandmag", + "id": 0, "canbus": null }, "inverted": { "drive": false, - "angle": true + "angle": false }, - "absoluteEncoderOffset": -120.6, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": -3.75, - "left": 3.75 + "front": 9.2813, + "left": 9.2813 } } diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/frontright.json similarity index 66% rename from src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/modules/frontright.json index 517bdf23..571e7b32 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/frontleft.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/frontright.json @@ -1,27 +1,27 @@ { "drive": { "type": "neo", - "id": 1, + "id": 2, "canbus": null }, "angle": { - "type": "neo550", - "id": 2, + "type": "neo", + "id": 1, "canbus": null }, "encoder": { - "type": "thrifty", + "type": "canandmag", "id": 0, "canbus": null }, "inverted": { "drive": false, - "angle": true + "angle": false }, - "absoluteEncoderOffset": -165.75, "absoluteEncoderInverted": true, + "absoluteEncoderOffset": 0, "location": { - "front": 3.75, - "left": 3.75 + "front": 9.2813, + "left": -9.2813 } } diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/physicalproperties.json similarity index 56% rename from src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/modules/physicalproperties.json index 2f0bd03d..bbe06e2a 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/physicalproperties.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/physicalproperties.json @@ -1,13 +1,11 @@ { "conversionFactors": { "angle": { - "gearRatio": 55.965, - "factor": 0 + "gearRatio": 25 }, "drive": { - "gearRatio": 5.25, - "diameter": 3.1242, - "factor": 0 + "gearRatio": 6.75, + "diameter": 4 } }, "currentLimit": { @@ -15,10 +13,10 @@ "angle": 20 }, "rampRate": { - "drive": 0.1, - "angle": 0.1 + "drive": 0.15, + "angle": 0.05 }, "wheelGripCoefficientOfFriction": 1.19, "optimalVoltage": 12, - "robotMass": 47.8 + "robotMass": 75 } diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/pidfproperties.json similarity index 54% rename from src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/modules/pidfproperties.json index a0d528dc..a5136120 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/modules/pidfproperties.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/modules/pidfproperties.json @@ -1,16 +1,16 @@ { "drive": { - "p": 0.00057373, + "p": 0.0020645, "i": 0, "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 0.01125, - "i": 0.000002, - "d": 0, + "p": 0.02, + "i": 0, + "d": 0.0000, "f": 0, - "iz": 5 + "iz": 0 } } diff --git a/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json b/src/main/deploy/buttercup_swerve/yagsl_swerve/swervedrive.json similarity index 73% rename from src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json rename to src/main/deploy/buttercup_swerve/yagsl_swerve/swervedrive.json index 5fb01c6b..993fe290 100644 --- a/src/main/deploy/baby_swerve/yagsl_swerve/swervedrive.json +++ b/src/main/deploy/buttercup_swerve/yagsl_swerve/swervedrive.json @@ -1,8 +1,8 @@ { "imu": { - "type": "navx", - "id": 0, - "canbus": null + "type": "pigeon2", + "id": 9, + "canbus": "" }, "invertedIMU": false, "modules": [ diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/BlueToRedAuto.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/BlueToRedAuto.auto index 268147bb..61765657 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/BlueToRedAuto.auto @@ -7,7 +7,13 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "BlueToRed" + } + }, + { + "type": "named", + "data": { + "name": "shoot" } } ] diff --git a/src/main/deploy/pathplanner/autos/RedToBlueAuto.auto b/src/main/deploy/pathplanner/autos/RedToBlueAuto.auto new file mode 100644 index 00000000..6d7637eb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RedToBlueAuto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RedToBlue" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueToRed.path b/src/main/deploy/pathplanner/paths/BlueToRed.path new file mode 100644 index 00000000..f6e1ec0e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueToRed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.61166963755651, + "y": 2.0885799168152124 + }, + "prevControl": null, + "nextControl": { + "x": 4.613482949066363, + "y": 3.2293620876070244 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.61166963755651, + "y": 4.938966131907309 + }, + "prevControl": { + "x": 4.635625576925734, + "y": 3.7837206284914484 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.25, + "maxAcceleration": 10.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.8938895949337096 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.07192794940851074 + }, + "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/New Path.path index 38908444..51f0698e 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.031762295081966, - "y": 6.728227459016393 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 6.677151639344262, - "y": 6.728227459016393 + "x": 3.0, + "y": 7.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.207684426229508, - "y": 6.88406762295082 + "x": 4.812601334742173, + "y": 1.232586014823413 }, "prevControl": { - "x": 5.3096147111058976, - "y": 7.435032765389015 - }, - "nextControl": { - "x": 3.8960040983606556, - "y": 6.728227459016393 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.812090163934426, - "y": 5.217776639344262 - }, - "prevControl": { - "x": 3.57233606557377, - "y": 6.728227459016393 + "x": 3.391450364298724, + "y": 1.235462424252745 }, "nextControl": null, "isLocked": false, @@ -49,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.25, + "maxAcceleration": 10.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -58,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -63.43494882292201 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -179.06080905426444 + "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RedToBlue.path b/src/main/deploy/pathplanner/paths/RedToBlue.path new file mode 100644 index 00000000..c8b5c03e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RedToBlue.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.696755793226381, + "y": 4.317837195484255 + }, + "prevControl": null, + "nextControl": { + "x": 4.685023010087242, + "y": 1.1560625455389912 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.696755793226381, + "y": 1.2972786690433742 + }, + "prevControl": { + "x": 4.714172063785987, + "y": 2.651095247012745 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.25, + "maxAcceleration": 10.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/robots.json b/src/main/deploy/robots.json index 50eac16d..4b5ca5ad 100644 --- a/src/main/deploy/robots.json +++ b/src/main/deploy/robots.json @@ -1,16 +1,17 @@ { "competitionPin": 0, "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", + "buttercup_swerve": { + "id": "00:80:2F:34:B2:C5", + "robotClass": "frc.robot.blackteam.BlackRobot", "simulate": true, - "competition": true + "competition": false + }, + "black_robot": { + "id": "00:80:2F:33:04:33", + "robotClass": "frc.robot.blackteam.BlackRobot", + "simulate": false, + "competition": false } } } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c8ece43a..e96a8ece 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 = "TeamBlack2025"; 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 = 188; + public static final String GIT_SHA = "137f19cef7acb02170a61d7d8edfbad3565e75f6"; + public static final String GIT_DATE = "2026-07-23 17:42:27 EDT"; + public static final String GIT_BRANCH = "LobbinLoco"; + public static final String BUILD_DATE = "2026-07-23 18:08:15 EDT"; + public static final long BUILD_UNIX_TIME = 1784844495235L; public static final int DIRTY = 1; private BuildConstants() {} 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/blackteam/BlackRobot.java b/src/main/java/frc/robot/blackteam/BlackRobot.java new file mode 100644 index 00000000..3cfefbcc --- /dev/null +++ b/src/main/java/frc/robot/blackteam/BlackRobot.java @@ -0,0 +1,206 @@ +package frc.robot.blackteam; + +import static edu.wpi.first.units.Units.RPM; + +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 org.frc5010.common.arch.GenericRobot; +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; + +public class BlackRobot extends GenericRobot { + private GenericDrivetrain drivetrain; + private LowerFlyWheel lowerFlyWheel; + private UpperFlyWheel upperFlyWheel; + private StateMachine flyWheelStateMachine = new StateMachine(logPrefix); + private final double SPEED1 = 0.20; + private final double SPEED2 = 0.35; + private final double SPEED3 = 0.16; + private final double SPEED4 = 0.17; + private final double UPPEROFFSET = 0.01; + private final AngularVelocity VELOCITY2 = RPM.of(1500); + private final String FEEDER_LED = "left_half"; + private final String SHOOTER_LED = "right_half"; + + public BlackRobot(String directory) { + super(directory); + drivetrain = (GenericDrivetrain) subsystems.get(ConfigConstants.DRIVETRAIN); + lowerFlyWheel = new LowerFlyWheel(); + upperFlyWheel = new UpperFlyWheel(); + + // NamedCommands.registerCommand("shoot", launchToDistance(20)); + } + + private FeederSubsystem feeder = new FeederSubsystem(); + + @SuppressWarnings("unused") + private Command launchToDistance(int i) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'launchToDistance'"); + } + + @Override + public void configureButtonBindings(Controller driver, Controller operator) { + drivetrain.configureButtonBindings(driver, operator); + driver + .createRightStickButton() + .onTrue(Commands.runOnce(() -> drivetrain.toggleFieldOrientedDrive())); + // driver.setRightTrigger(driver.createRightTrigger().cubed().deadzone(0.05).scale(0.35)); + // driver.setLeftTrigger(driver.createLeftTrigger().cubed().deadzone(0.05).scale(0.35)); + driver + .createLeftBumper() + .whileTrue( + (lowerFlyWheel + .set(SPEED1) + .alongWith(upperFlyWheel.set(SPEED1 - UPPEROFFSET)) + .beforeStarting( + () -> { + LEDStrip.setSegmentActive(SHOOTER_LED, true); + LEDStrip.changeSegmentPattern( + SHOOTER_LED, LEDStrip.getRainbowPattern(50)); + })) + .finallyDo(() -> LEDStrip.setSegmentActive(SHOOTER_LED, false))); + // driver + // .createXButton() + // .whileTrue( + // lowerFlyWheel + // .set(SPEED2) + // .alongWith(upperFlyWheel.set(SPEED2 - UPPEROFFSET)) + // .beforeStarting( + // () -> { + // LEDStrip.setSegmentActive(SHOOTER_LED, true); + // LEDStrip.changeSegmentPattern(SHOOTER_LED, + // LEDStrip.getRainbowPattern(100)); + // }) + // .finallyDo(() -> LEDStrip.setSegmentActive(SHOOTER_LED, false))); + + // driver + // .createYButton() + // .whileTrue( + // lowerFlyWheel + // .set(SPEED3) + // .alongWith(upperFlyWheel.set(SPEED3 - UPPEROFFSET)) + // .beforeStarting( + // () -> { + // LEDStrip.setSegmentActive(SHOOTER_LED, true); + // LEDStrip.changeSegmentPattern(SHOOTER_LED, + // LEDStrip.getRainbowPattern(100)); + // }) + // .finallyDo(() -> LEDStrip.setSegmentActive(SHOOTER_LED, false))); + // driver + // .createRightBumper() + // .whileTrue( + // lowerFlyWheel + // .set(SPEED4) + // .alongWith(upperFlyWheel.set(SPEED4 - UPPEROFFSET)) + // .beforeStarting( + // () -> { + // LEDStrip.setSegmentActive(SHOOTER_LED, true); + // LEDStrip.changeSegmentPattern(SHOOTER_LED, + // LEDStrip.getRainbowPattern(200)); + // }) + // .finallyDo(() -> LEDStrip.setSegmentActive(SHOOTER_LED, false))); + + driver + .createBButton() + .onFalse( + lowerFlyWheel + .set(0) + .alongWith(upperFlyWheel.set(0)) + .beforeStarting(() -> LEDStrip.setSegmentActive(SHOOTER_LED, false))); + + // driver.createBackButton().whileTrue(lowerFlyWheel.systemID()); + // driver.createStartButton().whileTrue(upperFlyWheel.systemID()); + driver + .createAButton() + .whileTrue( + feeder + .setSpeed(0.5) + .beforeStarting( + () -> { + LEDStrip.setSegmentActive(FEEDER_LED, true); + LEDStrip.changeSegmentPattern( + FEEDER_LED, LEDStrip.getLaserPattern(Color.kGreen, 0.2, 150)); + }) + .finallyDo(() -> LEDStrip.setSegmentActive(FEEDER_LED, false))); + + driver.A_BUTTON.onFalse(feeder.setSpeed(0)); + + driver + .createStartButton() + .whileTrue( + feeder + .setSpeed(-0.5) + .beforeStarting( + () -> { + LEDStrip.setSegmentActive(FEEDER_LED, true); + LEDStrip.changeSegmentPattern( + FEEDER_LED, LEDStrip.getLaserPattern(Color.kGreen, 0.2, 150)); + }) + .finallyDo(() -> LEDStrip.setSegmentActive(FEEDER_LED, false))); + + driver.START_BUTTON.onFalse(feeder.setSpeed(0)); + + State prep = + flyWheelStateMachine.addState( + "prep", + Commands.print("PREP") + .andThen( + lowerFlyWheel + .setVelocity(RPM.of(1000)) + .alongWith(upperFlyWheel.setVelocity(RPM.of(1000))))); + State fire = + flyWheelStateMachine.addState("fire", Commands.print("FIRE").andThen(feeder.setSpeed(0.5))); + + flyWheelStateMachine.setInitialState(prep); + prep.switchTo(fire).when(lowerFlyWheel.isNearTarget(RPM.of(200))); + // rightBumper.whileTrue(flyWheelStateMachine); + // rightBumper.onFalse( + // lowerFlyWheel + // .setVelocity(RPM.of(0)) + // .andThen(lowerFlyWheel.set(0)) + // .alongWith(feeder.setSpeed(0))); + } + + @Override + public void configureAltButtonBindings(Controller driver, Controller operator) { + // Test-mode diagnostics: run the deterministic azimuth step test (robot on blocks). + driver.createAButton().onTrue(drivetrain.azimuthStepTestCommand()); + } + + @Override + public void setupDefaultCommands(Controller driver, Controller operator) { + // lowerFlyWheel.setDefaultCommand(lowerFlyWheel.joyStickControl(() -> + // driver.getRightTrigger())); + // upperFlyWheel.setDefaultCommand(upperFlyWheel.joyStickControl(() -> + // driver.getLeftTrigger())); + + 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()); + } +} +// frontright has CanId 1, 2 and opposite side of RSL and by the battery +// frontleft has CanId 3, 4 and next to the RS light +// backright has CanId 7, 8 and next to the feeder motor with CanId 12 +// backleft has CanId 5, 6 and next to lower flywheel motor with CanId 10 diff --git a/src/main/java/frc/robot/blackteam/FeederSubsystem.java b/src/main/java/frc/robot/blackteam/FeederSubsystem.java new file mode 100644 index 00000000..1e8d1ed2 --- /dev/null +++ b/src/main/java/frc/robot/blackteam/FeederSubsystem.java @@ -0,0 +1,39 @@ +// 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.blackteam; + +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import yams.mechanisms.velocity.FlyWheel; + +/** Add your docs here. */ +public class FeederSubsystem extends GenericSubsystem { + private FlyWheel feeder; + + /** Creates a new Feeder configured from JSON. */ + public FeederSubsystem() { + super("feeder.json"); + feeder = (FlyWheel) devices.get("feeder"); + } + + public Command setSpeed(double speed) { + return feeder.set(speed); + } + + public Command joyStickControl(Supplier speedSupplier) { + return feeder.set(speedSupplier); + } + + @Override + public void periodic() { + feeder.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + feeder.simIterate(); + } +} diff --git a/src/main/java/frc/robot/blackteam/LowerFlyWheel.java b/src/main/java/frc/robot/blackteam/LowerFlyWheel.java new file mode 100644 index 00000000..c5c0c9d7 --- /dev/null +++ b/src/main/java/frc/robot/blackteam/LowerFlyWheel.java @@ -0,0 +1,156 @@ +package frc.robot.blackteam; + +import static edu.wpi.first.units.Units.RPM; +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 edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import yams.exceptions.FlyWheelConfigurationException; +import yams.mechanisms.velocity.FlyWheel; + +public class LowerFlyWheel extends GenericSubsystem { + private AngularVelocity setpoint = RPM.of(0); + + private InterpolatingDoubleTreeMap distanceToVelocityMap = + InterpolatingDoubleTreeMap.ofEntries( + Map.entry(0.0, 0.0), + Map.entry(0.5, 500.0), + Map.entry(1.0, 1000.0), + Map.entry(1.5, 1500.0)); + + private FlyWheel lowerFlyWheel; + + /** Creates a new Shooter. */ + public LowerFlyWheel() { + super("lowerflywheel.json"); + lowerFlyWheel = (FlyWheel) devices.get("lowerflywheel"); + } + + public Command setLowerFlyWheelSpeed(double speed) { + return lowerFlyWheel.set(speed); + } + + /** + * @param dutyCycle DutyCycle to set. + * @return {@link edu.wpi.first.wpilibj2.command.RunCommand} + */ + public Command set(double dutyCycle) { + return lowerFlyWheel.set(dutyCycle); + } + + public Command launchToDistance(Supplier distanceSupplier) { + return lowerFlyWheel.setSpeed(distanceSupplier); + } + + public Command spinAtSpeed(Supplier speedSupplier) { + return lowerFlyWheel.setSpeed(speedSupplier); + } + + public Command joyStickControl(Supplier speedSupplier) { + return lowerFlyWheel.set(speedSupplier); + } + + public Supplier getVelocity() { + return () -> lowerFlyWheel.getSpeed(); + } + + public BooleanSupplier isNearTarget(AngularVelocity range) { + return lowerFlyWheel.isNear(setpoint, range); + } + + @Override + public void periodic() { + lowerFlyWheel.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + lowerFlyWheel.simIterate(); + } + + public Command setVelocity(AngularVelocity speed) { + setpoint = speed; + return lowerFlyWheel.setSpeed(speed); + } + + public Command systemID() { + return sysId(Volts.of(12), Volts.of(1).per(Second), Seconds.of(7)); + } + + public Command sysId(Voltage maximumVoltage, Velocity step, Time duration) { + SysIdRoutine routine = lowerFlyWheel.getMotor().sysId(maximumVoltage, step, duration); + AngularVelocity max = RPM.of(1000); + AngularVelocity min = RPM.of(-1000); + if (lowerFlyWheel.getShooterConfig().getUpperSoftLimit().isPresent()) { + max = lowerFlyWheel.getShooterConfig().getUpperSoftLimit().get().minus(RPM.of(1)); + } else { + throw new FlyWheelConfigurationException( + "FlyWheel upper hard and motor controller soft limit is empty", + "Cannot create SysIdRoutine.", + "withSoftLimit(Angle,Angle)"); + } + if (lowerFlyWheel.getShooterConfig().getLowerSoftLimit().isPresent()) { + min = lowerFlyWheel.getShooterConfig().getLowerSoftLimit().get().plus(RPM.of(1)); + } else { + throw new FlyWheelConfigurationException( + "FlyWheel lower hard and motor controller soft limit is empty", + "Cannot create SysIdRoutine.", + "withSoftLimit(Angle,Angle)"); + } + Trigger maxTrigger = lowerFlyWheel.gte(max); + Trigger minTrigger = lowerFlyWheel.lte(min); + + Command group = + Commands.print("Starting SysId") + .andThen(Commands.runOnce(lowerFlyWheel.getMotor()::stopClosedLoopController)) + .andThen( + routine + .dynamic(Direction.kForward) + .until(maxTrigger) + .finallyDo( + (interrupted) -> { + if (maxTrigger.getAsBoolean()) System.err.println("Interrupted"); + else System.err.println("Forward done"); + })) + .andThen(Commands.waitSeconds(3)) + .andThen( + routine + .dynamic(Direction.kReverse) + .until(minTrigger) + .finallyDo(() -> System.err.println("Reverse done"))) + .andThen(Commands.waitSeconds(3)) + .andThen( + routine + .quasistatic(Direction.kForward) + .until(maxTrigger) + .finallyDo(() -> System.err.println("Quasistatic forward done"))) + .andThen(Commands.waitSeconds(3)) + .andThen( + routine + .quasistatic(Direction.kReverse) + .until(minTrigger) + .finallyDo(() -> System.err.println("Quasistatic reverse done"))); + + if (lowerFlyWheel.getShooterConfig().getTelemetryName().isPresent()) { + group = group.andThen(Commands.print(getName() + " SysId test done.")); + } + return group + .withName(this.getName() + " SysId") + .finallyDo(lowerFlyWheel.getMotor()::startClosedLoopController); + } +} diff --git a/src/main/java/frc/robot/blackteam/UpperFlyWheel.java b/src/main/java/frc/robot/blackteam/UpperFlyWheel.java new file mode 100644 index 00000000..8d95a44b --- /dev/null +++ b/src/main/java/frc/robot/blackteam/UpperFlyWheel.java @@ -0,0 +1,156 @@ +package frc.robot.blackteam; + +import static edu.wpi.first.units.Units.RPM; +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 edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; +import org.frc5010.common.arch.GenericSubsystem; +import yams.exceptions.FlyWheelConfigurationException; +import yams.mechanisms.velocity.FlyWheel; + +public class UpperFlyWheel extends GenericSubsystem { + private AngularVelocity setpoint = RPM.of(0); + + private InterpolatingDoubleTreeMap distanceToVelocityMap = + InterpolatingDoubleTreeMap.ofEntries( + Map.entry(0.0, 0.0), + Map.entry(0.5, 500.0), + Map.entry(1.0, 1000.0), + Map.entry(1.5, 1500.0)); + + private FlyWheel upperFlyWheel; + + /** Creates a new Shooter. */ + public UpperFlyWheel() { + super("upperflywheel.json"); + upperFlyWheel = (FlyWheel) devices.get("upperflywheel"); + } + + public Command setUpperFLyWheelSpeed(double speed) { + return upperFlyWheel.set(speed); + } + + /** + * @param dutyCycle DutyCycle to set. + * @return {@link edu.wpi.first.wpilibj2.command.RunCommand} + */ + public Command set(double dutyCycle) { + return upperFlyWheel.set(dutyCycle); + } + + public Command launchToDistance(Supplier distanceSupplier) { + return upperFlyWheel.setSpeed(distanceSupplier); + } + + public Command spinAtSpeed(Supplier speedSupplier) { + return upperFlyWheel.setSpeed(speedSupplier); + } + + public Command joyStickControl(Supplier speedSupplier) { + return upperFlyWheel.set(speedSupplier); + } + + public Supplier getVelocity() { + return () -> upperFlyWheel.getSpeed(); + } + + public BooleanSupplier isNearTarget(AngularVelocity range) { + return upperFlyWheel.isNear(setpoint, range); + } + + @Override + public void periodic() { + upperFlyWheel.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + upperFlyWheel.simIterate(); + } + + public Command setVelocity(AngularVelocity speed) { + setpoint = speed; + return upperFlyWheel.setSpeed(speed); + } + + public Command systemID() { + return sysId(Volts.of(12), Volts.of(1).per(Second), Seconds.of(10)); + } + + public Command sysId(Voltage maximumVoltage, Velocity step, Time duration) { + SysIdRoutine routine = upperFlyWheel.getMotor().sysId(maximumVoltage, step, duration); + AngularVelocity max = RPM.of(1000); + AngularVelocity min = RPM.of(-1000); + if (upperFlyWheel.getShooterConfig().getUpperSoftLimit().isPresent()) { + max = upperFlyWheel.getShooterConfig().getUpperSoftLimit().get().minus(RPM.of(1)); + } else { + throw new FlyWheelConfigurationException( + "FlyWheel upper hard and motor controller soft limit is empty", + "Cannot create SysIdRoutine.", + "withSoftLimit(Angle,Angle)"); + } + if (upperFlyWheel.getShooterConfig().getLowerSoftLimit().isPresent()) { + min = upperFlyWheel.getShooterConfig().getLowerSoftLimit().get().plus(RPM.of(1)); + } else { + throw new FlyWheelConfigurationException( + "FlyWheel lower hard and motor controller soft limit is empty", + "Cannot create SysIdRoutine.", + "withSoftLimit(Angle,Angle)"); + } + Trigger maxTrigger = upperFlyWheel.gte(max); + Trigger minTrigger = upperFlyWheel.lte(min); + + Command group = + Commands.print("Starting SysId") + .andThen(Commands.runOnce(upperFlyWheel.getMotor()::stopClosedLoopController)) + .andThen( + routine + .dynamic(Direction.kForward) + .until(maxTrigger) + .finallyDo( + (interrupted) -> { + if (maxTrigger.getAsBoolean()) System.err.println("Interrupted"); + else System.err.println("Forward done"); + })) + .andThen(Commands.waitSeconds(3)) + .andThen( + routine + .dynamic(Direction.kReverse) + .until(minTrigger) + .finallyDo(() -> System.err.println("Reverse done"))) + .andThen(Commands.waitSeconds(3)) + .andThen( + routine + .quasistatic(Direction.kForward) + .until(maxTrigger) + .finallyDo(() -> System.err.println("Quasistatic forward done"))) + .andThen(Commands.waitSeconds(3)) + .andThen( + routine + .quasistatic(Direction.kReverse) + .until(minTrigger) + .finallyDo(() -> System.err.println("Quasistatic reverse done"))); + + if (upperFlyWheel.getShooterConfig().getTelemetryName().isPresent()) { + group = group.andThen(Commands.print(getName() + " SysId test done.")); + } + return group + .withName(this.getName() + " SysId") + .finallyDo(upperFlyWheel.getMotor()::startClosedLoopController); + } +} 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/org/frc5010/common/auto/pathplanner/PathFinderCommand.java b/src/main/java/org/frc5010/common/auto/pathplanner/PathFinderCommand.java index 545652c5..ba225e34 100644 --- a/src/main/java/org/frc5010/common/auto/pathplanner/PathFinderCommand.java +++ b/src/main/java/org/frc5010/common/auto/pathplanner/PathFinderCommand.java @@ -301,7 +301,6 @@ public void initialize() { goalEndState = new GoalEndState(goalEndState.velocityMPS(), targetPose.getRotation()); } } - if (currentPose.getTranslation().getDistance(targetPose.getTranslation()) < 0.1) { output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); finish = true; diff --git a/src/main/java/org/frc5010/common/commands/calibration/SteeringRatioCharacterization.java b/src/main/java/org/frc5010/common/commands/calibration/SteeringRatioCharacterization.java new file mode 100644 index 00000000..fc57b48b --- /dev/null +++ b/src/main/java/org/frc5010/common/commands/calibration/SteeringRatioCharacterization.java @@ -0,0 +1,383 @@ +// 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.commands.calibration; + +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import org.frc5010.common.arch.GenericCommand; +import org.frc5010.common.drive.swerve.GenericSwerveDrivetrain; +import org.frc5010.common.drive.swerve.GenericSwerveModuleInfo; +import org.frc5010.common.telemetry.DisplayBoolean; +import org.frc5010.common.telemetry.DisplayDouble; +import org.frc5010.common.telemetry.DisplayString; +import org.frc5010.common.telemetry.DisplayValuesHelper; + +/** + * Diagnoses steering (azimuth) modules that do not rotate the commanded amount. + * + *

The command walks the commanded module azimuth around in small, equal steps (default 45° + * per step, 32 steps, i.e. four full azimuth revolutions). Because every step is smaller than + * 90°, {@link edu.wpi.first.math.kinematics.SwerveModuleState#optimize} never flips a module, + * so every module should physically rotate exactly the commanded amount, continuously, in the same + * direction. + * + *

At the end of each step three angles are sampled per module and accumulated as wrapped deltas: + * + *

    + *
  • commanded — the azimuth this command asked for + *
  • absolute — {@link GenericSwerveModuleInfo#steerAbsoluteDegrees()}, the + * CANcoder / duty-cycle encoder on the azimuth output, i.e. true shaft rotation + *
  • relative — {@link GenericSwerveModuleInfo#steerRelativeDegrees()}, the steer + * motor's internal encoder scaled by the configured angle conversion factor + *
+ * + * Two ratios fall out, both of which are 1.0 on a healthy module: + * + *
    + *
  • Rotation Ratio = absolute / commanded. Less than 1.0 means the azimuth physically + * moved less than it was told to — a slipping pulley/belt, a stripped gear, or a loose + * encoder magnet. This is the check that catches a module rotating at a different rate than + * its siblings. + *
  • Encoder Ratio = relative / absolute. Not 1.0 means the steer motor turned a + * different number of rotations than the azimuth actually moved, i.e. the configured angle + * conversion factor (gear ratio) does not match the hardware, or something is slipping + * between the motor and the azimuth ring. Multiply the configured angle conversion factor by + * the reported Suggested Conv Scale (= absolute / relative) to correct it. + *
+ * + *

Each module's Rotation Ratio is also compared against the median of all modules, so a single + * bad module is called out by name even when the absolute numbers look plausible. + * + *

Run this with the robot up on blocks. A small drive velocity is required so the + * drivetrain actually commands azimuth (YAGSL holds the last angle when the requested speed is near + * zero), which means the wheels will spin and the robot will drive itself in a spiral if it is on + * the floor. + * + *

Note: on the AdvantageKit drivetrain the absolute and relative angles come from the same + * signal, so Encoder Ratio is always exactly 1.0 there and only Rotation Ratio is meaningful. + */ +public class SteeringRatioCharacterization extends GenericCommand { + /** + * Default azimuth change per step, in degrees. Must stay below 90 to avoid state optimization. + */ + public static final double DEFAULT_STEP_DEGREES = 45.0; + + /** Default number of steps, 32 * 45 degrees = four full azimuth revolutions. */ + public static final int DEFAULT_STEPS = 32; + + /** Default fraction of a full turn a ratio may deviate before being called a fault. */ + public static final double DEFAULT_RATIO_TOLERANCE = 0.05; + + private final DisplayValuesHelper displayValuesHelper = + new DisplayValuesHelper(logPrefix, "Steering Ratio"); + + private final GenericSwerveDrivetrain drivetrain; + + private final double stepDegrees; + private final int totalSteps; + private final double ratioTolerance; + private final double driveSpeedMetersPerSecond; + + private final Time alignTime = Seconds.of(1.5); + private final Time holdTime = Seconds.of(0.5); + private final Time finalSettleTime = Seconds.of(1.5); + + // Test sequence, built in init() + private final List setpointDegrees = new ArrayList<>(); + private final List setpointHoldSeconds = new ArrayList<>(); + private int setpointIndex = 0; + + private final Timer holdTimer = new Timer(); + private boolean finished = false; + + // Per-module accumulators + private double[] lastAbsoluteDegrees; + private double[] lastRelativeDegrees; + private double[] cumulativeAbsoluteDegrees; + private double[] cumulativeRelativeDegrees; + private double[] worstStepErrorDegrees; + private double[] worstFollowErrorDegrees; + private double cumulativeCommandedDegrees = 0.0; + + // Telemetry + private DisplayDouble[] rotationRatioDisplays; + private DisplayDouble[] encoderRatioDisplays; + private DisplayDouble[] conversionScaleDisplays; + private DisplayDouble[] worstStepErrorDisplays; + private DisplayDouble[] worstFollowErrorDisplays; + private DisplayDouble commandedTotalDisplay; + private DisplayDouble progressDisplay; + private DisplayBoolean faultDetectedDisplay; + private DisplayString resultDisplay; + + /** + * Creates a steering ratio characterization with the default sweep. + * + * @param drivetrain the swerve drivetrain to test + */ + public SteeringRatioCharacterization(GenericSwerveDrivetrain drivetrain) { + this(drivetrain, DEFAULT_STEP_DEGREES, DEFAULT_STEPS, DEFAULT_RATIO_TOLERANCE, 0.25); + } + + /** + * Creates a steering ratio characterization. + * + * @param drivetrain the swerve drivetrain to test + * @param stepDegrees azimuth change per step; must be less than 90 so module state optimization + * never flips a module mid-sweep + * @param totalSteps how many steps to take; more steps means more accumulated rotation and a + * tighter ratio estimate + * @param ratioTolerance how far a ratio may sit from 1.0, or from the median of the modules, + * before it is reported as a fault + * @param driveSpeedMetersPerSecond drive velocity requested while stepping; must be large enough + * that the drivetrain actually commands azimuth rather than holding the last angle + */ + public SteeringRatioCharacterization( + GenericSwerveDrivetrain drivetrain, + double stepDegrees, + int totalSteps, + double ratioTolerance, + double driveSpeedMetersPerSecond) { + this.drivetrain = drivetrain; + this.stepDegrees = stepDegrees; + this.totalSteps = totalSteps; + this.ratioTolerance = ratioTolerance; + this.driveSpeedMetersPerSecond = driveSpeedMetersPerSecond; + + if (Math.abs(stepDegrees) >= 90.0) { + throw new IllegalArgumentException( + "stepDegrees must be less than 90 so module optimization does not flip a module"); + } + + int moduleCount = drivetrain.getModulesInfo().length; + rotationRatioDisplays = new DisplayDouble[moduleCount]; + encoderRatioDisplays = new DisplayDouble[moduleCount]; + conversionScaleDisplays = new DisplayDouble[moduleCount]; + worstStepErrorDisplays = new DisplayDouble[moduleCount]; + worstFollowErrorDisplays = new DisplayDouble[moduleCount]; + for (int i = 0; i < moduleCount; i++) { + rotationRatioDisplays[i] = displayValuesHelper.makeDisplayDouble("M" + i + " Rotation Ratio"); + encoderRatioDisplays[i] = displayValuesHelper.makeDisplayDouble("M" + i + " Encoder Ratio"); + conversionScaleDisplays[i] = + displayValuesHelper.makeDisplayDouble("M" + i + " Suggested Conv Scale"); + worstStepErrorDisplays[i] = + displayValuesHelper.makeDisplayDouble("M" + i + " Worst Step Error (deg)"); + worstFollowErrorDisplays[i] = + displayValuesHelper.makeDisplayDouble("M" + i + " Worst Follow Error (deg)"); + } + commandedTotalDisplay = displayValuesHelper.makeDisplayDouble("Commanded Total (deg)"); + progressDisplay = displayValuesHelper.makeDisplayDouble("Progress (%)"); + faultDetectedDisplay = displayValuesHelper.makeDisplayBoolean("Fault Detected"); + resultDisplay = displayValuesHelper.makeDisplayString("Result"); + + addRequirements(drivetrain); + } + + @Override + public void init() { + int moduleCount = drivetrain.getModulesInfo().length; + lastAbsoluteDegrees = new double[moduleCount]; + lastRelativeDegrees = new double[moduleCount]; + cumulativeAbsoluteDegrees = new double[moduleCount]; + cumulativeRelativeDegrees = new double[moduleCount]; + worstStepErrorDegrees = new double[moduleCount]; + worstFollowErrorDegrees = new double[moduleCount]; + cumulativeCommandedDegrees = 0.0; + + setpointDegrees.clear(); + setpointHoldSeconds.clear(); + // Align to zero first; the sample at the end of this hold is the baseline. + setpointDegrees.add(0.0); + setpointHoldSeconds.add(alignTime.in(Seconds)); + for (int step = 1; step <= totalSteps; step++) { + setpointDegrees.add(step * stepDegrees); + setpointHoldSeconds.add(holdTime.in(Seconds)); + } + // Repeat the last setpoint with a long settle so the final sample is not polluted by + // steering lag; only the final position matters, intermediate lag cancels out. + setpointDegrees.add(totalSteps * stepDegrees); + setpointHoldSeconds.add(finalSettleTime.in(Seconds)); + + setpointIndex = 0; + finished = false; + + faultDetectedDisplay.setValue(false); + resultDisplay.setValue("Running - keep the robot on blocks"); + progressDisplay.setValue(0.0); + + holdTimer.reset(); + holdTimer.start(); + } + + @Override + public void execute() { + double commanded = setpointDegrees.get(setpointIndex); + commandAzimuth(commanded); + + if (!holdTimer.hasElapsed(setpointHoldSeconds.get(setpointIndex))) { + return; + } + + sample(commanded, setpointIndex > 0); + + setpointIndex++; + progressDisplay.setValue(100.0 * setpointIndex / setpointDegrees.size()); + if (setpointIndex >= setpointDegrees.size()) { + report(); + finished = true; + return; + } + holdTimer.reset(); + } + + /** Points every module at the given azimuth by requesting a robot-relative translation. */ + private void commandAzimuth(double azimuthDegrees) { + double radians = Math.toRadians(azimuthDegrees); + drivetrain.drive( + new ChassisSpeeds( + driveSpeedMetersPerSecond * Math.cos(radians), + driveSpeedMetersPerSecond * Math.sin(radians), + 0.0)); + } + + /** + * Records the current module angles. + * + * @param commandedDegrees the azimuth that was being held + * @param accumulate false for the baseline sample, which only seeds the last-angle arrays + */ + private void sample(double commandedDegrees, boolean accumulate) { + GenericSwerveModuleInfo[] modules = drivetrain.getModulesInfo(); + double commandedStep = accumulate ? wrapHalf(commandedDegrees - lastCommandedDegrees()) : 0.0; + + for (int i = 0; i < modules.length; i++) { + double absolute = modules[i].steerAbsoluteDegrees(); + double relative = modules[i].steerRelativeDegrees(); + + if (accumulate) { + double absoluteStep = wrapHalf(absolute - lastAbsoluteDegrees[i]); + double relativeStep = wrapHalf(relative - lastRelativeDegrees[i]); + cumulativeAbsoluteDegrees[i] += absoluteStep; + cumulativeRelativeDegrees[i] += relativeStep; + worstStepErrorDegrees[i] = + Math.max(worstStepErrorDegrees[i], Math.abs(absoluteStep - commandedStep)); + worstFollowErrorDegrees[i] = + Math.max(worstFollowErrorDegrees[i], Math.abs(wrapHalf(absolute - commandedDegrees))); + } + + lastAbsoluteDegrees[i] = absolute; + lastRelativeDegrees[i] = relative; + } + + if (accumulate) { + cumulativeCommandedDegrees += commandedStep; + commandedTotalDisplay.setValue(cumulativeCommandedDegrees); + } + } + + /** The setpoint held during the previous sample. */ + private double lastCommandedDegrees() { + return setpointDegrees.get(Math.max(0, setpointIndex - 1)); + } + + /** Computes the ratios, publishes them, and writes a human readable verdict. */ + private void report() { + int moduleCount = cumulativeAbsoluteDegrees.length; + double[] rotationRatios = new double[moduleCount]; + double[] encoderRatios = new double[moduleCount]; + + for (int i = 0; i < moduleCount; i++) { + rotationRatios[i] = + cumulativeCommandedDegrees == 0.0 + ? Double.NaN + : cumulativeAbsoluteDegrees[i] / cumulativeCommandedDegrees; + encoderRatios[i] = + cumulativeAbsoluteDegrees[i] == 0.0 + ? Double.NaN + : cumulativeRelativeDegrees[i] / cumulativeAbsoluteDegrees[i]; + + rotationRatioDisplays[i].setValue(rotationRatios[i]); + encoderRatioDisplays[i].setValue(encoderRatios[i]); + conversionScaleDisplays[i].setValue(1.0 / encoderRatios[i]); + worstStepErrorDisplays[i].setValue(worstStepErrorDegrees[i]); + worstFollowErrorDisplays[i].setValue(worstFollowErrorDegrees[i]); + } + + double medianRotationRatio = median(rotationRatios); + StringBuilder verdict = new StringBuilder(); + boolean fault = false; + + for (int i = 0; i < moduleCount; i++) { + List problems = new ArrayList<>(); + if (Double.isNaN(rotationRatios[i]) || Math.abs(rotationRatios[i] - 1.0) > ratioTolerance) { + problems.add(String.format("rotates %.1f%% of commanded", 100.0 * rotationRatios[i])); + } + if (Math.abs(rotationRatios[i] - medianRotationRatio) > ratioTolerance) { + problems.add( + String.format( + "%.1f%% off the other modules", 100.0 * (rotationRatios[i] - medianRotationRatio))); + } + if (Double.isNaN(encoderRatios[i]) || Math.abs(encoderRatios[i] - 1.0) > ratioTolerance) { + problems.add( + String.format( + "gearing conversion off by %.1f%% (scale conv factor by %.4f)", + 100.0 * (encoderRatios[i] - 1.0), 1.0 / encoderRatios[i])); + } + if (!problems.isEmpty()) { + fault = true; + verdict + .append("M") + .append(i) + .append(": ") + .append(String.join("; ", problems)) + .append(" | "); + } + } + + if (!fault) { + verdict.append( + String.format( + "All modules within %.0f%% over %.0f deg commanded", + 100.0 * ratioTolerance, cumulativeCommandedDegrees)); + } + + faultDetectedDisplay.setValue(fault); + resultDisplay.setValue(verdict.toString()); + log(logPrefix + ": " + verdict); + } + + /** Wraps a degree delta into (-90, 90], the largest range a single step can legally span. */ + private static double wrapHalf(double degrees) { + return MathUtil.inputModulus(degrees, -90.0, 90.0); + } + + private static double median(double[] values) { + double[] sorted = values.clone(); + Arrays.sort(sorted); + int middle = sorted.length / 2; + return sorted.length % 2 == 0 ? (sorted[middle - 1] + sorted[middle]) / 2.0 : sorted[middle]; + } + + @Override + public void stop(boolean interrupted) { + drivetrain.stop(); + holdTimer.stop(); + if (interrupted) { + resultDisplay.setValue("Interrupted before completing the sweep"); + } + } + + @Override + public boolean isFinished() { + return finished; + } +} 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..7ecd3ee2 100644 --- a/src/main/java/org/frc5010/common/config/json/AKitSwerveDrivetrainJson.java +++ b/src/main/java/org/frc5010/common/config/json/AKitSwerveDrivetrainJson.java @@ -150,15 +150,17 @@ public void createDriveTrain(GenericRobot robot) { new ModuleIOSparkTalon(config, config.BackRight), (pose) -> {}); } else if ("Spark".equals(type)) { + // GyroIOPigeon2 samples on the Spark odometry thread in this configuration, so only that + // thread needs to be created. SparkOdometryThread.createInstance(); driveFunctions = new AkitSwerveDrive( config, new GyroIOPigeon2(config), - new ModuleIOSpark(0), - new ModuleIOSpark(1), - new ModuleIOSpark(2), - new ModuleIOSpark(3), + new ModuleIOSpark(config, config.FrontLeft), + new ModuleIOSpark(config, config.FrontRight), + new ModuleIOSpark(config, config.BackLeft), + new ModuleIOSpark(config, config.BackRight), (pose) -> {}); } else if ("TalonFX".equals(type)) { TalonFXOdometryThread.createInstance(config); diff --git a/src/main/java/org/frc5010/common/config/json/RobotsJson.java b/src/main/java/org/frc5010/common/config/json/RobotsJson.java index 8ee962c1..f314a6a2 100644 --- a/src/main/java/org/frc5010/common/config/json/RobotsJson.java +++ b/src/main/java/org/frc5010/common/config/json/RobotsJson.java @@ -42,7 +42,7 @@ public GenericRobot createRobot() { for (String robotName : robots.keySet()) { RobotIdentity robotIdentity = robots.get(robotName); if ((RobotBase.isSimulation() && robotIdentity.simulate) - || (competitionSwitch.map(it -> it.get()).orElse(false) && robotIdentity.competition) + || (competitionSwitch.map(it -> !it.get()).orElse(false) && robotIdentity.competition) || robotIdentity.id.equals(whichRobot)) { robotDirectory = robotName; break; diff --git a/src/main/java/org/frc5010/common/config/json/devices/ReflectionsManager.java b/src/main/java/org/frc5010/common/config/json/devices/ReflectionsManager.java index 85eb899f..419546d2 100644 --- a/src/main/java/org/frc5010/common/config/json/devices/ReflectionsManager.java +++ b/src/main/java/org/frc5010/common/config/json/devices/ReflectionsManager.java @@ -29,7 +29,7 @@ public enum SparkBaseType { put(VENDOR.REV, "com.revrobotics.spark.SparkBase"); put(VENDOR.PHOENIX5, "com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX"); put(VENDOR.PHOENIX6, "com.ctre.phoenix6.hardware.TalonFXS"); - put(VENDOR.THRIFTYBOT, "com.thethriftybot.ThriftyNova"); + put(VENDOR.THRIFTYBOT, "com.thethriftybot.devices.ThriftyNova"); put(VENDOR.YAMS, "yams.motorcontrollers.SmartMotorController"); } }; diff --git a/src/main/java/org/frc5010/common/config/json/devices/YamsConfigCommon.java b/src/main/java/org/frc5010/common/config/json/devices/YamsConfigCommon.java index 314728b8..ddbad3da 100644 --- a/src/main/java/org/frc5010/common/config/json/devices/YamsConfigCommon.java +++ b/src/main/java/org/frc5010/common/config/json/devices/YamsConfigCommon.java @@ -149,6 +149,7 @@ public static Optional configureSmartMotorController( case "nova": case "thriftynova": case "thrifty_nova": + break; default: motorConfig.withOpenLoopRampRate(UnitsParser.parseTime(motorSystemId.openLoopRamp)); break; diff --git a/src/main/java/org/frc5010/common/drive/GenericDrivetrain.java b/src/main/java/org/frc5010/common/drive/GenericDrivetrain.java index 08b774a5..2ddf9bb8 100644 --- a/src/main/java/org/frc5010/common/drive/GenericDrivetrain.java +++ b/src/main/java/org/frc5010/common/drive/GenericDrivetrain.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import java.util.Comparator; import java.util.HashMap; import java.util.List; @@ -237,7 +238,7 @@ public Command createDefaultCommand(Controller driver) { this, () -> driver.getLeftYAxis(), () -> driver.getLeftXAxis(), - () -> driver.getRightXAxis(), + () -> driver.getLeftTrigger(), () -> isFieldOrientedDrive.getValue()); } @@ -257,6 +258,17 @@ public Command createDefaultTestCommand(Controller driver) { () -> isFieldOrientedDrive.getValue()); } + /** + * Deterministic azimuth diagnostic command. Drivetrains that support direct azimuth control (e.g. + * swerve) override this to step the modules through a fixed sequence of angles; the default + * implementation does nothing. + * + * @return the azimuth step-test command + */ + public Command azimuthStepTestCommand() { + return Commands.none(); + } + /** Resets the encoders */ public void resetEncoders() {} 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..58226660 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java +++ b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveDrivetrain.java @@ -52,6 +52,7 @@ import org.frc5010.common.auto.pathplanner.PathFinderCommand; import org.frc5010.common.commands.DriveToPoseSupplier; import org.frc5010.common.commands.JoystickToSwerve; +import org.frc5010.common.commands.calibration.SteeringRatioCharacterization; import org.frc5010.common.constants.GenericDrivetrainConstants; import org.frc5010.common.constants.RobotConstantsDef; import org.frc5010.common.drive.GenericDrivetrain; @@ -60,6 +61,7 @@ import org.frc5010.common.drive.swerve_utils.SwerveSetpointGenerator5010; import org.frc5010.common.sensors.Controller; import org.json.simple.parser.ParseException; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -82,6 +84,10 @@ public GenericSwerveDrivetrain( setDrivetrainPoseEstimator(swerveDrive.initializePoseEstimator()); initializeSimulation(swerveConstants); driveTrainSimulationSupplier = swerveDrive.getDriveTrainSimulationSupplier(); + + // Expose the deterministic azimuth diagnostic as a dashboard button so it can be run + // (while enabled, robot on blocks) without entering Test mode. + SmartDashboard.putData("Azimuth Step Test", azimuthStepTestCommand()); } @Override @@ -167,6 +173,16 @@ public GenericDrivetrainConstants getSwerveConstants() { return swerveConstants; } + /** + * Gets per-module steering and drive readings, including both the absolute and the relative + * (motor internal) azimuth angles. + * + * @return one {@link GenericSwerveModuleInfo} per module + */ + public GenericSwerveModuleInfo[] getModulesInfo() { + return swerveDrive.getModulesInfo(); + } + @Override protected ChassisSpeeds getChassisSpeeds() { return swerveDrive.getRobotVelocity(); @@ -742,7 +758,7 @@ public Command driveWithSetpointGeneratorOrientationConsidered( public Command createDefaultCommand(Controller driverXbox) { DoubleSupplier leftX = () -> driverXbox.getLeftXAxis(); DoubleSupplier leftY = () -> driverXbox.getLeftYAxis(); - DoubleSupplier rightX = () -> driverXbox.getRightXAxis(); + DoubleSupplier rightX = () -> driverXbox.getLeftTrigger(); BooleanSupplier isFieldOriented = () -> isFieldOrientedDrive.getValue(); /** @@ -765,10 +781,12 @@ public Command createDefaultCommand(Controller driverXbox) { public Command createDefaultTestCommand(Controller driverXbox) { DoubleSupplier leftX = () -> driverXbox.getLeftXAxis(); DoubleSupplier leftY = () -> driverXbox.getLeftYAxis(); - DoubleSupplier rightX = () -> driverXbox.getRightXAxis(); + DoubleSupplier rightX = () -> driverXbox.getLeftTrigger(); BooleanSupplier isFieldOriented = () -> isFieldOrientedDrive.getValue(); driverXbox.createAButton().whileTrue(sysIdDriveMotorCommand()); + driverXbox.createBButton().whileTrue(steeringRatioCharacterizationCommand()); + SmartDashboard.putData("Steering Ratio Test", steeringRatioCharacterizationCommand()); // driverXbox.createBButton().whileTrue(sysIdAngleMotorCommand()); // return Commands.run(() -> SwerveDriveTest.centerModules(swerveDrive), this); return new JoystickToSwerve( @@ -787,6 +805,51 @@ public Command sysIdAngleMotorCommand() { return swerveDrive.sysIdAngleMotorCommand(this); } + /** + * Sweeps the module azimuths in equal steps and reports, per module, how much they actually + * rotated versus how much they were told to. Catches a module whose steering gearing is slipping + * or whose configured angle conversion factor does not match the hardware. + * + *

Run with the robot up on blocks; the wheels spin during the test. + * + * @return the characterization {@link Command} + */ + public Command steeringRatioCharacterizationCommand() { + return new SteeringRatioCharacterization(this); + } + + /** Azimuth angles (deg) the step test drives every module through, in order. */ + private static final double[] AZIMUTH_TEST_ANGLES = {0.0, 90.0, 180.0, -90.0, 45.0, 0.0}; + + /** How long (seconds) to hold each step so the modules can settle. */ + private static final double AZIMUTH_TEST_DWELL_SECONDS = 1.0; + + /** + * Deterministic azimuth diagnostic: points every module at a fixed sequence of angles, holding + * each for a fixed dwell, while the drive motors stay stopped. Run it with the robot on blocks + * and review {@code Swerve/Diag/*} (measured/error/steer effort) against {@code + * Swerve/Diag/StepTest/targetDeg} to see which module fails to reach its commanded rotation. The + * per-module diagnostics require the YAGSL telemetry verbosity to be HIGH. + * + * @return the step-test command (requires this drivetrain, interrupting the default drive + * command) + */ + @Override + public Command azimuthStepTestCommand() { + Command sequence = Commands.none(); + for (double angle : AZIMUTH_TEST_ANGLES) { + final double target = angle; + sequence = + sequence.andThen( + run(() -> { + Logger.recordOutput("Swerve/Diag/StepTest/targetDeg", target); + swerveDrive.pointModulesAt(target); + }) + .withTimeout(AZIMUTH_TEST_DWELL_SECONDS)); + } + return sequence.withName("AzimuthStepTest"); + } + public void resetEncoders() { swerveDrive.resetEncoders(); } 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..9261283e 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java +++ b/src/main/java/org/frc5010/common/drive/swerve/GenericSwerveModuleInfo.java @@ -4,8 +4,10 @@ package org.frc5010.common.drive.swerve; +import edu.wpi.first.math.kinematics.SwerveModuleState; import org.frc5010.common.drive.swerve.akit.Module; import swervelib.SwerveModule; +import swervelib.telemetry.SwerveDriveTelemetry; /** Add your docs here. */ public record GenericSwerveModuleInfo( @@ -23,7 +25,23 @@ public GenericSwerveModuleInfo(SwerveModule module) { module.getDriveMotor().getPosition(), module.getDriveMotor().getVelocity(), module.getAngleMotor().getVelocity(), - module.getState().angle.getDegrees()); + desiredSteerDegrees(module)); + } + + /** + * The module's commanded (desired) steer angle in degrees, taken from YAGSL's post-optimization + * telemetry. Falls back to the measured state angle before any command has been issued. + * + * @param module the swerve module + * @return the desired steer angle in degrees + */ + private static double desiredSteerDegrees(SwerveModule module) { + SwerveModuleState[] desired = SwerveDriveTelemetry.desiredStatesObj; + int index = module.moduleNumber; + if (desired != null && index >= 0 && index < desired.length && desired[index] != null) { + return desired[index].angle.getDegrees(); + } + return module.getState().angle.getDegrees(); } public GenericSwerveModuleInfo(Module module) { diff --git a/src/main/java/org/frc5010/common/drive/swerve/SwerveDriveFunctions.java b/src/main/java/org/frc5010/common/drive/swerve/SwerveDriveFunctions.java index ba3ac82d..a73633ec 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/SwerveDriveFunctions.java +++ b/src/main/java/org/frc5010/common/drive/swerve/SwerveDriveFunctions.java @@ -122,6 +122,16 @@ public void periodic() { // Default empty } + /** + * Commands every module's azimuth to the same absolute angle, in degrees. Default implementation + * does nothing; drivetrains that support direct azimuth control should override it. + * + * @param degrees the absolute steer angle to command, in degrees + */ + public void pointModulesAt(double degrees) { + // Default empty + } + public void updateSimulation() { // Default empty } 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..f63ddaa3 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java +++ b/src/main/java/org/frc5010/common/drive/swerve/YAGSLSwerveDrivetrain.java @@ -13,6 +13,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.DriveFeedforwards; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; @@ -44,6 +45,7 @@ import org.frc5010.common.constants.GenericDrivetrainConstants; import org.frc5010.common.drive.pose.DrivePoseEstimator; import org.frc5010.common.drive.pose.SwerveFunctionsPose; +import org.littletonrobotics.junction.Logger; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -612,6 +614,116 @@ public GenericSwerveModuleInfo[] getModulesInfo() { return moduleInfos; } + /** Azimuth error (deg) above which a settled module is flagged as misaligned. */ + private static final double AZIMUTH_ERROR_TOLERANCE_DEG = 5.0; + + /** Steer velocity (deg/s) below which a module is considered settled (not slewing). */ + private static final double AZIMUTH_SETTLED_VELOCITY_DPS = 10.0; + + @Override + public void periodic() { + logModuleDiagnostics(); + } + + /** Last azimuth commanded by {@link #pointModulesAt(double)}; NaN until the first command. */ + private double lastPointModulesTarget = Double.NaN; + + /** + * Commands every module's azimuth to the same absolute angle. Intended for deterministic + * diagnostics (see {@code azimuthStepTestCommand}); the drive motors are left stopped. + * + *

At the start of each new target this queues {@link + * SwerveDrive#synchronizeModuleEncoders()} and commands through {@link + * SwerveModule#setDesiredState(SwerveModuleState, boolean, double)}, which consumes the queued + * sync and re-seeds the internal (motor) encoder from the absolute encoder before commanding — + * mirroring how YAGSL corrects internal-encoder drift "in small doses" while stopped. The + * drive-feedforward overload is used so no azimuth optimization or anti-jitter is applied (the + * commanded angle stays exact) and the drive motor is held at zero. While dwelling on the same + * target it holds with {@link SwerveModule#setAngle(double)} (internal encoder only, no further + * sync), so the logged {@code Swerve/Diag/errorDeg} reflects the drift accrued during a single + * move from a freshly-synced start. That isolates per-move steering error from the cumulative + * drift seen when sync is bypassed entirely (a healthy gear train tracks within ~1 deg; a module + * with slip/backlash/wrong ratio shows the per-move error directly). + * + * @param degrees the absolute steer angle to command, in degrees + */ + @Override + public void pointModulesAt(double degrees) { + boolean newStep = degrees != lastPointModulesTarget; + lastPointModulesTarget = degrees; + if (newStep) { + swerveDrive.synchronizeModuleEncoders(); + SwerveModuleState target = new SwerveModuleState(0.0, Rotation2d.fromDegrees(degrees)); + for (SwerveModule module : swerveDrive.getModules()) { + module.setDesiredState(target, true, 0.0); + } + } else { + for (SwerveModule module : swerveDrive.getModules()) { + module.setAngle(degrees); + } + } + } + + /** + * Logs per-module azimuth diagnostics under {@code Swerve/Diag/*} so desired-vs-actual rotation + * can be reviewed live in AdvantageScope and replayed from the log. Only runs when the YAGSL + * telemetry verbosity is HIGH or above, matching the rest of the swerve high-verbosity logging. + */ + private void logModuleDiagnostics() { + if (SwerveDriveTelemetry.verbosity.ordinal() < TelemetryVerbosity.HIGH.ordinal()) { + return; + } + SwerveModule[] modules = swerveDrive.getModules(); + SwerveModuleState[] desiredStates = SwerveDriveTelemetry.desiredStatesObj; + int count = modules.length; + double[] desiredDeg = new double[count]; + double[] measuredDeg = new double[count]; + double[] errorDeg = new double[count]; + double[] absMinusRelDeg = new double[count]; + double[] steerAppliedOutput = new double[count]; + double[] steerVoltage = new double[count]; + double[] steerVelocityDps = new double[count]; + boolean[] absoluteEncoderReadIssue = new boolean[count]; + boolean[] misaligned = new boolean[count]; + double maxAbsErrorDeg = 0.0; + for (int i = 0; i < count; i++) { + SwerveModule module = modules[i]; + double measured = module.getAbsolutePosition(); + double relative = module.getRelativePosition(); + // Use YAGSL's post-optimization commanded state as the true target; fall back to the + // measured state on the first loops before any command has been issued. + double desired = + (desiredStates != null && i < desiredStates.length && desiredStates[i] != null) + ? desiredStates[i].angle.getDegrees() + : module.getState().angle.getDegrees(); + double error = MathUtil.inputModulus(desired - measured, -180.0, 180.0); + double steerVel = module.getAngleMotor().getVelocity(); + + desiredDeg[i] = desired; + measuredDeg[i] = measured; + errorDeg[i] = error; + absMinusRelDeg[i] = MathUtil.inputModulus(measured - relative, -180.0, 180.0); + steerAppliedOutput[i] = module.getAngleMotor().getAppliedOutput(); + steerVoltage[i] = module.getAngleMotor().getVoltage(); + steerVelocityDps[i] = steerVel; + absoluteEncoderReadIssue[i] = module.getAbsoluteEncoderReadIssue(); + misaligned[i] = + Math.abs(error) > AZIMUTH_ERROR_TOLERANCE_DEG + && Math.abs(steerVel) < AZIMUTH_SETTLED_VELOCITY_DPS; + maxAbsErrorDeg = Math.max(maxAbsErrorDeg, Math.abs(error)); + } + Logger.recordOutput("Swerve/Diag/desiredAngleDeg", desiredDeg); + Logger.recordOutput("Swerve/Diag/measuredAngleDeg", measuredDeg); + Logger.recordOutput("Swerve/Diag/errorDeg", errorDeg); + Logger.recordOutput("Swerve/Diag/absMinusRelDeg", absMinusRelDeg); + Logger.recordOutput("Swerve/Diag/steerAppliedOutput", steerAppliedOutput); + Logger.recordOutput("Swerve/Diag/steerVoltage", steerVoltage); + Logger.recordOutput("Swerve/Diag/steerVelocityDps", steerVelocityDps); + Logger.recordOutput("Swerve/Diag/absoluteEncoderReadIssue", absoluteEncoderReadIssue); + Logger.recordOutput("Swerve/Diag/misaligned", misaligned); + Logger.recordOutput("Swerve/Diag/maxAbsErrorDeg", maxAbsErrorDeg); + } + @Override public void stop() { swerveDrive.drive(new ChassisSpeeds()); 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..75676e1c 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 @@ -81,6 +81,17 @@ public class AkitSwerveDrive extends SwerveDriveFunctions { private SwerveDrivePoseEstimator poseEstimator; private final Consumer resetSimulationPoseCallBack; + // Diagnostics: last commanded (optimized) module setpoints and chassis speeds. Logged every loop + // so setpoint-vs-measured is visible even while disabled, when SwerveStates/Setpoints is blanked. + private SwerveModuleState[] lastSetpointStates = + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + private ChassisSpeeds lastSetpointSpeeds = new ChassisSpeeds(); + public AkitSwerveDrive( AkitSwerveConfig config, GyroIO gyroIO, @@ -171,8 +182,15 @@ public void periodic() { // Update gyro angle if (gyroInputs.connected) { - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; + // Use the real gyro angle. When the gyro samples on a different odometry thread than the + // modules (e.g. the Spark configuration, where the modules use the Spark thread but the + // Pigeon uses the Phoenix thread), its high-frequency queue can be empty even though the + // modules produced samples. Fall back to the latest gyro reading for those samples instead + // of indexing past the end of the array. + rawGyroRotation = + i < gyroInputs.odometryYawPositions.length + ? gyroInputs.odometryYawPositions[i] + : gyroInputs.yawPosition; } else { // Use the angle delta from the kinematics and module deltas Twist2d twist = kinematics.toTwist2d(moduleDeltas); @@ -185,6 +203,9 @@ public void periodic() { // Update gyro alert gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.CURRENT_MODE != Mode.SIM); + + // Always-on swerve diagnostics (commanded vs measured per module). + logModuleDiagnostics(); } /** @@ -209,6 +230,30 @@ public void runVelocity(ChassisSpeeds speeds) { // Log optimized setpoints (runSetpoint mutates each state) Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); + + // Retain the commanded setpoints for the always-on diagnostics in periodic(). + lastSetpointStates = setpointStates; + lastSetpointSpeeds = speeds; + } + + /** + * Logs per-module commanded-vs-measured azimuth and drive diagnostics every loop, independent of + * enable state, to make wrong-direction / tracking issues diagnosable from a log. + */ + private void logModuleDiagnostics() { + for (int i = 0; i < 4; i++) { + SwerveModuleState measured = modules[i].getState(); + SwerveModuleState setpoint = lastSetpointStates[i]; + double angleErrorDeg = Math.abs(measured.angle.minus(setpoint.angle).getDegrees()); + String base = "Drive/Diag/Module" + i + "/"; + Logger.recordOutput(base + "SetpointAngleDeg", setpoint.angle.getDegrees()); + Logger.recordOutput(base + "MeasuredAngleDeg", measured.angle.getDegrees()); + Logger.recordOutput(base + "AngleErrorDeg", angleErrorDeg); + Logger.recordOutput(base + "SetpointSpeedMps", setpoint.speedMetersPerSecond); + Logger.recordOutput(base + "MeasuredSpeedMps", measured.speedMetersPerSecond); + } + Logger.recordOutput("Drive/Diag/ChassisSpeedsSetpoint", lastSetpointSpeeds); + Logger.recordOutput("Drive/Diag/ChassisSpeedsMeasured", getChassisSpeeds()); } /** Runs the drive in a straight line with the specified drive output. */ @@ -235,6 +280,22 @@ public void stop() { runVelocity(new ChassisSpeeds()); } + /** + * Points every module's azimuth at the same absolute angle with the drive motors stopped. Backs + * the azimuth step test; also records the commanded angle as the module setpoint so the always-on + * Drive/Diag diagnostics report target-vs-measured error during the test. + * + * @param degrees the absolute steer angle to command, in degrees + */ + @Override + public void pointModulesAt(double degrees) { + Rotation2d target = Rotation2d.fromDegrees(degrees); + for (int i = 0; i < modules.length; i++) { + modules[i].pointAt(target); + lastSetpointStates[i] = new SwerveModuleState(0.0, target); + } + } + /** * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will * return to their normal orientations the next time a nonzero velocity is requested. diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/DriveConstants.java b/src/main/java/org/frc5010/common/drive/swerve/akit/DriveConstants.java index 580b4b19..74ffe626 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/DriveConstants.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/DriveConstants.java @@ -15,10 +15,11 @@ import edu.wpi.first.math.util.Units; public class DriveConstants { - public static final double maxSpeedMetersPerSec = 4.8; + // Buttercup values, derived from its YAGSL configuration (buttercup_swerve/yagsl_swerve) + public static final double maxSpeedMetersPerSec = 5.93; public static final double odometryFrequency = 100.0; // Hz - public static final double trackWidth = Units.inchesToMeters(26.5); - public static final double wheelBase = Units.inchesToMeters(26.5); + public static final double trackWidth = Units.inchesToMeters(18.5625); + public static final double wheelBase = Units.inchesToMeters(18.5625); public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); public static final Translation2d[] moduleTranslations = new Translation2d[] { @@ -34,25 +35,24 @@ public class DriveConstants { public static final Rotation2d backLeftZeroRotation = new Rotation2d(0.0); public static final Rotation2d backRightZeroRotation = new Rotation2d(0.0); - // Device CAN IDs - public static final int pigeonCanId = 50; + // Device CAN IDs (buttercup) + public static final int pigeonCanId = 9; - public static final int frontLeftDriveCanId = 1; - public static final int backLeftDriveCanId = 3; - public static final int frontRightDriveCanId = 5; - public static final int backRightDriveCanId = 7; + public static final int frontLeftDriveCanId = 4; + public static final int backLeftDriveCanId = 6; + public static final int frontRightDriveCanId = 2; + public static final int backRightDriveCanId = 8; - public static final int frontLeftTurnCanId = 2; - public static final int backLeftTurnCanId = 4; - public static final int frontRightTurnCanId = 8; - public static final int backRightTurnCanId = 6; + public static final int frontLeftTurnCanId = 3; + public static final int backLeftTurnCanId = 5; + public static final int frontRightTurnCanId = 1; + public static final int backRightTurnCanId = 7; - // Drive motor configuration - public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.5); - public static final double driveMotorReduction = - (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth - public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); + // Drive motor configuration (buttercup: NEO on SparkMax, 6.75:1, 4 in wheel) + public static final int driveMotorCurrentLimit = 40; + public static final double wheelRadiusMeters = Units.inchesToMeters(2.0); + public static final double driveMotorReduction = 6.75; + public static final DCMotor driveGearbox = DCMotor.getNEO(1); // Drive encoder configuration public static final double driveEncoderPositionFactor = @@ -60,8 +60,9 @@ public class DriveConstants { public static final double driveEncoderVelocityFactor = (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec - // Drive PID configuration - public static final double driveKp = 0.0; + // Drive PID configuration (driveKp from YAGSL pidfproperties; note YAGSL's native-unit gain is + // not directly equivalent to this rad/s voltage loop, so verify/tune on the robot) + public static final double driveKp = 0.0020645; public static final double driveKd = 0.0; public static final double driveKs = 0.0; public static final double driveKv = 0.1; @@ -70,11 +71,11 @@ public class DriveConstants { public static final double driveSimKs = 0.0; public static final double driveSimKv = 0.0789; - // Turn motor configuration + // Turn motor configuration (buttercup: NEO on SparkMax, 25:1, Canandmag via Spark abs encoder) public static final boolean turnInverted = false; public static final int turnMotorCurrentLimit = 20; - public static final double turnMotorReduction = 9424.0 / 203.0; - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); + public static final double turnMotorReduction = 25.0; + public static final DCMotor turnGearbox = DCMotor.getNEO(1); // Turn encoder configuration public static final boolean turnEncoderInverted = true; @@ -90,9 +91,9 @@ public class DriveConstants { public static final double turnPIDMaxInput = 2 * Math.PI; // Radians // PathPlanner configuration - public static final double robotMassKg = 74.088; + public static final double robotMassKg = 75.0; public static final double robotMOI = 6.883; - public static final double wheelCOF = 1.2; + public static final double wheelCOF = 1.19; public static final RobotConfig ppConfig = new RobotConfig( robotMassKg, diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/GyroIOPigeon2.java b/src/main/java/org/frc5010/common/drive/swerve/akit/GyroIOPigeon2.java index fd311bd8..1ade6228 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/GyroIOPigeon2.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/GyroIOPigeon2.java @@ -36,8 +36,9 @@ public GyroIOPigeon2(AkitSwerveConfig config) { yaw.setUpdateFrequency(config.ODOMETRY_FREQUENCY); yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); - yawTimestampQueue = TalonFXOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = TalonFXOdometryThread.getInstance().registerSignal(yaw.clone()); + yawTimestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = + SparkOdometryThread.getInstance().registerSignal(() -> yaw.clone().getValueAsDouble()); } @Override 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..c909bb75 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 @@ -104,6 +104,18 @@ public void runSteerCharacterization(double output) { io.setTurnOpenLoop(output); } + /** + * Points the module's azimuth at the given absolute angle with the drive motor stopped, without + * the nearest-equivalent optimization used by {@link #runSetpoint}. Used by the azimuth step test + * so the module physically rotates to the commanded angle (not an equivalent 180 deg flip). + * + * @param angle the absolute steer angle to hold + */ + public void pointAt(Rotation2d angle) { + io.setDriveOpenLoop(0.0); + io.setTurnPosition(angle); + } + /** Disables all outputs to motors. */ public void stop() { io.setDriveOpenLoop(0.0); 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..8970e42f 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 @@ -24,6 +24,9 @@ class ModuleIOInputs { public boolean turnEncoderConnected = false; public Rotation2d turnPosition = Rotation2d.kZero; public Rotation2d turnAbsolutePosition = Rotation2d.kZero; + /** Absolute encoder reading before the configured zero offset is applied (diagnostics). */ + public Rotation2d turnRawAbsolutePosition = Rotation2d.kZero; + public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSpark.java b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSpark.java index 8e6f652e..0571775b 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSpark.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSpark.java @@ -7,32 +7,9 @@ package org.frc5010.common.drive.swerve.akit; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.backLeftDriveCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.backLeftTurnCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.backLeftZeroRotation; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.backRightDriveCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.backRightTurnCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.backRightZeroRotation; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveEncoderPositionFactor; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveEncoderVelocityFactor; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveKd; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveKp; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveKs; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveKv; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.driveMotorCurrentLimit; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.frontLeftDriveCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.frontLeftTurnCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.frontLeftZeroRotation; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.frontRightDriveCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.frontRightTurnCanId; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.frontRightZeroRotation; import static org.frc5010.common.drive.swerve.akit.DriveConstants.odometryFrequency; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnEncoderInverted; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnEncoderPositionFactor; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnEncoderVelocityFactor; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnInverted; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnKd; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnKp; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnMotorCurrentLimit; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnPIDMaxInput; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnPIDMinInput; @@ -40,6 +17,9 @@ import static org.frc5010.common.drive.swerve.akit.util.SparkUtil.sparkStickyFault; import static org.frc5010.common.drive.swerve.akit.util.SparkUtil.tryUntilOk; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; @@ -50,25 +30,36 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import java.util.Queue; import java.util.function.DoubleSupplier; +import org.frc5010.common.drive.swerve.AkitSwerveConfig; /** - * Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller, - * and duty cycle absolute encoder. + * Module IO implementation for a NEO (Spark Max) drive motor, NEO (Spark Max) turn motor, and an + * absolute encoder (e.g. Canandmag) read through the turn Spark's absolute-encoder port. + * + *

Everything per-robot is read from the deploy JSON (via {@link AkitSwerveConfig} / {@link + * SwerveModuleConstants}): drive/steer CAN IDs, drive gear ratio, drive current limit, the + * motor/encoder inversions, the absolute encoder zero offset, and the drive/steer closed-loop gains + * ({@code driveMotorControl}/{@code steerMotorControl} in akit units). Only universal akit + * constants (encoder conversion factors, the steer wrap range, steer current limit, odometry + * frequency) come from {@link DriveConstants}. */ public class ModuleIOSpark implements ModuleIO { + /** Absolute encoder zero offset, from the JSON {@code absoluteOffset}. */ private final Rotation2d zeroRotation; + // Drive feedforward gains (from the JSON), used in setDriveVelocity. + private final double driveKs; + private final double driveKv; + // Hardware objects private final SparkBase driveSpark; private final SparkBase turnSpark; @@ -88,45 +79,31 @@ public class ModuleIOSpark implements ModuleIO { private final Debouncer driveConnectedDebounce = new Debouncer(0.5); private final Debouncer turnConnectedDebounce = new Debouncer(0.5); - public ModuleIOSpark(int module) { - zeroRotation = - switch (module) { - case 0 -> frontLeftZeroRotation; - case 1 -> frontRightZeroRotation; - case 2 -> backLeftZeroRotation; - case 3 -> backRightZeroRotation; - default -> new Rotation2d(); - }; - driveSpark = - new SparkFlex( - switch (module) { - case 0 -> frontLeftDriveCanId; - case 1 -> frontRightDriveCanId; - case 2 -> backLeftDriveCanId; - case 3 -> backRightDriveCanId; - default -> 0; - }, - MotorType.kBrushless); - turnSpark = - new SparkMax( - switch (module) { - case 0 -> frontLeftTurnCanId; - case 1 -> frontRightTurnCanId; - case 2 -> backLeftTurnCanId; - case 3 -> backRightTurnCanId; - default -> 0; - }, - MotorType.kBrushless); + public ModuleIOSpark( + AkitSwerveConfig config, + SwerveModuleConstants + constants) { + zeroRotation = Rotation2d.fromRotations(constants.EncoderOffset); + driveKs = constants.DriveMotorGains.kS; + driveKv = constants.DriveMotorGains.kV; + + // Rotor rotations -> wheel radians, using the JSON drive gear ratio. + double driveEncoderPositionFactor = 2 * Math.PI / constants.DriveMotorGearRatio; + double driveEncoderVelocityFactor = (2 * Math.PI) / 60.0 / constants.DriveMotorGearRatio; + + driveSpark = new SparkMax(constants.DriveMotorId, MotorType.kBrushless); + turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); driveEncoder = driveSpark.getEncoder(); turnEncoder = turnSpark.getAbsoluteEncoder(); driveController = driveSpark.getClosedLoopController(); turnController = turnSpark.getClosedLoopController(); // Configure drive motor - var driveConfig = new SparkFlexConfig(); + var driveConfig = new SparkMaxConfig(); driveConfig + .inverted(constants.DriveMotorInverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(driveMotorCurrentLimit) + .smartCurrentLimit((int) constants.SlipCurrent) .voltageCompensation(12.0); driveConfig .encoder @@ -134,7 +111,11 @@ public ModuleIOSpark(int module) { .velocityConversionFactor(driveEncoderVelocityFactor) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); - driveConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).p(driveKp).d(driveKd); + driveConfig + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(constants.DriveMotorGains.kP) + .d(constants.DriveMotorGains.kD); driveConfig .signals .primaryEncoderPositionAlwaysOn(true) @@ -155,13 +136,13 @@ public ModuleIOSpark(int module) { // Configure turn motor var turnConfig = new SparkMaxConfig(); turnConfig - .inverted(turnInverted) + .inverted(constants.SteerMotorInverted) .idleMode(IdleMode.kBrake) .smartCurrentLimit(turnMotorCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder - .inverted(turnEncoderInverted) + .inverted(constants.EncoderInverted) .positionConversionFactor(turnEncoderPositionFactor) .velocityConversionFactor(turnEncoderVelocityFactor) .averageDepth(2); @@ -170,8 +151,8 @@ public ModuleIOSpark(int module) { .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .p(turnKp) - .d(turnKd); + .p(constants.SteerMotorGains.kP) + .d(constants.SteerMotorGains.kD); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) @@ -214,7 +195,18 @@ public void updateInputs(ModuleIOInputs inputs) { ifOk( turnSpark, turnEncoder::getPosition, - (value) -> inputs.turnAbsolutePosition = new Rotation2d(value).minus(zeroRotation)); + (value) -> { + Rotation2d raw = new Rotation2d(value); + // Raw reading is logged for offset calibration: with the wheel pointed forward, the + // reported raw angle (deg) is the value to copy into the module's JSON absoluteOffset. + inputs.turnRawAbsolutePosition = raw; + inputs.turnAbsolutePosition = raw.minus(zeroRotation); + // The steering closed loop runs on the absolute encoder and there is no separate relative + // turn encoder, so turnPosition mirrors the absolute position. Without this, turnPosition + // stays at 0, which makes Module.getAngle()/cosineScale think the wheel is always at 0 - + // scaling drive speed by cos(targetAngle) and producing no drive for 90 deg (strafe). + inputs.turnPosition = inputs.turnAbsolutePosition; + }); ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); ifOk( turnSpark, diff --git a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSparkTalon.java b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSparkTalon.java index 32f914ea..e4f8c20f 100644 --- a/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSparkTalon.java +++ b/src/main/java/org/frc5010/common/drive/swerve/akit/ModuleIOSparkTalon.java @@ -8,12 +8,8 @@ package org.frc5010.common.drive.swerve.akit; import static org.frc5010.common.drive.swerve.akit.DriveConstants.odometryFrequency; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnEncoderInverted; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnEncoderPositionFactor; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnEncoderVelocityFactor; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnInverted; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnKd; -import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnKp; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnMotorCurrentLimit; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnPIDMaxInput; import static org.frc5010.common.drive.swerve.akit.DriveConstants.turnPIDMinInput; @@ -106,7 +102,7 @@ public ModuleIOSparkTalon( constants) { this.constants = constants; - zeroRotation = new Rotation2d(); + zeroRotation = Rotation2d.fromRotations(constants.EncoderOffset); driveTalon = new TalonFX(constants.DriveMotorId, config.getCANBus()); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); @@ -134,13 +130,13 @@ public ModuleIOSparkTalon( // Configure turn motor var turnConfig = new SparkMaxConfig(); turnConfig - .inverted(turnInverted) + .inverted(constants.SteerMotorInverted) .idleMode(IdleMode.kBrake) .smartCurrentLimit(turnMotorCurrentLimit) .voltageCompensation(12.0); turnConfig .absoluteEncoder - .inverted(turnEncoderInverted) + .inverted(constants.EncoderInverted) .positionConversionFactor(turnEncoderPositionFactor) .velocityConversionFactor(turnEncoderVelocityFactor) .averageDepth(2); @@ -149,8 +145,8 @@ public ModuleIOSparkTalon( .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) - .p(turnKp) - .d(turnKd); + .p(constants.SteerMotorGains.kP) + .d(constants.SteerMotorGains.kD); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) @@ -193,7 +189,15 @@ public void updateInputs(ModuleIOInputs inputs) { ifOk( turnSpark, turnEncoder::getPosition, - (value) -> inputs.turnAbsolutePosition = new Rotation2d(value).minus(zeroRotation)); + (value) -> { + Rotation2d raw = new Rotation2d(value); + inputs.turnRawAbsolutePosition = raw; + inputs.turnAbsolutePosition = raw.minus(zeroRotation); + // Steering closes the loop on the absolute encoder with no separate relative turn + // encoder, so turnPosition mirrors the absolute position. Without this, turnPosition + // stays at 0, breaking Module.getAngle()/cosineScale (no drive at 90 deg) and odometry. + inputs.turnPosition = inputs.turnAbsolutePosition; + }); ifOk(turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value); ifOk( turnSpark, diff --git a/src/main/java/org/frc5010/common/subsystems/LEDStrip.java b/src/main/java/org/frc5010/common/subsystems/LEDStrip.java index 1fce3083..71bdbf40 100644 --- a/src/main/java/org/frc5010/common/subsystems/LEDStrip.java +++ b/src/main/java/org/frc5010/common/subsystems/LEDStrip.java @@ -227,55 +227,56 @@ public static void setSegmentActive(String name, boolean active) { } /** - * Adds a pattern to the LED strip with the given name, starting at the given index and spanning - * the given length. The pattern is active by default. If the name already corresponds to a - * pattern, this method does nothing. + * Adds a pattern to the LED strip with the given name, spanning the LEDs from the given start + * index to the given end index (inclusive). The pattern is active by default. If the name already + * corresponds to a pattern, this method does nothing. * * @param name The name of the pattern to add * @param pattern The pattern to add * @param startIndex The starting index of the pattern - * @param length The length of the pattern + * @param endIndex The ending index of the pattern (inclusive) */ - public static void addSegment(String name, LEDPattern pattern, int startIndex, int length) { + public static void addSegment(String name, LEDPattern pattern, int startIndex, int endIndex) { if (null == instance) { return; } - AddressableLEDBufferView view = instance.m_buffer.createView(startIndex, length); + AddressableLEDBufferView view = instance.m_buffer.createView(startIndex, endIndex); segments.put(name, Segment.create().setView(view).setPattern(pattern).setActive(true)); } /** - * Adds a pattern to the LED strip with the given name, starting at the given index and spanning - * the given length. If the name already corresponds to a pattern, this method does nothing. + * Adds a pattern to the LED strip with the given name, spanning the LEDs from the given start + * index to the given end index (inclusive). If the name already corresponds to a pattern, this + * method does nothing. * * @param name The name of the pattern to add * @param startIndex The starting index of the pattern - * @param length The length of the pattern + * @param endIndex The ending index of the pattern (inclusive) */ - public static void addSegment(String name, int startIndex, int length) { + public static void addSegment(String name, int startIndex, int endIndex) { if (null == instance) { return; } - AddressableLEDBufferView view = instance.m_buffer.createView(startIndex, length); + AddressableLEDBufferView view = instance.m_buffer.createView(startIndex, endIndex); segments.put(name, Segment.create().setView(view).setPattern(LEDPattern.kOff).setActive(false)); } /** - * Adds a pattern to the LED strip with the given name, starting at the given index and spanning - * the given length. The pattern is active by default. If the name already corresponds to a - * pattern, this method does nothing. + * Adds a pattern to the LED strip with the given name, spanning the LEDs from the given start + * index to the given end index (inclusive). The pattern is active by default. If the name already + * corresponds to a pattern, this method does nothing. * * @param name The name of the pattern to add * @param startIndex The starting index of the pattern - * @param length The length of the pattern + * @param endIndex The ending index of the pattern (inclusive) * @param order The order in which the pattern will be displayed. Lower order patterns will be * displayed before higher order patterns. */ - public static void addSegment(String name, int startIndex, int length, int order) { + public static void addSegment(String name, int startIndex, int endIndex, int order) { if (null == instance) { return; } - AddressableLEDBufferView view = instance.m_buffer.createView(startIndex, length); + AddressableLEDBufferView view = instance.m_buffer.createView(startIndex, endIndex); segments.put( name, Segment.create() @@ -351,6 +352,26 @@ public static LEDPattern getSolidPattern(Color color) { return LEDPattern.solid(color); } + /** + * Returns a scrolling "laser" pattern: a narrow bright band of the given color that sweeps along + * the segment over an unlit (black) background. + * + * @param color the color of the laser band + * @param percentWidth the width of the laser band, as a fraction (0.0-1.0) of the segment's + * length + * @param percentScrollingSpeed the speed at which the band sweeps, as a percentage of the + * segment's length per second + * @return the new scrolling laser pattern + */ + public static LEDPattern getLaserPattern( + Color color, double percentWidth, double percentScrollingSpeed) { + Map maskSteps = Map.of(0.0, Color.kWhite, percentWidth, Color.kBlack); + LEDPattern mask = + LEDPattern.steps(maskSteps) + .scrollAtRelativeSpeed(Percent.per(Second).of(percentScrollingSpeed)); + return LEDPattern.solid(color).mask(mask); + } + /** * Returns a new LEDPattern that masks the given base pattern with a band of given visibility and * scrolling speed. The mask has a band of white (visible) that is percentVisible of the pattern's @@ -395,7 +416,7 @@ public static LEDPattern getBand( Color.kBlack, bandCenter - (percentWidth / 2), Color.kWhite, - bandCenter - (percentWidth / 2), + bandCenter + (percentWidth / 2), Color.kBlack); LEDPattern mask = LEDPattern.steps(maskSteps) diff --git a/src/main/java/org/frc5010/lobbinloco/LobbinLoco.json b/src/main/java/org/frc5010/lobbinloco/LobbinLoco.json new file mode 100644 index 00000000..477386e3 --- /dev/null +++ b/src/main/java/org/frc5010/lobbinloco/LobbinLoco.json @@ -0,0 +1,116 @@ +{ + "field": { + "width": 6.095996708161778, + "length": 15.239991770404444 + }, + "tags": [ + { + "ID": 17, + "pose": { + "translation": { + "x": 8.229595556018399, + "y": 4.876797366529422, + "z": 1.0382244393588027 + }, + "rotation": { + "quaternion": { + "X": 0, + "Y": 0, + "Z": 1, + "W": 6.123233995736766e-17 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 8.229595556018399, + "y": 1.2191993416323554, + "z": 1.0382244393588027 + }, + "rotation": { + "quaternion": { + "X": 0, + "Y": 0, + "Z": 1, + "W": 6.123233995736766e-17 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 11.887193580915467, + "y": 2.489198655832726, + "z": 1.0382244393588027 + }, + "rotation": { + "quaternion": { + "X": 0, + "Y": 0, + "Z": 1, + "W": 6.123233995736766e-17 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 11.887193580915467, + "y": 3.505198107193022, + "z": 1.0382244393588027 + }, + "rotation": { + "quaternion": { + "X": 0, + "Y": 0, + "Z": 1, + "W": 6.123233995736766e-17 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 14.325592264180177, + "y": 5.28319714707354, + "z": 1.0382244393588027 + }, + "rotation": { + "quaternion": { + "X": 0, + "Y": 0, + "Z": 1, + "W": 6.123233995736766e-17 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 14.325592264180177, + "y": 0.7111996159522074, + "z": 1.0382244393588027 + }, + "rotation": { + "quaternion": { + "X": 0, + "Y": 0, + "Z": 1, + "W": 6.123233995736766e-17 + } + } + } + } + ] +} diff --git a/src/main/resources/lobbinloco/userAssets/Field3d_LobbinLoco/LobbinLoco.glb b/src/main/resources/lobbinloco/userAssets/Field3d_LobbinLoco/LobbinLoco.glb new file mode 100644 index 00000000..d99a1db5 Binary files /dev/null and b/src/main/resources/lobbinloco/userAssets/Field3d_LobbinLoco/LobbinLoco.glb differ diff --git a/src/test/java/org/frc5010/common/arch/GenericRobotTest.java b/src/test/java/org/frc5010/common/arch/GenericRobotTest.java index 96e0e771..a06ad3d3 100644 --- a/src/test/java/org/frc5010/common/arch/GenericRobotTest.java +++ b/src/test/java/org/frc5010/common/arch/GenericRobotTest.java @@ -20,6 +20,7 @@ public void testCreateDefaultGenericRobot() { TestRobot robot = new TestRobot(); assertNotNull(robot); assertNotNull(robot.getMechVisual()); + // assertEquals(Alliance.Blue, TestRobot.getAlliance()); assertNotNull(robot.getController("driver")); assertNotNull(robot.getController("operator")); } diff --git a/tools/swerve_log_analysis.py b/tools/swerve_log_analysis.py new file mode 100644 index 00000000..1774d9da --- /dev/null +++ b/tools/swerve_log_analysis.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 +"""Swerve azimuth diagnostics from an AdvantageKit WPILOG. + +Reads a ``.wpilog`` (no external dependencies) and summarizes the +``Swerve/Diag/*`` signals added in GenericSwerveDrivetrain/YAGSLSwerveDrivetrain, +so a wheel that fails to reach its commanded rotation can be diagnosed offline. + +Usage: + python3 tools/swerve_log_analysis.py [--error-deg 5] + +For each per-module signal it reports min/max/range; it also lists the moments +where a module is flagged ``misaligned`` (settled but off target) and prints the +concurrent signals (absolute-vs-relative encoder gap, absolute-encoder read +issue, steer applied output / voltage / velocity) so the failure can be mapped +to a cause: + * abs-vs-rel gap grows -> relative/absolute encoder desync + * absoluteEncoderReadIssue true -> intermittent CAN read failure + * steer output railed, vel ~0 -> saturation / mechanical stall + * small steady error, ~0 output -> weak angle PID + +The signals only exist in logs from the instrumented build (the commit that +added ``Swerve/Diag/*``). For older logs, fall back to YAGSL's ``swerve/`` +desired/measured states in AdvantageScope. +""" + +import bisect +import struct +import sys + +DIAG = "/RealOutputs/Swerve/Diag/" + + +def _read_str(buf, off): + n = struct.unpack_from("(name,type) and name->[(ts_us, value), ...].""" + buf = open(path, "rb").read() + if buf[:6] != b"WPILOG": + raise ValueError("not a WPILOG file") + extra_len = struct.unpack_from("> 2) & 0x3) + 1 + ts_len = ((head >> 4) & 0x7) + 1 + if off + id_len + size_len + ts_len > n: + break + eid = int.from_bytes(buf[off : off + id_len], "little") + off += id_len + psize = int.from_bytes(buf[off : off + size_len], "little") + off += size_len + ts = int.from_bytes(buf[off : off + ts_len], "little") + off += ts_len + payload = buf[off : off + psize] + off += psize + if eid == 0: # control record + if not payload: + continue + if payload[0] == 0: # Start + start_id = struct.unpack_from("= 0 else series[0][1] + + +def main(): + if len(sys.argv) < 2: + print(__doc__) + sys.exit(1) + path = sys.argv[1] + error_deg = 5.0 + if "--error-deg" in sys.argv: + error_deg = float(sys.argv[sys.argv.index("--error-deg") + 1]) + + _, data = parse(path) + all_ts = [t for v in data.values() for t, _ in v] + if not all_ts: + print("No records decoded.") + return + t0, t1 = min(all_ts), max(all_ts) + print("file: %s" % path) + print("duration: %.1f s entries: %d" % ((t1 - t0) / 1e6, len(data))) + + enabled = data.get("/DriverStation/Enabled", []) + if enabled: + frac = sum(1 for _, x in enabled if x) / len(enabled) + print("enabled fraction: %.2f (%d transitions)" % (frac, len(enabled))) + if frac == 0: + print("WARNING: robot never enabled in this log - no driving data.") + + diag = {k: v for k, v in data.items() if k.startswith(DIAG)} + if not diag: + print( + "\nNo Swerve/Diag/* signals - log predates the instrumented build.\n" + "Use YAGSL's swerve/ desired vs measured states in AdvantageScope instead." + ) + return + + print("\n--- per-signal ranges ---") + for name in sorted(diag): + short = name[len(DIAG):] + vals = [x for _, x in diag[name] if x is not None] + if not vals: + print("%-26s (no data)" % short) + continue + if isinstance(vals[0], list): + cols = max(len(a) for a in vals) + spans = [] + for c in range(cols): + col = [a[c] for a in vals if len(a) > c] + spans.append("[% .1f,% .1f]" % (min(col), max(col))) + print("%-26s %s" % (short, " ".join(spans))) + elif isinstance(vals[0], bool): + print("%-26s true=%d / %d samples" % (short, sum(vals), len(vals))) + else: + print("%-26s min=% .3f max=% .3f" % (short, min(vals), max(vals))) + + mis = data.get(DIAG + "misaligned", []) + err = data.get(DIAG + "errorDeg", []) + absrel = data.get(DIAG + "absMinusRelDeg", []) + readissue = data.get(DIAG + "absoluteEncoderReadIssue", []) + out = data.get(DIAG + "steerAppliedOutput", []) + vel = data.get(DIAG + "steerVelocityDps", []) + + print("\n--- misalignment events (settled but off target) ---") + events = 0 + for ts, flags in mis: + if not flags or not any(flags): + continue + events += 1 + rel = (ts - t0) / 1e6 + bad = [i for i, f in enumerate(flags) if f] + e = value_at(err, ts) or [] + ar = value_at(absrel, ts) or [] + ri = value_at(readissue, ts) or [] + ao = value_at(out, ts) or [] + sv = value_at(vel, ts) or [] + + def g(arr, i): + return arr[i] if i < len(arr) else float("nan") + + print("t=%7.2fs modules %s" % (rel, bad)) + for i in bad: + print( + " mod%d: err=% 7.1f absMinusRel=% 7.1f readIssue=%s " + "out=% .2f vel=% .1f" + % (i, g(e, i), g(ar, i), bool(g(ri, i)), g(ao, i), g(sv, i)) + ) + if events >= 40: + print(" ... (more events truncated)") + break + if events == 0: + print("none flagged (no module exceeded %.0f deg while settled)" % error_deg) + + +if __name__ == "__main__": + main() diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 91b4e342..469ae542 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.1", + "version": "26.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.1" + "version": "26.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.1", + "version": "26.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 1d80ce77..e7a0655f 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.3", + "version": "2026.0.5", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.3" + "version": "2026.0.5" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.3", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.3", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.3", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.3", + "version": "2026.0.5", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.3", + "version": "2026.0.5", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.3", + "version": "2026.0.5", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.3", + "version": "2026.0.5", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/ReduxLib-2026.1.1.json b/vendordeps/ReduxLib-2026.1.2.json similarity index 89% rename from vendordeps/ReduxLib-2026.1.1.json rename to vendordeps/ReduxLib-2026.1.2.json index 9081f8b2..431fe17e 100644 --- a/vendordeps/ReduxLib-2026.1.1.json +++ b/vendordeps/ReduxLib-2026.1.2.json @@ -1,7 +1,7 @@ { - "fileName": "ReduxLib-2026.1.1.json", + "fileName": "ReduxLib-2026.1.2.json", "name": "ReduxLib", - "version": "2026.1.1", + "version": "2026.1.2", "frcYear": "2026", "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2026.1.1" + "version": "2026.1.2" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-fifo", - "version": "2026.1.1", + "version": "2026.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2026.1.1", + "version": "2026.1.2", "libName": "ReduxLib", "headerClassifier": "headers", "sourcesClassifier": "sources", @@ -52,7 +52,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-fifo", - "version": "2026.1.1", + "version": "2026.1.2", "libName": "reduxfifo", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/ThriftyLib-2026.1.0.json b/vendordeps/ThriftyLib-2026.json similarity index 81% rename from vendordeps/ThriftyLib-2026.1.0.json rename to vendordeps/ThriftyLib-2026.json index ec3338c5..69917f06 100644 --- a/vendordeps/ThriftyLib-2026.1.0.json +++ b/vendordeps/ThriftyLib-2026.json @@ -1,7 +1,7 @@ { - "fileName": "ThriftyLib-2026.1.0.json", + "fileName": "ThriftyLib-2026.json", "name": "ThriftyLib", - "version": "2026.1.0", + "version": "2026.1.2", "frcYear": "2026", "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.thethriftybot.frc", "artifactId": "ThriftyLib-java", - "version": "2026.1.0" + "version": "2026.1.2" } ], "jniDependencies": [], diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 62661dd2..4280265f 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.3.1", + "version": "v2026.3.4", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.3.1", + "version": "v2026.3.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.3.1", + "version": "v2026.3.4", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.3.1", + "version": "v2026.3.4", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.3.1" + "version": "v2026.3.4" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.3.1" + "version": "v2026.3.4" } ] } diff --git a/vendordeps/yams.json b/vendordeps/yams.json index 597f32f1..034dc6a8 100644 --- a/vendordeps/yams.json +++ b/vendordeps/yams.json @@ -1,7 +1,7 @@ { "fileName": "yams.json", "name": "Yet Another Mechanism System", - "version": "2026.2.24", + "version": "2026.2.23", "frcYear": "2026", "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "yams", "artifactId": "YAMS-java", - "version": "2026.2.24" + "version": "2026.2.23" } ], "cppDependencies": [],