diff --git a/AdvantageScope RBSI Standard.json b/AdvantageScope RBSI Standard.json new file mode 100644 index 00000000..67dc05a9 --- /dev/null +++ b/AdvantageScope RBSI Standard.json @@ -0,0 +1,541 @@ +{ + "hubs": [ + { + "x": 71, + "y": 33, + "width": 1315, + "height": 776, + "state": { + "sidebar": { + "width": 300, + "expanded": [ + "/RealOutputs", + "/RealOutputs/LoopSpike/Drive", + "/RealOutputs/Drive/OdomThread", + "/RealOutputs/CAN/DriveTrain", + "/RealOutputs/CAN/Module0", + "/RealOutputs/LoopSpike", + "/AdvantageKit", + "/RealOutputs/Loop/Drive", + "/RealOutputs/Loop/Mech", + "/RealOutputs/Odometry/Yaw", + "/RealOutputs/Vision/Camera0", + "/RealOutputs/Vision/Camera0/TagPoses", + "/RealOutputs/Vision/Summary/TagPoses", + "/RealOutputs/Vision/Camera1/RobotPosesAccepted", + "/RealOutputs/Vision/Camera0/RobotPosesAccepted", + "/RealOutputs/Power", + "/RealOutputs/Power/Subsystems", + "/NetworkInputs/SmartDashboard", + "/RealOutputs/Loop", + "/RealOutputs/Loop/Virtual" + ] + }, + "tabs": { + "selected": 7, + "tabs": [ + { + "type": 0, + "title": "", + "controller": null, + "controllerUUID": "27w43phb8fm0oyp20p0wxms1a35yrw0i", + "renderer": "", + "controlsHeight": 0 + }, + { + "type": 2, + "title": "2D Field - Odometry", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "bumpers": "" + } + } + ], + "field": "FRC:2026 Field", + "orientation": 0, + "size": "large" + }, + "controllerUUID": "ibthfkqod1n0vovjoxhcg2d9954wcpe4", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 2, + "title": "2D Field - Vision", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": false, + "options": { + "bumpers": "" + } + }, + { + "type": "arrow", + "logKey": "/RealOutputs/Vision/Summary/TagPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "position": "center" + } + }, + { + "type": "ghost", + "logKey": "/RealOutputs/Vision/Camera0/RobotPosesAccepted", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "/RealOutputs/Vision/Camera1/RobotPosesAccepted", + "logType": "Pose3d[]", + "visible": true, + "options": { + "color": "#ffff00" + } + } + ], + "field": "FRC:2026 Field", + "orientation": 0, + "size": "large" + }, + "controllerUUID": "u9myuirzag5qydjp9nozn8qxpfqofuuq", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 9, + "title": "Swerve", + "controller": { + "sources": [ + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/Measured", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#00ffff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "rotation", + "logKey": "/RealOutputs/Odometry/Robot/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + }, + { + "type": "chassisSpeeds", + "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + } + ], + "maxSpeed": 5, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "zo4ukgbiguacvfe9e2iui2ytosl7gutn", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Power", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/PowerDistribution/Voltage", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/PowerDistribution/TotalCurrent", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Power/Subsystems/DriveCurrent", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Power/Subsystems/SteerCurrent", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Power/Subsystems/FlywheelCurrent", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [ + { + "type": "stripes", + "logKey": "/RealOutputs/Power/BrownoutImminent", + "logType": "Boolean", + "visible": true, + "options": { + "color": "#af2437" + } + } + ], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "9vyygyem9we0y5917klbu25cz9hruszx", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "CAN Health", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/CAN//Utilization", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/DriveTrain/Utilization", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + } + ], + "rightSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/DriveTrain/RxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/DriveTrain/TxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN//RxErrorCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/CAN/DriveTrain/TxFullCount", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "f15s8kcvuqce63dlc7cf4yunt6c0372w", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Loop Time - Robot", + "controller": { + "leftSources": [], + "rightSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/LoggedRobot/FullCycleMS", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/LoggedRobot/GCTimeMS", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/LoggedRobot/LogPeriodicMS", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/LoggedRobot/UserCodeMS", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + } + ], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "yktsi99006yw1sq2h8mihr8kiy9h5xk9", + "renderer": null, + "controlsHeight": 200 + }, + { + "type": 1, + "title": "Loop Time - Subsystems", + "controller": { + "leftSources": [ + { + "type": "stepped", + "logKey": "/RealOutputs/Loop/Mech/Drive_ms", + "logType": "Number", + "visible": true, + "options": { + "color": "#2b66a2", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Loop/Mech/Flywheel_ms", + "logType": "Number", + "visible": true, + "options": { + "color": "#e5b31b", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Loop/Virtual/Accelerometer_ms", + "logType": "Number", + "visible": true, + "options": { + "color": "#af2437", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Loop/Virtual/RBSICANHealth_ms", + "logType": "Number", + "visible": true, + "options": { + "color": "#80588e", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Loop/Virtual/RBSIPowerMonitor_ms", + "logType": "Number", + "visible": true, + "options": { + "color": "#e48b32", + "size": "normal" + } + }, + { + "type": "stepped", + "logKey": "/RealOutputs/Loop/Virtual/Vision_ms", + "logType": "Number", + "visible": true, + "options": { + "color": "#c0b487", + "size": "normal" + } + } + ], + "rightSources": [], + "discreteSources": [], + "leftLockedRange": null, + "rightLockedRange": null, + "leftUnitConversion": { + "autoTarget": null, + "preset": null + }, + "rightUnitConversion": { + "autoTarget": null, + "preset": null + }, + "leftFilter": 0, + "rightFilter": 0 + }, + "controllerUUID": "no6ewy1a2lc06qvs7einn5i430wwub9n", + "renderer": null, + "controlsHeight": 195 + }, + { + "type": 3, + "title": "3D Field", + "controller": { + "sources": [ + { + "type": "robot", + "logKey": "/RealOutputs/Vision/Summary/TagPoses", + "logType": "Pose3d[]", + "visible": true, + "options": { + "model": "Crab Bot" + } + }, + { + "type": "robot", + "logKey": "/RealOutputs/Odometry/Robot", + "logType": "Pose2d", + "visible": true, + "options": { + "model": "2026 KitBot" + } + } + ], + "game": "FRC:2026 Field" + }, + "controllerUUID": "x1j1ypjwdchzmk115shjrh4ml6w4osq9", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 1.4695761589768238e-15, + 5.999999999999999, + -12 + ], + "cameraTarget": [ + 0, + 0.5, + 0 + ] + }, + "controlsHeight": 200 + } + ] + } + } + } + ], + "satellites": [], + "version": "26.0.0" +} diff --git a/build.gradle b/build.gradle index 4df38fab..8334e361 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2026.2.1" id "com.peterabeles.gversion" version "1.10.3" - id "com.diffplug.spotless" version "8.1.0" + id "com.diffplug.spotless" version "8.2.1" id "io.freefair.lombok" version "9.2.0" id "com.google.protobuf" version "0.9.6" } diff --git a/doc/INSTALL.md b/doc/INSTALL.md index c3c23727..ac19b1c4 100644 --- a/doc/INSTALL.md +++ b/doc/INSTALL.md @@ -13,7 +13,7 @@ following components on your laptop and devices. This includes all motors, CANivore, Pigeon 2.0, and all CANcoders! * Rev Hardware Client `2.0`, with the PDH and all SparkMax's, and other devices running firmware `26.1` or newer. -* Vivid Hosting Radio firmware `2.0` or newer is required for competition this +* Vivid Hosting Radio firmware `2.0.1` or newer is required for competition this year. * Photon Vision ([Orange Pi or other device](https://docs.photonvision.org/en/latest/docs/quick-start/quick-install.html)) **running `26.1` or newer** (make sure you are **not** acidentially running @@ -22,6 +22,8 @@ following components on your laptop and devices. It is highly recommmended to update all you devices, and label what can id's or ip adresses and firmware versions they are running. This helps your team, and the FRC field staff quickly identify issues. +If you are running a RoboRIO 1.0 (no sd card) you also neeed to disable the web server ([Instructions Here](https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/roborio-team-number-setter/index.html)) + -------- ### Getting Az-RBSI @@ -118,7 +120,10 @@ steps you need to complete: https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/1db713d75b08a4315c9273cebf5b5e6a130ed3f7/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java#L171-L175). Before removing them, both lines will be marked as errors in VSCode. -5. In `TunerConstants.java`, change `kSteerInertia` to `0.004` and +5. In `TunerConstants.java`, change `kSlipCurrent` to `60` amps. This will + keep your robot from tearing holes in the carpet at competition! + +6. In `TunerConstants.java`, change `kSteerInertia` to `0.004` and `kDriveInertia` to `0.025` to allow the AdvantageKit simulation code to operate as expected. diff --git a/doc/PV_Cameras.png b/doc/PV_Cameras.png new file mode 100644 index 00000000..b5293864 Binary files /dev/null and b/doc/PV_Cameras.png differ diff --git a/doc/PV_Network.png b/doc/PV_Network.png new file mode 100644 index 00000000..5db753c3 Binary files /dev/null and b/doc/PV_Network.png differ diff --git a/doc/RBSI-GSG.md b/doc/RBSI-GSG.md index da7774e3..b7f541d8 100644 --- a/doc/RBSI-GSG.md +++ b/doc/RBSI-GSG.md @@ -41,6 +41,10 @@ modifications to extant RBSI code will be done to files within the ### Tuning constants for optimal performance +**It cannot be overemphasized the importance of tuning your drivetrain for +smooth and consistent performance, battery longevity, and not tearing up the +field.** + 4. Over the course of your robot project, you will need to tune PID parameters for both your drivebase and any mechanisms you build to play the game. AdvantageKit includes detailed instructions for how to tune the various @@ -128,7 +132,22 @@ section of [each release](https://github.com/AZ-First/Az-RBSI/releases). See the [PhotonVision Wiring documentation ](https://docs.photonvision.org/en/latest/docs/quick-start/wiring.html) for - more details. + more details. DO NOT put the orange pi's (or any devices that cannnot loose power) on port 23 of the PDH. It is a mechanical switch, and if the robot is hit, it briefly will loose power. Mounting the case to the robot requires 4x #10-32 nylock nuts (placed in the hex-shaped mounts inside the case) and 4x #10-32 bolts. + + Order of addembly of the Orange Pi Double Case matters given tight clearances: + 1. Super-glue the nylock nuts into the hex mounting holes. + 2. Intall the fans and grates into the case side. + 3. Assemble the Pi's into the standoffs outside the box. + 4. Solder / mount the Voltage Regular solution of your choice. + 5. Connect the USB-C power cables to the Pi's. + 6. Connect the fan power to the 5V (red) and GND (black) pins in the Pi's. + 7. Install the Pi/standoff assembly into the case using screws at the bottom, + be careful of the tight clearance between the USB sockets and the case opening. + 8. Tie a knot in the incoing power line _to be placed inside the box + for strain relief_, and pass the incoming power line through the notch + in the lower case. + 9. Install the cover on the box using screws. + 10. Mount the case to your robot using the #10-32 screws. diff --git a/doc/RBSI-PoseBuffer.md b/doc/RBSI-PoseBuffer.md new file mode 100644 index 00000000..60823c4a --- /dev/null +++ b/doc/RBSI-PoseBuffer.md @@ -0,0 +1,150 @@ +# Az-RBSI Pose Buffering + +This page contains documentation for the Odometry and Vision Pose Buffering +system included in Az-RBSI. It is more for reference than instructing teams in +how to do something. + +-------- + +### Background for Need + +In previous versions of the Az-RBSI (or the base [AdvantageKit templates]( +https://docs.advantagekit.org/getting-started/template-projects)), there is a +temporal disconnect between the "now" state of the drivetrain odometry (wheel +encoders + gyro) and the "delayed" state of vision measurements (by exposure +time, pipeline processing, and network transport). Essentially, **the +different sensors on the robot do not all report data at the same time**. This +delay can be 30–120 ms -- which is huge when your robot can move a foot in that +time. Attempting to correct the "now" odometric pose with a "delayed" vision +estimate introduces systematic error than can cause jitter in the pose estimate +and incorrect downstream computed values. + + +### What is Pose Buffering + +A pose buffer lets you store a time history of your robot’s estimated pose so +that when a delayed vision measurement arrives, you can rewind the state +estimator to the exact timestamp the image was captured, inject the correction, +and then replay forward to the present. In essence, pose buffers enable **time +alignment between subsystems**. Since the Az-RBSI assumes input from multiple +cameras and combining that with the IMU yaw queues and high-frequency module +odometry, everything must agree on a common timebase. Introducing a pose +buffer allows us to query, "What did odometry think the robot pose was at time +`t`?" and compute the transform between two timestamps. This is the key to +making atency-aware fusion mathematically valid. + +Pose buffers dramatically improve **stability and predictability** as well. +They can prevent feedback oscillations caused by injecting corrections at +inconsistent times and enable smoothing, gating, and replay-based estimators. +These are incredibly important for accurate autonomous paths, reliable +auto-aim, and multi-camera fusion. + + +### Implementation as Virtual Subsystems + +The Az-RBSI pose buffer implementation is based on the principle that **Drive +owns the authoritative pose history** via a `poseBuffer` keyed by FPGA +timestamp, and we make sure that buffer is populated using the *same timebase* +as the estimator. Rather than updating the estimator only “once per loop,” +**all high-rate odometry samples** collected since the last loop are replayed +and inserted into the buffer. + +We have a series of three Virtual Subsystems that work together to compute the +estimated position of the robot each loop (20 ms), polled in this order: +* Imu +* DriveOdometry +* Vision + +The IMU is treated separately from the rest of the drive odometry because we +use its values in the Accelerometer virtual subsystem to compute the +accelerations the robot undergoes. Its `inputs` snapshot is refreshed before +odometry replay runs so that during odometry replay, we prefer using the IMU’s +**odometry yaw queue** when it exists and is aligned to the drivetrain odometry +timestamps. If now, we fall back to interpolating yaw from `yawBuffer` (or +"now" yaw if we have no queue). This allows for stable yaw-rate gating +(single-camera measurements discarded if the robot is spinning too quickly) +because `yawRateBuffer` is updated in the same timebase as the odometry replay. +When vision asks, "what is the max yaw rate over `[ts-lookback, ts]`," it is +querying a consistent history instead of a mix of current-time samples. + +The `DriveOdometry` virtual subsystem drains the PhoenixOdometryThread queues +to get a canonical timestamp array upon which is built the +`SwerveModulePosition` snapshots for each sample index. The YAW from the IMU +is computed for that same sample time, then the module positions and YAW are +added to the pose estimator using the `.updateWithTime()` function for each +timestamp in the queue. At the same time, we add the sample to the pose buffer +so that later consumers (vision alignment, gating, smoothing) can ask, "What +did the robot think at time `t`?" Practically, it runs early in the loop, +drains module odometry queues, performs the `updateWithTime(...)` replay, and +keeps the `poseBuffer`, `yawBuffer`, and `yawRateBuffer` coherent. + +Vision measurements get included *after* odometry has advanced and buffered +poses for the relevant timestamps. Vision reads all camera observations, +applies various gates, chooses one best observation per camera, then fuses them +by picking a fusion time `tF` (newest accepted timestamp), and **time-aligning +each camera estimate from its `ts` to `tF` using Drive’s pose buffer**. The +smoothed/fused result is then injected through the `addVisionMeasurement()` +consumer in `Drive` at the correct timestamp. The key is: we never try to +"correct the present" with delayed vision; we correct the past, and the +estimator/pose buffer machinery carries that correction forward coherently. + +To guarantee the right computation order, we implemented a simple +**priority/ordering mechanism for VirtualSubsystems** rather than relying on +construction order. Conceptually: `Imu` runs first (refresh sensor snapshot and +yaw queue), then `DriveOdometry` runs (drain odometry queues, replay estimator, +update buffers), then `Vision` runs (query pose history, fuse, inject +measurements), and finally anything downstream (targeting, coordinators, etc.). +With explicit ordering, vision always sees a pose buffer that is current +through the latest replayed timestamps, and its time alignment transform uses +consistent odometry states. + + +### Relationship Between Pose Buffering and Hardware Control Subsystems + +The `DriveOdometry` virtual subsystem exists to **isolate the heavy, +timing-sensitive replay logic** from the rest of `Drive` "control" behavior. +This separation allows `Drive`’s main subsystem to focus on setpoints/commands, +while `DriveOdometry` guarantees that by the time anything else runs, odometry +state and buffers are already up to date for the current cycle. + +The pose buffering system sits cleanly between **raw hardware sampling** and +**high-level control**, acting as a time-synchronized "memory layer" for the +robot’s physical motion. At the bottom, hardware devices are sampled at high +frequency with timestamps measurements in FPGA time and compensation for CAN +latency. Those timestamped samples are drained and replayed inside +`DriveOdometry`, which feeds the `SwerveDrivePoseEstimator` object. This means +the estimator is not tied to the 20 ms main loop -- it advances according to +the *actual sensor sample times*. The result is a pose estimate that reflects +real drivetrain physics at the rate the hardware can provide, not just the +scheduler tick rate. + +On the control side, everything -- heading controllers, auto-alignment, vision +fusion, targeting -- consumes pose data that is both temporally accurate and +historically queryable. Controllers still run at the 20 ms loop cycle, but +they operate on a pose that was built from high-rate, latency-compensated +measurements. When vision injects corrections, it does so at the correct +historical timestamp, and the estimator propagates that correction forward +consistently. The net effect is tighter autonomous path tracking, more stable +aiming, and reduced oscillation under aggressive maneuvers -- because control +decisions are based on a pose model that more faithfully represents the real +robot’s motion and sensor timing rather than a simplified "latest value only" +approximation. + + +### Behavior when the Robot is Disabled + +In normal operation (robot enabled), vision measurements are incorporated using standard Kalman fusion via addVisionMeasurement(). This is a probabilistic update: the estimator weighs the vision measurement against the predicted state based on covariance, producing a smooth, statistically optimal correction. Small errors are gently nudged toward vision; large discrepancies are handled proportionally according to the reported measurement uncertainty. This is the correct and intended behavior for a moving robot. + +When the robot is disabled, however, the estimator is no longer operating in a dynamic system. Wheel odometry is effectively static, process noise collapses, and repeated vision corrections can cause pathological estimator behavior (particularly in translation). Instead of performing another Kalman update in this regime, the system switches to a controlled pose blending strategy. Each accepted vision pose is blended toward the current estimate using a fixed interpolation factor (e.g., 10–20%), and the estimator is explicitly reset to that blended pose. This produces a gradual convergence toward the vision solution without allowing covariance collapse or runaway corrections. + +The result is a stable and intuitive pre-match behavior: while disabled, the robot will slowly “walk” its pose estimate toward the vision solution if needed, but it will not snap violently or diverge numerically. Once enabled, the system seamlessly returns to proper Kalman fusion, where vision and odometry interact in a fully dynamic and statistically grounded manner. + +### Design Rationale – Split Enabled/Disabled Vision Handling + +The core reason for separating enabled and disabled behavior is that the Kalman filter assumes a **dynamic system model**. When the robot is enabled, the drivetrain is actively moving and the estimator continuously predicts forward using wheel odometry and gyro inputs. Vision measurements then act as bounded corrections against that prediction. In this regime, Kalman fusion is mathematically appropriate: process noise and measurement noise are balanced, covariance evolves realistically, and corrections remain stable. + +When the robot is disabled, however, the system is no longer dynamic. Wheel distances stop changing, gyro rate is near zero, and process noise effectively collapses. If vision continues injecting measurements into the estimator with timestamps that are slightly offset from the estimator’s internal state, the filter can enter a degenerate regime. Because translational covariance may shrink aggressively while no true motion exists, even small inconsistencies between time-aligned vision and odometry can produce disproportionately large corrections. This is why translation can numerically “explode” while rotation often remains stable—rotation is typically better constrained by the gyro and wraps naturally, while translation depends more directly on integrated wheel deltas and covariance scaling. + +The disabled blending pattern avoids this pathological case by temporarily stepping outside strict Kalman fusion. Instead of applying repeated measurement updates against a near-zero process model, we treat vision as a slowly converging reference and explicitly blend the current estimate toward it. This maintains numerical stability, prevents covariance collapse artifacts, and still allows the pose to settle accurately before a match begins. + +Once the robot transitions back to enabled, the estimator resumes normal probabilistic fusion with a healthy process model. The split-mode approach therefore preserves mathematical correctness while the robot is moving, and guarantees numerical stability while it is stationary. diff --git a/doc/RBSI-Vision.md b/doc/RBSI-Vision.md new file mode 100644 index 00000000..18892855 --- /dev/null +++ b/doc/RBSI-Vision.md @@ -0,0 +1,80 @@ +# Az-RBSI Vision Integration + +This page includes detailed steps for integrating robot vision for your +2026 REBUILT robot. + +-------- + +### PhotonVision + +The preferred method for adding vision to your robot is with [PhotonVision]( +https://photonvision.org/). This community-developed open-source package +combines coprocessor-based camera control and analysis with a Java library +for consuming the processed targeting information in the robot code. + +#### Recommended Setup with Az-RBSI + +We recommend using Arducam [OV9281](https://www.amazon.com/dp/B096M5DKY6) +(black & white) and/or [OV9782](https://www.amazon.com/dp/B0CLXZ29F9) (color) +cameras for robot vision due to their Global Shutter, Low Distortion, and USB +connection. In addition to the lens delivered with the camera, supplementary +lenses may be purchased to vary the FOV available to the detector for various +robot applications, such as [Low-Distortion]( +https://www.amazon.com/dp/B07NW8VR71) or [General Purpose]( +https://www.amazon.com/dp/B096V2NP2T). + +For the coprocessor that controls the cameras and analyzes the images for +AprilTag and gamepiece detection, we recommend using one or two Orange Pi 5 +single-board computers -- although PhotonVision does support a number of +[different coprocessor options]( +https://docs.photonvision.org/en/latest/docs/quick-start/quick-install.html). +As decribed in the [Getting Started Guide](RBSI-GSG.md), we include a 3D print +for a case that can hold one or two of these computers. + +#### Setting up PhotonVision on the Coprocessor + +Download the appropriate [disk image]( +https://github.com/PhotonVision/photonvision/releases/tag/v2026.2.1) for your +coprocessor and burn it to an SD card using the [Raspberry Pi Imager]( +https://www.raspberrypi.com/software). Connect the powered-on coprocessor +to the Vivivid radio (port 2 or 3) via ethernet, or connect to a network switch connected to the radio via ethernet, and connect to the PhotonVision software +at the address ``http://photonvision.local:5800``. + +Before you connect the coprocessor to your robot, be sure to set your team +number, set the IP address to "Static" and give it the number ``10.TE.AM.11``, +where "TE.AM" is the approprate parsing of your team number into IP address, +as used by your robot radio and RoboRIO. If desired, you can also give your +coprocessor a hostname. + +![PhotonVision Network Settings](PV_Network.png) + +We suggest you give your first coprocessor the static IP address +``10.TE.AM.11``, and your second coprocessor (if desired) ``10.TE.AM.12``. +The static address allows for more stable operation, and the these particular +addresses do not conflict with other devices on your robot network. + +Plug in cameras (two or three per coprocessor) and navigate to the Camera +Configs page (see below). Activate the cameras. + +![PhotonVision Camera Configs](PV_Cameras.png) + +#### Configuring and Calibrating your Cameras + +This is the most important part! + +Instructions are in the [PhotonVision Documentation]( +https://docs.photonvision.org/en/latest/docs/calibration/calibration.html). + +You should consider calibrating your cameras early and often, including daily +during a competition to ensure that the cameras are reporting as accurate a +pose as possible for your odometry. Also, double-check your calibration by +using a measuring tape to compare the reported vision-derived distance from +each camera to one or more AprilTags with reality. + + +#### Using PhotonVision for Vision Simulation + +This is an advanced topic, and is therefore in the Restricted Section. (More +information about vision simulation to come in a future release.) + +![Restricted Section](restricted_section.jpg) diff --git a/doc/restricted_section.jpg b/doc/restricted_section.jpg new file mode 100644 index 00000000..4d5bc210 Binary files /dev/null and b/doc/restricted_section.jpg differ diff --git a/src/main/deploy/apriltags/2024-crescendo.json b/src/main/deploy/apriltags/2024-crescendo.json new file mode 100644 index 00000000..8cb837a5 --- /dev/null +++ b/src/main/deploy/apriltags/2024-crescendo.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/apriltags/2025-reefscape-andymark.json b/src/main/deploy/apriltags/2025-reefscape-andymark.json new file mode 100644 index 00000000..60b60e5e --- /dev/null +++ b/src/main/deploy/apriltags/2025-reefscape-andymark.json @@ -0,0 +1,404 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.687292, + "y": 0.628142, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.687292, + "y": 7.414259999999999, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.45399049973954675, + "X": -0.0, + "Y": 0.0, + "Z": 0.8910065241883679 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.49096, + "y": 8.031733999999998, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 6.132575999999999, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 1.9098259999999998, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 13.890498, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 13.474446, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.643358, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.227305999999999, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.643358, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 0.8613139999999999, + "y": 0.628142, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.8910065241883679, + "X": 0.0, + "Y": 0.0, + "Z": 0.45399049973954675 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.8613139999999999, + "y": 7.414259999999999, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.8910065241883678, + "X": -0.0, + "Y": 0.0, + "Z": 0.45399049973954686 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 8.272272, + "y": 6.132575999999999, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 8.272272, + "y": 1.9098259999999998, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 6.057646, + "y": 0.010667999999999999, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 3.6576, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 5.321046, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.042 + } +} diff --git a/src/main/deploy/apriltags/2025-official.json b/src/main/deploy/apriltags/2025-reefscape-welded.json similarity index 100% rename from src/main/deploy/apriltags/2025-official.json rename to src/main/deploy/apriltags/2025-reefscape-welded.json diff --git a/src/main/deploy/apriltags/2026-none.json b/src/main/deploy/apriltags/2026-none.json deleted file mode 100644 index 4763fdac..00000000 --- a/src/main/deploy/apriltags/2026-none.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "tags": [], - "field": { - "length": 16.4592, - "width": 8.2296 - } -} diff --git a/src/main/deploy/apriltags/2026-rebuilt-andymark.json b/src/main/deploy/apriltags/2026-rebuilt-andymark.json new file mode 100644 index 00000000..ecc03907 --- /dev/null +++ b/src/main/deploy/apriltags/2026-rebuilt-andymark.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/src/main/deploy/apriltags/2026-official.json b/src/main/deploy/apriltags/2026-rebuilt-welded.json similarity index 100% rename from src/main/deploy/apriltags/2026-official.json rename to src/main/deploy/apriltags/2026-rebuilt-welded.json diff --git a/src/main/deploy/apriltags/none-andymark.json b/src/main/deploy/apriltags/none-andymark.json new file mode 100644 index 00000000..318fdac6 --- /dev/null +++ b/src/main/deploy/apriltags/none-andymark.json @@ -0,0 +1,7 @@ +{ + "tags": [], + "field": { + "length": 16.518, + "width": 8.043 + } +} diff --git a/src/main/deploy/apriltags/none-welded.json b/src/main/deploy/apriltags/none-welded.json new file mode 100644 index 00000000..63337983 --- /dev/null +++ b/src/main/deploy/apriltags/none-welded.json @@ -0,0 +1,7 @@ +{ + "tags": [], + "field": { + "length": 16.541, + "width": 8.069 + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d30eaed..1f552a66 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,6 +30,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.RobotBase; @@ -46,6 +47,7 @@ import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; import java.util.Set; +import org.littletonrobotics.junction.Logger; import org.photonvision.simulation.SimCameraProperties; import swervelib.math.Matter; @@ -72,9 +74,9 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED + private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.MANUAL; // MANUAL, PATHPLANNER, CHOREO - private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE + private static VisionType visionType = VisionType.PHOTON; // PHOTON, LIMELIGHT, NONE /** Enumerate the robot types (name your robots here) */ public static enum RobotType { @@ -106,6 +108,8 @@ public static void disableHAL() { public static final boolean tuningMode = false; + public static final double G_TO_MPS2 = 9.80665; // Gravitational acceleration in m/s/s + /************************************************************************* */ /** Physical Constants for Robot Operation ******************************* */ public static final class RobotConstants { @@ -164,7 +168,7 @@ public static final class PowerConstants { /************************************************************************* */ /** List of Robot CAN Busses ********************************************* */ public static final class CANBuses { - public static final String RIO = ""; + public static final String RIO = "rio"; public static final String DRIVE = "DriveTrain"; public static final String[] ALL = {RIO, DRIVE}; @@ -180,30 +184,30 @@ public static class RobotDevices { // Front Left public static final RobotDeviceId FL_DRIVE = - new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 16); + new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18); public static final RobotDeviceId FL_ROTATION = - new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 17); + new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19); public static final RobotDeviceId FL_CANCODER = new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null); // Front Right public static final RobotDeviceId FR_DRIVE = - new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 13); + new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17); public static final RobotDeviceId FR_ROTATION = - new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 12); + new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16); public static final RobotDeviceId FR_CANCODER = new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null); // Back Left public static final RobotDeviceId BL_DRIVE = - new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 2); + new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1); public static final RobotDeviceId BL_ROTATION = - new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 3); + new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0); public static final RobotDeviceId BL_CANCODER = new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null); // Back Right public static final RobotDeviceId BR_DRIVE = - new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 7); + new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2); public static final RobotDeviceId BR_ROTATION = - new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 6); + new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3); public static final RobotDeviceId BR_CANCODER = new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null); // Pigeon @@ -324,9 +328,26 @@ public static final class DrivebaseConstants { SwerveConstants.kDriveGearRatio / DCMotor.getKrakenX60Foc(1).KtNMPerAmp; public static final double kSteerP = 400.0; public static final double kSteerD = 20.0; + public static final double kSteerS = 2.0; - // Odometry-related constants + // Odometry-related constants ================================== public static final double kHistorySize = 1.5; // seconds + // How aggressively to pull pose toward vision while DISABLED. + // 0.10 = gentle, 0.25 = fairly quick, 1.0 = full snap. + public static final double kDisabledVisionBlendAlpha = 0.15; + // Optional: ignore obviously insane measurements while disabled. + public static final double kDisabledVisionMaxJumpM = 2.0; // meters + public static final double kDisabledVisionMaxJumpRad = Units.degreesToRadians(20.0); + public static final double kDisabledVisionStale = 0.75; // seconds + + // Coast window config + public static final double kDisabledCoastSeconds = 5.0; + + // "Stationary" detection config (tune) + public static final double kStationaryMaxWheelDeltaM = 0.002; // 2mm per loop + public static final double kStationaryMaxYawRateRadPerSec = 0.05; // ~3 deg/s + public static final int kStationaryLoopsToEndCoast = 10; // ~0.20s @ 20ms + public static final double kDisabledVisionIgnoreAfterDisableSec = 0.25; // 250ms } /************************************************************************* */ @@ -466,7 +487,7 @@ public record CameraConfig( // Example Cameras are mounted in the back corners, 18" up from the floor, facing sideways public static final CameraConfig[] ALL = { new CameraConfig( - "camera_0", + "Photon_BW7", new Transform3d( Inches.of(-13.0), Inches.of(13.0), @@ -483,23 +504,23 @@ public record CameraConfig( } }), // - new CameraConfig( - "camera_1", - new Transform3d( - Inches.of(-13.0), - Inches.of(-13.0), - Inches.of(12.0), - new Rotation3d(0.0, 0.0, -Math.PI / 2)), - 1.0, - new SimCameraProperties() { - { - setCalibration(1280, 800, Rotation2d.fromDegrees(120)); - setCalibError(0.25, 0.08); - setFPS(30); - setAvgLatencyMs(20); - setLatencyStdDevMs(5); - } - }), + // new CameraConfig( + // "camera_1", + // new Transform3d( + // Inches.of(-13.0), + // Inches.of(-13.0), + // Inches.of(12.0), + // new Rotation3d(0.0, 0.0, -Math.PI / 2)), + // 1.0, + // new SimCameraProperties() { + // { + // setCalibration(1280, 800, Rotation2d.fromDegrees(120)); + // setCalibError(0.25, 0.08); + // setFPS(30); + // setAvgLatencyMs(20); + // setLatencyStdDevMs(5); + // } + // }), // ... And more, if needed }; @@ -535,6 +556,12 @@ public static Mode getMode() { }; } + /** Return whether this is pure simulation */ + public static boolean isPureSim() { + boolean isReplay = Logger.hasReplaySource(); + return getMode() == Mode.SIM && !isReplay; + } + /** Get the current swerve drive type */ public static SwerveType getSwerveType() { return swerveType; diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 729b45fe..34c356df 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -36,25 +36,32 @@ */ public class FieldConstants { + /** Specify which type of field your robot is using */ + private static final AprilTagLayoutType fieldType = AprilTagLayoutType.REBUILT_WELDED; + /** AprilTag Field Layout ************************************************ */ public static final double aprilTagWidth = Inches.of(6.50).in(Meters); public static final String aprilTagFamily = "36h11"; - public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); - public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + public static final double fieldLength = fieldType.getLayout().getFieldLength(); + public static final double fieldWidth = fieldType.getLayout().getFieldWidth(); - public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); - public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + public static final int aprilTagCount = fieldType.getLayout().getTags().size(); + public static final AprilTagLayoutType defaultAprilTagType = fieldType; public static final AprilTagFieldLayout aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); @Getter public enum AprilTagLayoutType { - OFFICIAL("2026-official"), - NONE("2026-none"), - REEFSCAPE("2025-official"); + REBUILT_WELDED("2026-rebuilt-welded"), + REBUILT_ANDYMARK("2026-rebuilt-welded"), + REEFSCAPE_WELDED("2025-reefscape-welded"), + REEFSCAPE_ANDYMARK("2025-reefscape-andymark"), + CRESCENDO("2024-crescendo"), + NONE_WELDED("none-welded"), + NONE_ANDYMARK("none-andymark"); AprilTagLayoutType(String name) { try { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b9668ae3..0dbae5e0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,12 +19,12 @@ import com.revrobotics.util.StatusLogger; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.PowerConstants; +import frc.robot.util.TimeUtil; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedPowerDistribution; @@ -48,7 +48,6 @@ public class Robot extends LoggedRobot { private Command m_autoCommandPathPlanner; private RobotContainer m_robotContainer; private Timer m_disabledTimer; - private static boolean isBlueAlliance = false; // Define simulation fields here private VisionSystemSim visionSim; @@ -120,26 +119,8 @@ public Robot() { } // /** This function is called periodically during all modes. */ - // @Override - // public void robotPeriodic() { - - // // Run all virtual subsystems each time through the loop - // VirtualSubsystem.periodicAll(); - - // // Runs the Scheduler. This is responsible for polling buttons, adding - // // newly-scheduled commands, running already-scheduled commands, removing - // // finished or interrupted commands, and running subsystem periodic() methods. - // // This must be called from the robot's periodic block in order for anything in - // // the Command-based framework to work. - // CommandScheduler.getInstance().run(); - // } - - /** TESTING VERSION OF ROBOTPERIODIC FOR OVERRUN SOURCES */ @Override public void robotPeriodic() { - - isBlueAlliance = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; - final long t0 = System.nanoTime(); if (isReal()) { @@ -148,20 +129,27 @@ public void robotPeriodic() { } final long t1 = System.nanoTime(); + // Run all virtual subsystems each time through the loop VirtualSubsystem.periodicAll(); final long t2 = System.nanoTime(); + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. CommandScheduler.getInstance().run(); final long t3 = System.nanoTime(); - Threads.setCurrentThreadPriority(false, 10); + if (isReal()) { + // Return thread to normal priority + Threads.setCurrentThreadPriority(false, 10); + } final long t4 = System.nanoTime(); - Logger.recordOutput("Loop/RobotPeriodic_ms", (t4 - t0) / 1e6); - Logger.recordOutput("Loop/ThreadBoost_ms", (t1 - t0) / 1e6); - Logger.recordOutput("Loop/Virtual_ms", (t2 - t1) / 1e6); - Logger.recordOutput("Loop/Scheduler_ms", (t3 - t2) / 1e6); - Logger.recordOutput("Loop/ThreadRestore_ms", (t4 - t3) / 1e6); + Logger.recordOutput("LogPeriodic/CodeLoop/RobotPeriodicMS", (t4 - t0) / 1e6); + Logger.recordOutput("LogPeriodic/CodeLoop/VirtualMS", (t2 - t1) / 1e6); + Logger.recordOutput("LogPeriodic/CodeLoop/SchedulerMS", (t3 - t2) / 1e6); } /** This function is called once when the robot is disabled. */ @@ -192,7 +180,7 @@ public void autonomousInit() { CommandScheduler.getInstance().cancelAll(); m_robotContainer.getDrivebase().setMotorBrake(true); m_robotContainer.getDrivebase().resetHeadingController(); - m_robotContainer.getVision().resetPoseGate(Timer.getFPGATimestamp()); + m_robotContainer.getVision().resetPoseGate(TimeUtil.now()); // TODO: Make sure Gyro inits here with whatever is in the path planning thingie switch (Constants.getAutoType()) { @@ -322,13 +310,4 @@ public void simulationPeriodic() { // Update sim each sim tick visionSim.update(m_robotContainer.getDrivebase().getPose()); } - - // Helper method to simplify checking if the robot is blue or red alliance - public static boolean isBlue() { - return isBlueAlliance; - } - - public static boolean isRed() { - return !isBlue(); - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 521e09d5..dfd7d65e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc.robot.commands.DriveCommands; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveOdometry; import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; @@ -81,9 +82,6 @@ /** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { - private static final boolean USE_MAPLESIM = true; - public static final boolean MAPLESIM = USE_MAPLESIM && Robot.isSimulation(); - /** Define the Driver and, optionally, the Operator/Co-Driver Controllers */ // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed final CommandXboxController driverController = new CommandXboxController(0); // Main Driver @@ -107,6 +105,9 @@ public class RobotContainer { private final Imu m_imu; private final Vision m_vision; + @SuppressWarnings("unused") + private final DriveOdometry m_driveOdometry; + @SuppressWarnings("unused") private final Accelerometer m_accel; @@ -135,8 +136,6 @@ public class RobotContainer { // Alerts private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO); - public static RobotContainer instance; - /** * Constructor for the Robot Container. This container holds subsystems, opertator interface * devices, and commands. @@ -156,10 +155,11 @@ public RobotContainer() { m_imu = new Imu(SwerveConstants.kImu.factory.get()); m_drivebase = new Drive(m_imu); - m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); + m_driveOdometry = new DriveOdometry(m_drivebase, m_imu, m_drivebase.getModules()); m_vision = new Vision( m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReal(m_drivebase)); + m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); m_accel = new Accelerometer(m_imu); sweep = null; break; @@ -169,27 +169,22 @@ public RobotContainer() { m_imu = new Imu(new ImuIOSim()); m_drivebase = new Drive(m_imu); - m_flywheel = new Flywheel(new FlywheelIOSim()); - - // ---------------- Vision IOs (robot code) ---------------- - var cams = Cameras.ALL; + m_driveOdometry = new DriveOdometry(m_drivebase, m_imu, m_drivebase.getModules()); m_vision = new Vision( m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsSim(m_drivebase)); + m_flywheel = new Flywheel(new FlywheelIOSim()); m_accel = new Accelerometer(m_imu); - // ---------------- CameraSweepEvaluator (sim-only analysis) ---------------- + // CameraSweepEvaluator (sim-only analysis) VisionSystemSim visionSim = new VisionSystemSim("CameraSweepWorld"); visionSim.addAprilTags(FieldConstants.aprilTagLayout); - + var cams = Cameras.ALL; PhotonCameraSim[] simCams = new PhotonCameraSim[cams.length]; - for (int i = 0; i < cams.length; i++) { var cfg = cams[i]; - PhotonCamera photonCam = new PhotonCamera(cfg.name()); PhotonCameraSim camSim = new PhotonCameraSim(photonCam, cfg.simProps()); - visionSim.addCamera(camSim, cfg.robotToCamera()); simCams[i] = camSim; } @@ -208,16 +203,18 @@ public RobotContainer() { RBSICANBusRegistry.initSim(CANBuses.RIO, CANBuses.DRIVE); m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); - m_flywheel = new Flywheel(new FlywheelIO() {}); + m_driveOdometry = new DriveOdometry(m_drivebase, m_imu, m_drivebase.getModules()); m_vision = - new Vision(m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReplay()); + new Vision( + m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsReplay(m_drivebase)); + + m_flywheel = new Flywheel(new FlywheelIO() {}); m_accel = new Accelerometer(m_imu); sweep = null; break; } // Init all CAN busses specified in the `Constants.CANBuses` class - RBSICANBusRegistry.initReal(Constants.CANBuses.ALL); canHealth = Arrays.stream(Constants.CANBuses.ALL).map(RBSICANHealth::new).toList(); // In addition to the initial battery capacity from the Dashbaord, ``RBSIPowerMonitor`` takes @@ -457,7 +454,8 @@ public void getAutonomousCommandChoreo() { /** Updates the alerts. */ public void updateAlerts() { // AprilTag layout alert - boolean aprilTagAlertActive = Constants.getAprilTagLayoutType() != AprilTagLayoutType.OFFICIAL; + boolean aprilTagAlertActive = + Constants.getAprilTagLayoutType() != AprilTagLayoutType.REBUILT_WELDED; aprilTagLayoutAlert.set(aprilTagAlertActive); if (aprilTagAlertActive) { aprilTagLayoutAlert.setText( @@ -550,8 +548,21 @@ private VisionIO[] buildVisionIOsSim(Drive drive) { } // Vision Factories (REPLAY) - private VisionIO[] buildVisionIOsReplay() { - return new VisionIO[] {}; // simplest: Vision does nothing during replay + private VisionIO[] buildVisionIOsReplay(Drive drive) { + var cams = Constants.Cameras.ALL; + + VisionIO[] ios = new VisionIO[cams.length]; + for (int i = 0; i < cams.length; i++) { + ios[i] = + new VisionIO() { + @Override + public void updateInputs(VisionIOInputs inputs) { + // Intentionally empty. + // Logger.processInputs("Vision/Camera" + i, inputs) will populate these from the log. + } + }; + } + return ios; } /** diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index fcb7dd6c..e6ebe08d 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -14,10 +14,10 @@ package frc.robot.subsystems.accelerometer; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.Constants.RobotConstants; import frc.robot.subsystems.imu.Imu; +import frc.robot.util.TimeUtil; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; @@ -30,9 +30,6 @@ */ public class Accelerometer extends VirtualSubsystem { - // Gravitational acceleration - private static final double G_TO_MPS2 = 9.80665; - // Define hardware interfaces private final RioAccelIO rio; private final RioAccelIO.Inputs rioInputs = new RioAccelIO.Inputs(); @@ -55,6 +52,12 @@ public Accelerometer(Imu imu) { this.rio = new RioAccelIORoboRIO(200.0); // 200 Hz is a good start } + // Priority value for this virtual subsystem + @Override + protected int getPeriodPriority() { + return +10; + } + @Override public void rbsiPeriodic() { final boolean doProfile = (++profileCount >= PROFILE_EVERY_N); @@ -65,8 +68,12 @@ public void rbsiPeriodic() { rio.updateInputs(rioInputs); // Compute RIO accelerations and jerks - rawRio = new Translation3d(rioInputs.xG, rioInputs.yG, rioInputs.zG); - rioAcc = rawRio.rotateBy(RobotConstants.kRioOrientation).times(G_TO_MPS2); + rawRio = + new Translation3d( + rioInputs.xG * Constants.G_TO_MPS2, + rioInputs.yG * Constants.G_TO_MPS2, + rioInputs.zG * Constants.G_TO_MPS2); + rioAcc = rawRio.rotateBy(RobotConstants.kRioOrientation); // Acceleration from previous loop prevRioAcc = rioAcc; @@ -89,7 +96,7 @@ public void rbsiPeriodic() { final double[] ts = imuInputs.odometryYawTimestamps; if (ts.length > 0) { - Logger.recordOutput("IMU/OdometryLatencySec", Timer.getFPGATimestamp() - ts[ts.length - 1]); + Logger.recordOutput("Odometry/IMULatencySec", TimeUtil.now() - ts[ts.length - 1]); } } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index aeb71e65..c6e36a5a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,11 +1,19 @@ // Copyright (c) 2024-2026 Az-FIRST // http://github.com/AZ-First -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage // -// Use of this source code is governed by a BSD -// license that can be found in the AdvantageKit-License.md file -// at the root directory of this project. +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// 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.subsystems.drive; @@ -22,7 +30,6 @@ import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -33,14 +40,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; @@ -53,6 +57,8 @@ import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; import frc.robot.util.RBSISubsystem; +import frc.robot.util.TimeUtil; +import frc.robot.util.TimedPose; import java.util.Optional; import java.util.OptionalDouble; import java.util.concurrent.locks.Lock; @@ -73,7 +79,7 @@ public class Drive extends RBSISubsystem { private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; - // Buffers for necessary things + // Pose Buffer Declarations private final ConcurrentTimeInterpolatableBuffer poseBuffer = ConcurrentTimeInterpolatableBuffer.createBuffer(DrivebaseConstants.kHistorySize); private final ConcurrentTimeInterpolatableBuffer yawBuffer = @@ -86,6 +92,8 @@ public class Drive extends RBSISubsystem { new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); // Declare odometry and pose-related variables + // This one is package-private; used in DriveOdometry, PhoenixOdometryThread, and + // SparkOdometryThread static final Lock odometryLock = new ReentrantLock(); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private SwerveModulePosition[] lastModulePositions = // For delta tracking @@ -95,12 +103,6 @@ public class Drive extends RBSISubsystem { new SwerveModulePosition(), new SwerveModulePosition() }; - private final SwerveModulePosition[] odomPositions = { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, Rotation2d.kZero, lastModulePositions, Pose2d.kZero); @@ -112,6 +114,19 @@ public class Drive extends RBSISubsystem { private volatile long poseResetEpoch = 0; // monotonic counter private volatile double lastPoseResetTimestamp = Double.NEGATIVE_INFINITY; + // Pose Regimes (ENABLED, DISABLED_COAST, DISABLE_STATIONARY) + private boolean lastEnabled = false; + private double disabledCoastUntilTs = Double.NEGATIVE_INFINITY; + private double disabledCoastStartTs = Double.NEGATIVE_INFINITY; + private final double[] lastWheelDistM = new double[4]; + private boolean haveLastWheelDist = false; + private int stationaryLoops = 0; + + // Related to vision injection of pose + private boolean disabledVisionInitialized = false; + private Pose2d lastDisabledVisionPose = new Pose2d(); + private double lastDisabledVisionTs = Double.NaN; + /** Constructor */ public Drive(Imu imu) { this.imu = imu; @@ -250,157 +265,16 @@ public Drive(Imu imu) { } /************************************************************************* */ - /** Periodic function that is called each robot cycle by the command scheduler */ + /** Periodic function that is called each cycle by the command scheduler */ @Override public void rbsiPeriodic() { - odometryLock.lock(); - try { - // Ensure IMU inputs are fresh for this cycle - final var imuInputs = imu.getInputs(); - - // Stop modules & log empty setpoint states if disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - } - - // Drain per-module odometry queues for this cycle - for (var module : modules) { - module.periodic(); - } - - // -------- REAL ODOMETRY REPLAY (canonical timestamps from module[0]) -------- - if (Constants.getMode() != Mode.SIM) { - final double[] ts = modules[0].getOdometryTimestamps(); - final int n = (ts == null) ? 0 : ts.length; - - // Cache per-module histories ONCE (avoid repeated getters in the loop) - final SwerveModulePosition[][] modHist = new SwerveModulePosition[4][]; - for (int m = 0; m < 4; m++) { - modHist[m] = modules[m].getOdometryPositions(); - } - - // Determine yaw queue availability - final boolean hasYawQueue = - imuInputs.connected - && imuInputs.odometryYawTimestamps != null - && imuInputs.odometryYawPositionsRad != null - && imuInputs.odometryYawTimestamps.length - == imuInputs.odometryYawPositionsRad.length - && imuInputs.odometryYawTimestamps.length > 0; - - final double[] yawTs = hasYawQueue ? imuInputs.odometryYawTimestamps : null; - final double[] yawPos = hasYawQueue ? imuInputs.odometryYawPositionsRad : null; - - // If we have no module samples, still keep yaw buffers “alive” for gating callers - if (n == 0) { - final double now = Timer.getFPGATimestamp(); - yawBuffer.addSample(now, imuInputs.yawPositionRad); - yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); - - gyroDisconnectedAlert.set(!imuInputs.connected); - return; - } - - // Decide whether yaw queue is index-aligned with module[0] timestamps. - // We only trust index alignment if BOTH: - // - yaw has at least n samples - // - yawTs[i] ~= ts[i] for i in range (tight epsilon) - boolean yawIndexAligned = false; - if (hasYawQueue && yawTs.length >= n) { - yawIndexAligned = true; - final double eps = 1e-3; // 1 ms - for (int i = 0; i < n; i++) { - if (Math.abs(yawTs[i] - ts[i]) > eps) { - yawIndexAligned = false; - break; - } - } - } - - // If yaw is NOT index-aligned but we have a yaw queue, build yaw/yawRate buffers ONCE. - if (hasYawQueue && !yawIndexAligned) { - for (int k = 0; k < yawTs.length; k++) { - yawBuffer.addSample(yawTs[k], yawPos[k]); - if (k > 0) { - final double dt = yawTs[k] - yawTs[k - 1]; - if (dt > 1e-6) { - yawRateBuffer.addSample(yawTs[k], (yawPos[k] - yawPos[k - 1]) / dt); - } - } - } - } - - // If NO yaw queue, add a single “now” sample once (don’t do this per replay sample) - if (!hasYawQueue) { - final double now = Timer.getFPGATimestamp(); - yawBuffer.addSample(now, imuInputs.yawPositionRad); - yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); - } - - // Replay each odometry sample - for (int i = 0; i < n; i++) { - final double t = ts[i]; - - // Build module positions at this sample index (clamp defensively) - for (int m = 0; m < 4; m++) { - final SwerveModulePosition[] hist = modHist[m]; - if (hist == null || hist.length == 0) { - odomPositions[m] = modules[m].getPosition(); - } else if (i < hist.length) { - odomPositions[m] = hist[i]; - } else { - odomPositions[m] = hist[hist.length - 1]; - } - } - - // Determine yaw at this timestamp - double yawRad = imuInputs.yawPositionRad; // fallback - - if (hasYawQueue) { - if (yawIndexAligned) { - yawRad = yawPos[i]; - - // Keep yaw/yawRate buffers updated in odometry timebase (good for yaw-gate) - yawBuffer.addSample(t, yawRad); - if (i > 0) { - final double dt = yawTs[i] - yawTs[i - 1]; - if (dt > 1e-6) { - yawRateBuffer.addSample(t, (yawPos[i] - yawPos[i - 1]) / dt); - } - } - } else { - // yawBuffer was pre-filled above; interpolate here - yawRad = yawBuffer.getSample(t).orElse(imuInputs.yawPositionRad); - } - } - - // Feed estimator at this historical timestamp - m_PoseEstimator.updateWithTime(t, Rotation2d.fromRadians(yawRad), odomPositions); - - // Maintain pose history in SAME timebase as estimator - poseBuffer.addSample(t, m_PoseEstimator.getEstimatedPosition()); - } - - Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); - gyroDisconnectedAlert.set(!imuInputs.connected); - return; - } - - // SIMULATION: Keep sim pose buffer time-aligned, too - double now = Timer.getFPGATimestamp(); - poseBuffer.addSample(now, simPhysics.getPose()); - yawBuffer.addSample(now, simPhysics.getYaw().getRadians()); - yawRateBuffer.addSample(now, simPhysics.getOmegaRadPerSec()); - - Logger.recordOutput("Drive/Pose", simPhysics.getPose()); - gyroDisconnectedAlert.set(false); - } finally { - odometryLock.unlock(); + // The only function of the drive periodic() is to stop the modules if the DriverStation is + // diabled. + if (DriverStation.isDisabled()) { + for (var module : modules) module.stop(); + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } } @@ -413,6 +287,10 @@ public void rbsiPeriodic() { */ @Override public void simulationPeriodic() { + + // IMPORTANT: do not run sim physics during REPLAY + if (Constants.getMode() != Mode.SIM) return; + final double dt = Constants.loopPeriodSecs; // Advance module wheel physics @@ -420,7 +298,7 @@ public void simulationPeriodic() { modules[i].simulationPeriodic(); } - // Get module states from modules + // Get module states from modules (ok to allocate; can be cached later if desired) final SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; for (int i = 0; i < modules.length; i++) { moduleStates[i] = modules[i].getState(); @@ -440,31 +318,14 @@ public void simulationPeriodic() { imu.simulationSetOmegaRadPerSec(omegaRadPerSec); imu.simulationSetLinearAccelMps2(ax, ay, 0.0); - // Feed PoseEstimator with authoritative yaw and module positions - final SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; - for (int i = 0; i < modules.length; i++) { - modulePositions[i] = modules[i].getPosition(); - } - m_PoseEstimator.resetPosition( - Rotation2d.fromRadians(yawRad), modulePositions, simPhysics.getPose()); - - // If simulated vision available, inject vision measurement - if (simulatedVisionAvailable) { - final Pose2d visionPose = getSimulatedVisionPose(); - final double visionTimestamp = Timer.getFPGATimestamp(); - final var visionStdDevs = getSimulatedVisionStdDevs(); - m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); - } - - poseBuffer.addSample(Timer.getFPGATimestamp(), simPhysics.getPose()); - - // Logging + // Logging ONLY for physics (NOT estimator) Logger.recordOutput("Sim/Pose", simPhysics.getPose()); Logger.recordOutput("Sim/YawRad", yawRad); - Logger.recordOutput("Sim/OmegaRadPerSec", simPhysics.getOmegaRadPerSec()); + Logger.recordOutput("Sim/OmegaRadPerSec", omegaRadPerSec); Logger.recordOutput("Sim/LinearAccelXY_mps2", new double[] {ax, ay}); } + /************************************************************************* */ /** Drive Base Action Functions ****************************************** */ /** @@ -522,7 +383,11 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - /** Runs the drive in a straight line with the specified drive output. */ + /** + * Runs the drive in a straight line with the specified drive output + * + * @param output Specified drive output for characterization + */ public void runCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runCharacterization(output); @@ -538,11 +403,97 @@ public void resetHeadingController() { angleController.reset(getHeading().getRadians()); } - /** Getter function for the angle controller */ - public ProfiledPIDController getAngleController() { - return angleController; + /** + * Update the Disabled Coast State + * + *

The purpose of this function is to determine the coasting state of the robot on the ENABLE + * -> DISABLE edge. While the robot coasts to a stop, the wheel odometry will continue to + * integrate with usual vision input. Once the robot stops moving (within tolerance), the vision + * injection to the Pose will take over. + * + * @param enabledNow Are we enabled now? + * @param disabledNow Are we disabled now? + * @param now When is now? + * @param yawRateRadPerSec Current drivebase rotation rate + * @param odomPositions List of module odometry positions + */ + public void updateDisabledCoastState( + boolean enabledNow, + boolean disabledNow, + double now, + double yawRateRadPerSec, + SwerveModulePosition[] odomPositions) { + + // Don’t end coast “instantly” right after disable edge + final double minCoastTime = 0.25; // seconds -- maybe put into Constants??? + final boolean pastMin = (now - disabledCoastStartTs) >= minCoastTime; + + // Detect ENABLED -> DISABLED edge -- set `disabledCoastUntilTs` when COAST-phase ends + if (lastEnabled && !enabledNow) { + disabledCoastStartTs = now; + disabledCoastUntilTs = now + DrivebaseConstants.kDisabledCoastSeconds; + + stationaryLoops = 0; + haveLastWheelDist = false; // reset delta baseline on transition + } + lastEnabled = enabledNow; + + // If not disabled, no coast. + if (!disabledNow) { + stationaryLoops = 0; + haveLastWheelDist = false; + return; + } + + // If coast already expired, nothing to do. + if (!(now < disabledCoastUntilTs)) { + return; + } + + // Need odometry positions to detect motion + if (odomPositions == null || odomPositions.length < 4) { + return; + } + + // Compute max wheel delta this loop + double maxDelta = 0.0; + if (haveLastWheelDist) { + for (int i = 0; i < 4; i++) { + double dist = odomPositions[i].distanceMeters; + double d = Math.abs(dist - lastWheelDistM[i]); + if (d > maxDelta) maxDelta = d; + } + } + + // Update baseline for next loop + for (int i = 0; i < 4; i++) { + lastWheelDistM[i] = odomPositions[i].distanceMeters; + } + haveLastWheelDist = true; + + // Stationary test (must have baseline) + if (haveLastWheelDist + && maxDelta <= DrivebaseConstants.kStationaryMaxWheelDeltaM + && Math.abs(yawRateRadPerSec) <= DrivebaseConstants.kStationaryMaxYawRateRadPerSec) { + stationaryLoops++; + } else { + stationaryLoops = 0; + } + + // End coast early if stationary long enough + if (pastMin && stationaryLoops >= DrivebaseConstants.kStationaryLoopsToEndCoast) { + disabledCoastUntilTs = now; // expires immediately + } + + // Debug logs (optional) + Logger.recordOutput("Odometry/Coast/active", isDisabledCoast(now)); + Logger.recordOutput("Odometry/Coast/untilTs", disabledCoastUntilTs); + Logger.recordOutput("Odometry/Coast/stationaryLoops", stationaryLoops); + Logger.recordOutput("Odometry/Coast/maxWheelDeltaM", maxDelta); + Logger.recordOutput("Odometry/Coast/yawRateRadPerSec", yawRateRadPerSec); } + /************************************************************************* */ /** SysId Characterization Routines ************************************** */ /** Returns a command to run a quasistatic test in the specified direction. */ @@ -557,8 +508,19 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction)); } + /************************************************************************* */ /** Getter Functions ***************************************************** */ + /** Returns the module array */ + public Module[] getModules() { + return modules; + } + + /** Return the prodiledPID angle controller */ + public ProfiledPIDController getAngleController() { + return angleController; + } + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -571,7 +533,7 @@ private SwerveModuleState[] getModuleStates() { /** Returns the module positions (turn angles and drive positions) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Positions") - private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { states[i] = modules[i].getPosition(); @@ -585,19 +547,23 @@ public ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") + /** + * Returns the current odometry pose. + * + *

If the code is running as pure simulation (i.e., not REPLAY of a log), return the simulated + * physics pose. Otherwise, return the pose from the pose estimator. + */ public Pose2d getPose() { - if (Constants.getMode() == Mode.SIM) { + if (Constants.isPureSim()) { return simPhysics.getPose(); } return m_PoseEstimator.getEstimatedPosition(); } - /** Returns the current odometry rotation. */ + /** Returns the current odometry YAW. */ @AutoLogOutput(key = "Odometry/Yaw") public Rotation2d getHeading() { - if (Constants.getMode() == Mode.SIM) { + if (Constants.isPureSim()) { return simPhysics.getYaw(); } return imu.getYaw(); @@ -632,6 +598,16 @@ public Optional getPoseAtTime(double timestampSeconds) { return poseBuffer.getSample(timestampSeconds); } + /** Returns the oldest timetamp in the current pose buffer */ + public double getPoseBufferOldestTime() { + return poseBuffer.getOldestTimestamp().getAsDouble(); + } + + /** Returns the newest timetamp in the current pose buffer */ + public double getPoseBufferNewestTime() { + return poseBuffer.getNewestTimestamp().getAsDouble(); + } + /** * Max abs yaw rate over [t0, t1] using buffered yaw-rate history * @@ -698,6 +674,21 @@ public static Translation2d[] getModuleTranslations() { }; } + /** Returns whether the robot is currently in the DISABLED_COAST state */ + public boolean isDisabledCoast() { + return isDisabledCoast(TimeUtil.now()); + } + + /** Returns whether the robot was in the DISABLED_COAST state at time `timestamp` */ + public boolean isDisabledCoast(double timestamp) { + return DriverStation.isDisabled() && (timestamp < disabledCoastUntilTs); + } + + /** Returns the disabledCoastStartTs variable */ + public double getDisabledCoastStartTs() { + return disabledCoastStartTs; + } + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; @@ -716,12 +707,17 @@ public double getFFCharacterizationVelocity() { return output; } + /************************************************************************* */ /* Setter Functions ****************************************************** */ - /** Resets the current odometry pose. */ + /** + * Resets the current odometry pose + * + * @param pose The specified pose to which to reset the poseEsitmator + */ public void resetPose(Pose2d pose) { m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), pose); - markPoseReset(Timer.getFPGATimestamp()); + markPoseReset(TimeUtil.now()); } /** Zeros the gyro based on alliance color */ @@ -731,21 +727,137 @@ public void zeroHeadingForAlliance() { ? Rotation2d.kZero : Rotation2d.k180deg); resetHeadingController(); - markPoseReset(Timer.getFPGATimestamp()); + markPoseReset(TimeUtil.now()); } - /** Zeros the heading */ + /** Zeros the gyro regardless of the alliance */ public void zeroHeading() { imu.zeroYaw(Rotation2d.kZero); resetHeadingController(); - markPoseReset(Timer.getFPGATimestamp()); + markPoseReset(TimeUtil.now()); } - /** Adds a vision measurement safely into the PoseEstimator. */ - public void addVisionMeasurement(Pose2d pose, double timestampSeconds, Matrix stdDevs) { + /** + * Adds a vision measurement safely into the PoseEstimator + * + * @param measurement The pose @ timestamp to add to the pose estimator + */ + // Called by Vision via consumer.accept(TimedPose) + public void addVisionMeasurement(TimedPose meas) { odometryLock.lock(); try { - m_PoseEstimator.addVisionMeasurement(pose, timestampSeconds, stdDevs); + // Always use measurement timestamp when fusing (enabled path) + final double t = meas.timestampSeconds(); + final Pose2d vision = meas.pose(); + + // ENABLED: normal fusion + if (!DriverStation.isDisabled()) { + disabledVisionInitialized = false; + lastDisabledVisionTs = Double.NaN; + m_PoseEstimator.addVisionMeasurement(vision, t, meas.stdDevs()); + return; + } + + // DISABLED -- check if within "coast phase" + final boolean coast = isDisabledCoast(t); + + // If coasting, + if (coast) { + final double coastAge = t - getDisabledCoastStartTs(); + Logger.recordOutput("Vision/Debug/disabledCoastAge", coastAge); + + // Ignore vision briefly right after ENABLE->DISABLE (prevents “phase mismatch” at disable + // edge) + if (coastAge >= 0.0 && coastAge < DrivebaseConstants.kDisabledVisionIgnoreAfterDisableSec) { + Logger.recordOutput("Vision/Debug/disabledIgnoreEarlyCoast", true); + return; + } + } + Logger.recordOutput("Vision/Debug/disabledIgnoreEarlyCoast", false); + + // If we're coasting, avoid snapping Pose to Vision; lean gentler than stationary. + final double alpha = + coast + ? Math.min(DrivebaseConstants.kDisabledVisionBlendAlpha, 0.05) + : DrivebaseConstants.kDisabledVisionBlendAlpha; + + // "Current" for blending target (estimator pose) + final Pose2d current = m_PoseEstimator.getEstimatedPosition(); + + // Debug + Logger.recordOutput("Vision/Debug/disabledCoast", coast); + Logger.recordOutput("Vision/Debug/disabledVisionInitialized", disabledVisionInitialized); + Logger.recordOutput("Vision/Debug/disabledVisionTs", t); + Logger.recordOutput( + "Vision/Debug/disabledVisionAge", + Double.isFinite(lastDisabledVisionTs) ? (t - lastDisabledVisionTs) : Double.NaN); + + // Check if the last while-disabled vision timestamp is stale (too old) + final boolean stale = + Double.isFinite(lastDisabledVisionTs) + && (t - lastDisabledVisionTs) > DrivebaseConstants.kDisabledVisionStale; + Logger.recordOutput("Vision/Debug/visionStale", stale); + + // If coasting, intentionally DO NOT snap; reset initialization so that once coast ends, the + // first good stationary frame snaps. + if (coast) { + disabledVisionInitialized = false; + } + + // If not initialized AND not coasting: snap hard to vision once + if (!disabledVisionInitialized && !coast) { + disabledVisionInitialized = true; + lastDisabledVisionPose = vision; + lastDisabledVisionTs = t; + + m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), vision); + markPoseReset(t); + poseBufferAddSample(t, vision); + + Logger.recordOutput("Vision/DisabledInitSnap", true); + Logger.recordOutput("Vision/DisabledReject", false); + Logger.recordOutput("Vision/DisabledBlendAlphaUsed", alpha); + return; + } + Logger.recordOutput("Vision/DisabledInitSnap", false); + + // Check that there is not a huge jump from the last accepted disabled vision pose + final Pose2d gateRef = + Double.isFinite(lastDisabledVisionTs) ? lastDisabledVisionPose : vision; + + final double deltaTranslation = gateRef.getTranslation().getDistance(vision.getTranslation()); + final double deltaRotation = + Math.abs(gateRef.getRotation().minus(vision.getRotation()).getRadians()); + + Logger.recordOutput("Vision/Debug/dTransFromLastVision", deltaTranslation); + Logger.recordOutput("Vision/Debug/dRotFromLastVision", deltaRotation); + + // Reject large jumps only if vision measurement is not stale (large delta-T can mean large + // change in position) + if (!stale + && (deltaTranslation > DrivebaseConstants.kDisabledVisionMaxJumpM + || deltaRotation > DrivebaseConstants.kDisabledVisionMaxJumpRad)) { + Logger.recordOutput("Vision/DisabledReject", true); + Logger.recordOutput("Vision/DisabledBlendAlphaUsed", alpha); + return; + } + Logger.recordOutput("Vision/DisabledReject", false); + + // Accept this vision frame as the new reference + lastDisabledVisionPose = vision; + lastDisabledVisionTs = t; + + // Blend toward vision -- gentle correction + final Pose2d blended = current.interpolate(vision, alpha); + + // Push values to pose estimator and pose buffer + m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), blended); + markPoseReset(t); + poseBufferAddSample(t, blended); + + Logger.recordOutput("Vision/DisabledBlendedPose", blended); + Logger.recordOutput("Vision/DisabledBlendAlphaUsed", alpha); + } finally { odometryLock.unlock(); } @@ -763,8 +875,80 @@ private void markPoseReset(double fpgaNow) { Logger.recordOutput("Drive/PoseResetTimestamp", lastPoseResetTimestamp); } - /** UTILITY FUNCTION SECTION ********************************************* */ + /************************************************************************* */ + /** + * DriveOdometry Helpers (package-private) + * + *

The pose estimator and pose buffers are owned by Drive, but DriveOdometry needs access to + * them in order to update and process the odometry. These functions are the appropriate + * pass-throughs to allow this functionality. + */ + + /** Update the pose estimator at a timestamp */ + void poseEstimatorUpdateWithTime(double t, Rotation2d yaw, SwerveModulePosition[] positions) { + m_PoseEstimator.updateWithTime(t, yaw, positions); + } + + /** Add a sample to the pose buffer */ + void poseBufferAddSample(double t, Pose2d pose) { + poseBuffer.addSample(t, pose); + } + + /** Yaw buffer helper */ + double yawBufferSampleOr(double t, double fallbackYawRad) { + return yawBuffer.getSample(t).orElse(fallbackYawRad); + } + + /** Yaw buffer helper */ + void yawBuffersAddSample(double t, double yawRad, double yawRateRadPerSec) { + yawBuffer.addSample(t, yawRad); + yawRateBuffer.addSample(t, yawRateRadPerSec); + } + + /** Yaw buffer helper */ + void yawBuffersFillFromQueue(double[] yawTs, double[] yawPosRad) { + for (int k = 0; k < yawTs.length; k++) { + yawBuffer.addSample(yawTs[k], yawPosRad[k]); + if (k > 0) { + double dt = yawTs[k] - yawTs[k - 1]; + if (dt > 1e-6) { + yawRateBuffer.addSample(yawTs[k], (yawPosRad[k] - yawPosRad[k - 1]) / dt); + } + } + } + } + + /** Yaw buffer helper */ + void yawBuffersAddSampleIndexAligned(double t, double[] yawTs, double[] yawPos, int i) { + yawBuffer.addSample(t, yawPos[i]); + if (i > 0) { + double dt = yawTs[i] - yawTs[i - 1]; + if (dt > 1e-6) { + yawRateBuffer.addSample(t, (yawPos[i] - yawPos[i - 1]) / dt); + } + } + } + + /** Set the gyroDisconnectedAlert */ + void setGyroDisconnectedAlert(boolean disconnected) { + gyroDisconnectedAlert.set(disconnected); + } + + /************************************************************************* */ + /** Simulation Getter Functions (from simPhysics) */ + public Pose2d getSimPose() { + return simPhysics.getPose(); + } + + public double getSimYawRad() { + return simPhysics.getYaw().getRadians(); + } + public double getSimYawRateRadPerSec() { + return simPhysics.getOmegaRadPerSec(); + } + + /************************************************************************* */ /** CHOREO SECTION (Ignore if AutoType == PATHPLANNER) ******************* */ /** Choreo: Reset odometry */ @@ -820,44 +1004,4 @@ public void followTrajectory(SwerveSample sample) { // Apply the generated speeds runVelocity(speeds); } - - /** SIMULATION VISION FUNCTIONS ****************************************** */ - - // Vision measurement enabled in simulation - private boolean simulatedVisionAvailable = true; - - // Maximum simulated noise in meters/radians - private static final double SIM_VISION_POS_NOISE_M = 0.02; // +/- 2cm - private static final double SIM_VISION_YAW_NOISE_RAD = Math.toRadians(2); // +/- 2 degrees - - /** - * Returns a simulated Pose2d for vision in field coordinates. Adds a small random jitter to - * simulate measurement error. - */ - private Pose2d getSimulatedVisionPose() { - Pose2d truePose = simPhysics.getPose(); // authoritative pose - - // Add small random noise - double dx = (Math.random() * 2 - 1) * SIM_VISION_POS_NOISE_M; - double dy = (Math.random() * 2 - 1) * SIM_VISION_POS_NOISE_M; - double dTheta = (Math.random() * 2 - 1) * SIM_VISION_YAW_NOISE_RAD; - - return new Pose2d( - truePose.getX() + dx, - truePose.getY() + dy, - truePose.getRotation().plus(new Rotation2d(dTheta))); - } - - /** - * Returns the standard deviations for the simulated vision measurement. These values are used by - * the PoseEstimator to weight vision updates. - */ - private edu.wpi.first.math.Matrix getSimulatedVisionStdDevs() { - edu.wpi.first.math.Matrix stdDevs = - new edu.wpi.first.math.Matrix<>(N3.instance, N1.instance); - stdDevs.set(0, 0, 0.02); // X standard deviation (meters) - stdDevs.set(1, 0, 0.02); // Y standard deviation (meters) - stdDevs.set(2, 0, Math.toRadians(2)); // rotation standard deviation (radians) - return stdDevs; - } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java new file mode 100644 index 00000000..7dc4925b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java @@ -0,0 +1,269 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// 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.subsystems.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants; +import frc.robot.subsystems.imu.Imu; +import frc.robot.util.RBSIEnum.Mode; +import frc.robot.util.TimeUtil; +import frc.robot.util.VirtualSubsystem; +import org.littletonrobotics.junction.Logger; + +public final class DriveOdometry extends VirtualSubsystem { + + // Declare the io and inputs + private final Drive drive; + private final Imu imu; + private final Module[] modules; + + // Per-cycle cached objects (to avoid repeated allocations) + private final SwerveModulePosition[] odomPositions = new SwerveModulePosition[4]; + + // Checking whether this is a REPLAY + private boolean isReplayActive = Logger.hasReplaySource(); + + /** Constructor */ + public DriveOdometry(Drive drive, Imu imu, Module[] modules) { + this.drive = drive; + this.imu = imu; + this.modules = modules; + } + + /** + * Priority value for this virtual subsystem + * + *

See `frc.robot.util.VirtualSubsystem` for a description of the suggested values for various + * virtual subsystems. + */ + @Override + protected int getPeriodPriority() { + return -20; + } + + /** Periodic function to read inputs */ + @Override + public void rbsiPeriodic() { + + Drive.odometryLock.lock(); + try { + final var imuInputs = imu.getInputs(); + + // Drain per-module odometry queues ONCE per loop (refresh signals). + for (var module : modules) { + module.periodic(); + } + + // ---------------------------------------------------------------------- + // Pure SIM (not replaying a log): use sim pose/yaw + // ---------------------------------------------------------------------- + if (Constants.isPureSim()) { + final double now = TimeUtil.now(); + + // Keep buffers alive + drive.poseBufferAddSample(now, drive.getSimPose()); + drive.yawBuffersAddSample(now, drive.getSimYawRad(), drive.getSimYawRateRadPerSec()); + + // Coast state uses "now" + current module positions + drive.updateDisabledCoastState( + DriverStation.isEnabled(), + DriverStation.isDisabled(), + now, + drive.getSimYawRateRadPerSec(), + drive.getModulePositions()); + + return; + } + + // ---------------------------------------------------------------------- + // DISABLED (REAL only): minimal ticking — keep buffers alive, do NOT integrate module deltas. + // (If you want replay integration while disabled, this branch is already !isReplayActive.) + // ---------------------------------------------------------------------- + if (DriverStation.isDisabled() && !isReplayActive) { + final double now = TimeUtil.now(); + + // keep yaw buffers alive + if (imuInputs.connected) { + drive.yawBuffersAddSample(now, imuInputs.yawPositionRad, imuInputs.yawRateRadPerSec); + } + + // Coast state from "now" + current module positions + drive.updateDisabledCoastState( + DriverStation.isEnabled(), + DriverStation.isDisabled(), + now, + imuInputs.yawRateRadPerSec, + drive.getModulePositions()); + + // keep pose buffer alive with the *current estimator pose* + drive.poseBufferAddSample(now, drive.getPose()); + drive.setGyroDisconnectedAlert(!imuInputs.connected); + return; + } + + // ---------------------------------------------------------------------- + // Canonical timestamp queue from module[0] + // ---------------------------------------------------------------------- + final double[] ts = modules[0].getOdometryTimestamps(); + final int n = (ts == null) ? 0 : ts.length; + + // Always keep yaw buffers “alive” even if no samples + if (n == 0) { + if (Constants.getMode() != Mode.REPLAY) { + final double now = TimeUtil.now(); + drive.yawBuffersAddSample(now, imuInputs.yawPositionRad, imuInputs.yawRateRadPerSec); + + // Coast state update (no per-sample positions available; use current) + drive.updateDisabledCoastState( + DriverStation.isEnabled(), + DriverStation.isDisabled(), + now, + imuInputs.yawRateRadPerSec, + drive.getModulePositions()); + } + drive.setGyroDisconnectedAlert(!imuInputs.connected); + return; + } + + // Cache module histories once + final SwerveModulePosition[][] modHist = new SwerveModulePosition[4][]; + for (int m = 0; m < 4; m++) { + modHist[m] = modules[m].getOdometryPositions(); + } + + // ---------------------------------------------------------------------- + // Determine YAW queue availability (everything exists and lines up) + // ---------------------------------------------------------------------- + final boolean hasYawQueue = + imuInputs.connected + && imuInputs.odometryYawTimestamps != null + && imuInputs.odometryYawPositionsRad != null + && imuInputs.odometryYawTimestamps.length == imuInputs.odometryYawPositionsRad.length + && imuInputs.odometryYawTimestamps.length > 0; + + final double[] yawTs = hasYawQueue ? imuInputs.odometryYawTimestamps : null; + final double[] yawPos = hasYawQueue ? imuInputs.odometryYawPositionsRad : null; + + // Determine index alignment + boolean yawIndexAligned = false; + if (hasYawQueue && yawTs.length >= n) { + yawIndexAligned = true; + final double eps = 1e-3; // 1ms + for (int i = 0; i < n; i++) { + if (Math.abs(yawTs[i] - ts[i]) > eps) { + yawIndexAligned = false; + break; + } + } + } + + // If yaw not aligned, pre-fill yaw buffers once and interpolate later + if (hasYawQueue && !yawIndexAligned) { + drive.yawBuffersFillFromQueue(yawTs, yawPos); + } else if (!hasYawQueue) { + final double now = TimeUtil.now(); + drive.yawBuffersAddSample(now, imuInputs.yawPositionRad, imuInputs.yawRateRadPerSec); + } + + // ---------------------------------------------------------------------- + // Replay each odometry sample + // ---------------------------------------------------------------------- + final double[] lastDist = new double[4]; + boolean haveLastDist = false; + + for (int i = 0; i < n; i++) { + final double t = ts[i]; + + // Build module positions at sample i (clamp defensively) + for (int m = 0; m < 4; m++) { + final SwerveModulePosition[] hist = modHist[m]; + if (hist == null || hist.length == 0) { + odomPositions[m] = modules[m].getPosition(); + } else if (i < hist.length) { + odomPositions[m] = hist[i]; + } else { + odomPositions[m] = hist[hist.length - 1]; + } + } + + // Determine yaw at this timestamp + double yawRad = imuInputs.yawPositionRad; + if (hasYawQueue) { + if (yawIndexAligned) { + yawRad = yawPos[i]; + drive.yawBuffersAddSampleIndexAligned(t, yawTs, yawPos, i); + } else { + yawRad = drive.yawBufferSampleOr(t, imuInputs.yawPositionRad); + } + } + + // Coast state update IN REPLAY TIMEBASE + // Yaw rate: if you have a buffered rate, use it; otherwise imuInputs.yawRateRadPerSec is + // ok. + drive.updateDisabledCoastState( + DriverStation.isEnabled(), + DriverStation.isDisabled(), + t, + imuInputs.yawRateRadPerSec, + odomPositions); + + // Debugging + Logger.recordOutput("Odometry/Debug/timestamp", t); + Logger.recordOutput("Odometry/Debug/now", TimeUtil.now()); + if (i > 0) { + Logger.recordOutput("Odometry/Debug/timeNowDiff", t - ts[i - 1]); + } + Logger.recordOutput("Odometry/Debug/replay_t", t); + Logger.recordOutput("Odometry/Debug/replay_yawRad", yawRad); + + // Module distance deltas (valid within batch) + for (int m = 0; m < 4; m++) { + final SwerveModulePosition pos = odomPositions[m]; + final double dist = pos.distanceMeters; + + Logger.recordOutput("Odometry/Debug/mod" + m + "_distanceMeters", dist); + Logger.recordOutput("Odometry/Debug/mod" + m + "_angleRad", pos.angle.getRadians()); + + if (haveLastDist) { + final double delta = dist - lastDist[m]; + Logger.recordOutput("Odometry/Debug/mod" + m + "_deltaMeters", delta); + } + + lastDist[m] = dist; + } + haveLastDist = true; + + // Feed estimator at this historical timestamp + drive.poseEstimatorUpdateWithTime(t, Rotation2d.fromRadians(yawRad), odomPositions); + + // Maintain pose history in SAME timebase as estimator + drive.poseBufferAddSample(t, drive.getPose()); + } + + drive.setGyroDisconnectedAlert(!imuInputs.connected); + + } finally { + + Logger.recordOutput("Odometry/Robot", drive.getPose()); + + Drive.odometryLock.unlock(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 8e9c1132..10055776 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -13,7 +13,6 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog public static class ModuleIOInputs { public boolean driveConnected = false; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 5c68481c..9ae8105b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -56,6 +56,7 @@ import frc.robot.util.PhoenixUtil; import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.SparkUtil; +import java.util.Arrays; import java.util.Queue; import org.littletonrobotics.junction.Logger; @@ -73,6 +74,9 @@ public class ModuleIOBlended implements ModuleIO { TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; + // This module number (for logging) + private final int module; + // Hardware Objects private final TalonFX driveTalon; private final SparkBase turnSpark; @@ -134,6 +138,8 @@ public class ModuleIOBlended implements ModuleIO { * added in appropriately. */ public ModuleIOBlended(int module) { + // Record the module number for logging purposes + this.module = module; constants = switch (module) { @@ -243,7 +249,8 @@ public ModuleIOBlended(int module) { SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward - .kV(0.0); + .kV(0.0) + .kS(DrivebaseConstants.kSteerS); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) @@ -311,21 +318,28 @@ public ModuleIOBlended(int module) { @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh all signals + // Refresh signals var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + if (!driveStatus.isOK()) { + Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString()); + } + if (!encStatus.isOK()) { + Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString()); + } - // Update drive inputs + // Drive inputs inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - // Update turn inputs + // Turn inputs (Spark for turn motor, CANcoder for absolute) SparkUtil.sparkStickyFault = false; - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); @@ -333,20 +347,43 @@ public void updateInputs(ModuleIOInputs inputs) { turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); inputs.turnConnected = turnConnectedDebounce.calculate(!SparkUtil.sparkStickyFault); - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); + // Odometry queue drain (common prefix only) + final int tsCount = timestampQueue.size(); + final int driveCount = drivePositionQueue.size(); + final int turnCount = turnPositionQueue.size(); + final int sampleCount = Math.min(tsCount, Math.min(driveCount, turnCount)); + + if (sampleCount <= 0) { + inputs.odometryTimestamps = new double[0]; + inputs.odometryDrivePositionsRad = new double[0]; + inputs.odometryTurnPositions = new Rotation2d[0]; + return; + } + + final double[] outTs = new double[sampleCount]; + final double[] outDriveRad = new double[sampleCount]; + final Rotation2d[] outTurn = new Rotation2d[sampleCount]; + + for (int i = 0; i < sampleCount; i++) { + final Double t = timestampQueue.poll(); + final Double driveRot = drivePositionQueue.poll(); + final Double turnRot = turnPositionQueue.poll(); + + if (t == null || driveRot == null || turnRot == null) { + inputs.odometryTimestamps = Arrays.copyOf(outTs, i); + inputs.odometryDrivePositionsRad = Arrays.copyOf(outDriveRad, i); + inputs.odometryTurnPositions = Arrays.copyOf(outTurn, i); + return; + } + + outTs[i] = t.doubleValue(); + outDriveRad[i] = Units.rotationsToRadians(driveRot.doubleValue()); + outTurn[i] = Rotation2d.fromRotations(turnRot.doubleValue()); + } + + inputs.odometryTimestamps = outTs; + inputs.odometryDrivePositionsRad = outDriveRad; + inputs.odometryTurnPositions = outTurn; } /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 8fb64fe4..ec87f59e 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -15,9 +15,9 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants; +import frc.robot.util.TimeUtil; /** * Physics sim implementation of module IO. The sim models are configured using a set of module @@ -116,7 +116,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); // Odometry (single sample per loop is fine) - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryTimestamps = new double[] {TimeUtil.now()}; inputs.odometryDrivePositionsRad = new double[] {mechanismPositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index 75738618..c79d5ee9 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -32,6 +32,7 @@ import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.SparkUtil; +import java.util.Arrays; import java.util.Queue; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -183,7 +184,8 @@ public ModuleIOSpark(int module) { SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward - .kV(0.0); + .kV(0.0) + .kS(DrivebaseConstants.kSteerS); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) @@ -213,7 +215,7 @@ public ModuleIOSpark(int module) { @Override public void updateInputs(ModuleIOInputs inputs) { - // Update drive inputs + // Drive inputs SparkUtil.sparkStickyFault = false; SparkUtil.ifOk( driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); @@ -227,7 +229,7 @@ public void updateInputs(ModuleIOInputs inputs) { driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); inputs.driveConnected = driveConnectedDebounce.calculate(!SparkUtil.sparkStickyFault); - // Update turn inputs + // Turn inputs SparkUtil.sparkStickyFault = false; SparkUtil.ifOk( turnSpark, @@ -243,18 +245,47 @@ public void updateInputs(ModuleIOInputs inputs) { turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); inputs.turnConnected = turnConnectedDebounce.calculate(!SparkUtil.sparkStickyFault); - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); + // If you don’t have an absolute encoder on this variant, keep these consistent: + inputs.turnEncoderConnected = true; // or a real debounce if you have one + inputs.turnAbsolutePosition = inputs.turnPosition; + + // Odometry queue drain (common prefix only) + final int tsCount = timestampQueue.size(); + final int driveCount = drivePositionQueue.size(); + final int turnCount = turnPositionQueue.size(); + final int sampleCount = Math.min(tsCount, Math.min(driveCount, turnCount)); + + if (sampleCount <= 0) { + inputs.odometryTimestamps = new double[0]; + inputs.odometryDrivePositionsRad = new double[0]; + inputs.odometryTurnPositions = new Rotation2d[0]; + return; + } + + final double[] outTs = new double[sampleCount]; + final double[] outDriveRad = new double[sampleCount]; + final Rotation2d[] outTurn = new Rotation2d[sampleCount]; + + for (int i = 0; i < sampleCount; i++) { + final Double t = timestampQueue.poll(); + final Double drivePosRad = drivePositionQueue.poll(); // already rad in your existing code + final Double turnPosRad = turnPositionQueue.poll(); // rad in your existing code + + if (t == null || drivePosRad == null || turnPosRad == null) { + inputs.odometryTimestamps = Arrays.copyOf(outTs, i); + inputs.odometryDrivePositionsRad = Arrays.copyOf(outDriveRad, i); + inputs.odometryTurnPositions = Arrays.copyOf(outTurn, i); + return; + } + + outTs[i] = t.doubleValue(); + outDriveRad[i] = drivePosRad.doubleValue(); + outTurn[i] = new Rotation2d(turnPosRad.doubleValue()).minus(zeroRotation); + } + + inputs.odometryTimestamps = outTs; + inputs.odometryDrivePositionsRad = outDriveRad; + inputs.odometryTurnPositions = outTurn; } /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index fc76b593..7c106838 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -38,6 +38,7 @@ import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.SparkUtil; +import java.util.Arrays; import java.util.Queue; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -47,6 +48,10 @@ * and duty cycle absolute encoder. */ public class ModuleIOSparkCANcoder implements ModuleIO { + + // This module number (for logging) + private final int module; + private final Rotation2d zeroRotation; // Hardware objects @@ -87,6 +92,9 @@ public class ModuleIOSparkCANcoder implements ModuleIO { * Spark I/O w/ CANcoders */ public ModuleIOSparkCANcoder(int module) { + // Record the module number for logging purposes + this.module = module; + zeroRotation = switch (module) { case 0 -> new Rotation2d(SwerveConstants.kFLEncoderOffset); @@ -202,7 +210,8 @@ public ModuleIOSparkCANcoder(int module) { SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward - .kV(0.0); + .kV(0.0) + .kS(DrivebaseConstants.kSteerS); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) @@ -236,11 +245,13 @@ public ModuleIOSparkCANcoder(int module) { @Override public void updateInputs(ModuleIOInputs inputs) { + // Refresh CANcoder absolute + var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + if (!encStatus.isOK()) { + Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString()); + } - // Refresh all signals - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - - // Update drive inputs + // Drive inputs (Spark) SparkUtil.sparkStickyFault = false; SparkUtil.ifOk( driveSpark, driveEncoder::getPosition, (value) -> inputs.drivePositionRad = value); @@ -254,9 +265,9 @@ public void updateInputs(ModuleIOInputs inputs) { driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value); inputs.driveConnected = driveConnectedDebounce.calculate(!SparkUtil.sparkStickyFault); - // Update turn inputs + // Turn inputs (CANcoder abs + Spark velocity/applied/current) SparkUtil.sparkStickyFault = false; - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); @@ -268,18 +279,43 @@ public void updateInputs(ModuleIOInputs inputs) { turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value); inputs.turnConnected = turnConnectedDebounce.calculate(!SparkUtil.sparkStickyFault); - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> new Rotation2d(value).minus(zeroRotation)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); + // Odometry queue drain (common prefix only) + final int tsCount = timestampQueue.size(); + final int driveCount = drivePositionQueue.size(); + final int turnCount = turnPositionQueue.size(); + final int sampleCount = Math.min(tsCount, Math.min(driveCount, turnCount)); + + if (sampleCount <= 0) { + inputs.odometryTimestamps = new double[0]; + inputs.odometryDrivePositionsRad = new double[0]; + inputs.odometryTurnPositions = new Rotation2d[0]; + return; + } + + final double[] outTs = new double[sampleCount]; + final double[] outDriveRad = new double[sampleCount]; + final Rotation2d[] outTurn = new Rotation2d[sampleCount]; + + for (int i = 0; i < sampleCount; i++) { + final Double t = timestampQueue.poll(); + final Double drivePosRad = drivePositionQueue.poll(); // already rad in your existing code + final Double turnPosRad = turnPositionQueue.poll(); // rad, then minus zeroRotation below + + if (t == null || drivePosRad == null || turnPosRad == null) { + inputs.odometryTimestamps = Arrays.copyOf(outTs, i); + inputs.odometryDrivePositionsRad = Arrays.copyOf(outDriveRad, i); + inputs.odometryTurnPositions = Arrays.copyOf(outTurn, i); + return; + } + + outTs[i] = t.doubleValue(); + outDriveRad[i] = drivePosRad.doubleValue(); + outTurn[i] = new Rotation2d(turnPosRad.doubleValue()).minus(zeroRotation); + } + + inputs.odometryTimestamps = outTs; + inputs.odometryDrivePositionsRad = outDriveRad; + inputs.odometryTurnPositions = outTurn; } /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 65da2c71..ccf9c270 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -192,7 +192,7 @@ public ModuleIOTalonFX(int module) { .withKP(DrivebaseConstants.kSteerP) .withKI(0.0) .withKD(DrivebaseConstants.kSteerD) - .withKS(0.0) + .withKS(DrivebaseConstants.kSteerS) .withKV(0.0) .withKA(0.0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); @@ -265,14 +265,14 @@ public ModuleIOTalonFX(int module) { @Override public void updateInputs(ModuleIOInputs inputs) { - // -------------------- Refresh Phoenix signals -------------------- + // Refresh Phoenix signals var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - // Log refresh failures explicitly (debug gold) + // Log *which* groups are failing and what the code is if (!driveStatus.isOK()) { Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString()); } @@ -283,29 +283,30 @@ public void updateInputs(ModuleIOInputs inputs) { Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString()); } - // -------------------- Connectivity flags -------------------- + // Connectivity flags inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); - // -------------------- Instantaneous state -------------------- + // Update drive inputs inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + // Update turn inputs inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - // -------------------- Odometry queue drain -------------------- + // Odometry queue drain final int tsCount = timestampQueue.size(); final int driveCount = drivePositionQueue.size(); final int turnCount = turnPositionQueue.size(); - // Only consume the common prefix — guarantees alignment + // Only consume the common prefix -- guarantees alignment final int sampleCount = Math.min(tsCount, Math.min(driveCount, turnCount)); if (sampleCount <= 0) { @@ -362,7 +363,7 @@ public void setDriveOpenLoop(double output) { // Log output and battery Logger.recordOutput("Swerve/Drive/OpenLoopOutput", scaledOutput); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); + Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); } /** @@ -383,7 +384,7 @@ public void setTurnOpenLoop(double output) { // Log output and battery Logger.recordOutput("Swerve/Turn/OpenLoopOutput", scaledOutput); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); + Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); } /** @@ -426,7 +427,7 @@ public void setDriveVelocity(double velocityRadPerSec) { Logger.recordOutput("Swerve/Drive/VelocityRotPerSec", velocityRotPerSec); Logger.recordOutput("Swerve/Drive/AccelerationRotPerSec2", accelerationRotPerSec2); Logger.recordOutput("Swerve/Drive/FeedForwardVolts", scaledFFVolts); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); + Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); Logger.recordOutput("Swerve/Drive/ClosedLoopMode", m_DriveMotorClosedLoopOutput); } @@ -454,7 +455,7 @@ public void setTurnPosition(Rotation2d rotation) { }); Logger.recordOutput("Swerve/Turn/TargetRotations", rotation.getRotations()); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); + Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 23f2885c..6226ed2f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -7,438 +7,507 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot.subsystems.drive; - -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANdiConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXSConfiguration; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANdi; -import com.ctre.phoenix6.hardware.ParentDevice; -import com.ctre.phoenix6.hardware.TalonFXS; -import com.ctre.phoenix6.signals.BrushedMotorWiringValue; -import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorArrangementValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; -import frc.robot.Constants; -import frc.robot.Constants.DrivebaseConstants; -import frc.robot.util.PhoenixUtil; -import frc.robot.util.RBSICANBusRegistry; -import java.util.Queue; -import org.littletonrobotics.junction.Logger; - -/** - * Module IO implementation for Talon FXS drive motor controller, Talon FXS turn motor controller, - * and CANdi (PWM 1). Configured using a set of module constants from Phoenix. - * - *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. - */ -public class ModuleIOTalonFXS implements ModuleIO { - // Hardware objects - private final TalonFXS driveTalon; - private final TalonFXS turnTalon; - private final CANdi candi; - private final ClosedLoopOutputType m_DriveMotorClosedLoopOutput = - switch (Constants.getPhoenixPro()) { - case LICENSED -> ClosedLoopOutputType.TorqueCurrentFOC; - case UNLICENSED -> ClosedLoopOutputType.Voltage; - }; - private final ClosedLoopOutputType m_SteerMotorClosedLoopOutput = - switch (Constants.getPhoenixPro()) { - case LICENSED -> ClosedLoopOutputType.TorqueCurrentFOC; - case UNLICENSED -> ClosedLoopOutputType.Voltage; - }; - - // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); - private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = - new PositionTorqueCurrentFOC(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); - - // Timestamp inputs from Phoenix thread - private final Queue timestampQueue; - - // Inputs from drive motor - private final StatusSignal drivePosition; - private final StatusSignal drivePositionOdom; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - // Inputs from turn motor - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnPositionOdom; - private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Connection debouncers - private final Debouncer driveConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private final Debouncer turnConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private final Debouncer turnEncoderConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - // Config - private final TalonFXSConfiguration driveConfig = new TalonFXSConfiguration(); - private final TalonFXSConfiguration turnConfig = new TalonFXSConfiguration(); - - // Values used for calculating feedforward from kS, kV, and kA - private double lastVelocityRotPerSec = 0.0; - private long lastTimestampNano = System.nanoTime(); - - /* - * TalonFXS I/O - */ - public ModuleIOTalonFXS( - SwerveModuleConstants - constants) { - CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName); - driveTalon = new TalonFXS(constants.DriveMotorId, canBus); - turnTalon = new TalonFXS(constants.SteerMotorId, canBus); - candi = new CANdi(constants.EncoderId, canBus); - - // Configure drive motor - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Slot0 = - new Slot0Configs() - .withKP(DrivebaseConstants.kDriveP) - .withKI(0.0) - .withKD(DrivebaseConstants.kDriveD) - .withKS(DrivebaseConstants.kDriveS) - .withKV(DrivebaseConstants.kDriveV) - .withKA(DrivebaseConstants.kDriveA); - driveConfig.Commutation.MotorArrangement = - switch (constants.DriveMotorType) { - case TalonFXS_NEO_JST -> MotorArrangementValue.NEO_JST; - case TalonFXS_VORTEX_JST -> MotorArrangementValue.VORTEX_JST; - default -> MotorArrangementValue.Disabled; - }; - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.ExternalFeedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrent; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing - OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs(); - openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod; - openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod; - openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod; - ClosedLoopRampsConfigs closedRamps = new ClosedLoopRampsConfigs(); - closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod; - closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod; - closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod; - // Apply the open- and closed-loop ramp configuration for current smoothing - driveConfig.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps); - // Set motor inversions - driveConfig.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - // Apply everything to the motor controllers - PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - - // Configure turn motor - turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turnConfig.Slot0 = - new Slot0Configs() - .withKP(DrivebaseConstants.kSteerP) - .withKI(0.0) - .withKD(DrivebaseConstants.kSteerD) - .withKS(0.0) - .withKV(0.0) - .withKA(0.0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - turnConfig.Commutation.MotorArrangement = - switch (constants.SteerMotorType) { - case TalonFXS_Minion_JST -> MotorArrangementValue.Minion_JST; - case TalonFXS_NEO_JST -> MotorArrangementValue.NEO_JST; - case TalonFXS_VORTEX_JST -> MotorArrangementValue.VORTEX_JST; - case TalonFXS_NEO550_JST -> MotorArrangementValue.NEO550_JST; - case TalonFXS_Brushed_AB, TalonFXS_Brushed_AC, TalonFXS_Brushed_BC -> - MotorArrangementValue.Brushed_DC; - default -> MotorArrangementValue.Disabled; - }; - turnConfig.Commutation.BrushedMotorWiring = - switch (constants.SteerMotorType) { - case TalonFXS_Brushed_AC -> BrushedMotorWiringValue.Leads_A_and_C; - case TalonFXS_Brushed_BC -> BrushedMotorWiringValue.Leads_B_and_C; - default -> BrushedMotorWiringValue.Leads_A_and_B; - }; - turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turnConfig.Slot0 = constants.SteerMotorGains; - turnConfig.ExternalFeedback.FeedbackRemoteSensorID = constants.EncoderId; - turnConfig.ExternalFeedback.ExternalFeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANdiPWM1 -> ExternalFeedbackSensorSourceValue.RemoteCANdiPWM1; - case FusedCANdiPWM1 -> ExternalFeedbackSensorSourceValue.FusedCANdiPWM1; - case SyncCANdiPWM1 -> ExternalFeedbackSensorSourceValue.SyncCANdiPWM1; - default -> - throw new RuntimeException( - "You have selected a turn feedback source that is not supported by the default implementation of ModuleIOTalonFXS (CANdi PWM 1). Please check the AdvantageKit documentation for more information on alternative configurations: https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#custom-module-implementations"); - }; - turnConfig.ExternalFeedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicAcceleration = - turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; - turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - turnConfig.MotorOutput.Inverted = - constants.SteerMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - PhoenixUtil.tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); - - // Configure CANdi - CANdiConfiguration candiConfig = constants.EncoderInitialConfigs; - candiConfig.PWM1.AbsoluteSensorOffset = constants.EncoderOffset; - candiConfig.PWM1.SensorDirection = constants.EncoderInverted; - candi.getConfigurator().apply(candiConfig); - - // Create timestamp queue - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - // Create drive status signals - drivePosition = driveTalon.getPosition(); - drivePositionOdom = drivePosition.clone(); // NEW - drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom); - - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - - // Create turn status signals - turnPosition = turnTalon.getPosition(); - turnPositionOdom = turnPosition.clone(); // NEW - turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPositionOdom); - - turnAbsolutePosition = candi.getPWM1Position(); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - - // Configure periodic frames (IMPORTANT: apply odometry rate to the *odom clones*) - BaseStatusSignal.setUpdateFrequencyForAll( - SwerveConstants.kOdometryFrequency, drivePositionOdom, turnPositionOdom); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - drivePosition, - turnPosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - // Refresh all signals - var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - - // Update drive inputs - inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - - // Update turn inputs - inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value)) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - /** - * Set the drive motor to an open-loop voltage, scaled to battery voltage - * - * @param output Specified open-loop voltage requested - */ - @Override - public void setDriveOpenLoop(double output) { - // Scale by actual battery voltage to keep full output consistent - double busVoltage = RobotController.getBatteryVoltage(); - double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage; - - driveTalon.setControl( - switch (m_DriveMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(scaledOutput); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput); - }); - - // Log output and battery - Logger.recordOutput("Swerve/Drive/OpenLoopOutput", scaledOutput); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); - } - - /** - * Set the turn motor to an open-loop voltage, scaled to battery voltage - * - * @param output Specified open-loop voltage requested - */ - @Override - public void setTurnOpenLoop(double output) { - double busVoltage = RobotController.getBatteryVoltage(); - double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage; - - turnTalon.setControl( - switch (m_SteerMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(scaledOutput); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput); - }); - - Logger.recordOutput("Swerve/Turn/OpenLoopOutput", scaledOutput); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); - } - - /** - * Set the velocity of the module - * - * @param velocityRadPerSec Requested module drive velocity in radians per second - */ - @Override - public void setDriveVelocity(double velocityRadPerSec) { - double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); - double busVoltage = RobotController.getBatteryVoltage(); - - // Compute the Feedforward voltage for CTRE UNLICENSED operation ***** - // Compute acceleration - long currentTimeNano = System.nanoTime(); - double deltaTimeSec = (currentTimeNano - lastTimestampNano) * 1e-9; - double accelerationRotPerSec2 = - deltaTimeSec > 0 ? (velocityRotPerSec - lastVelocityRotPerSec) / deltaTimeSec : 0.0; - // Update last values for next loop - lastVelocityRotPerSec = velocityRotPerSec; - lastTimestampNano = currentTimeNano; - // Compute feedforward voltage: kS + kV*v + kA*a - double nominalFFVolts = - Math.signum(velocityRotPerSec) * DrivebaseConstants.kDriveS - + DrivebaseConstants.kDriveV * velocityRotPerSec - + DrivebaseConstants.kDriveA * accelerationRotPerSec2; - double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage; - - // Set the drive motor control based on CTRE LICENSED status - driveTalon.setControl( - switch (m_DriveMotorClosedLoopOutput) { - case Voltage -> - velocityVoltageRequest.withVelocity(velocityRotPerSec).withFeedForward(scaledFFVolts); - case TorqueCurrentFOC -> - velocityTorqueCurrentRequest.withVelocity(RotationsPerSecond.of(velocityRotPerSec)); - }); - - // AdvantageKit logging - Logger.recordOutput("Swerve/Drive/VelocityRadPerSec", velocityRadPerSec); - Logger.recordOutput("Swerve/Drive/VelocityRotPerSec", velocityRotPerSec); - Logger.recordOutput("Swerve/Drive/AccelerationRotPerSec2", accelerationRotPerSec2); - Logger.recordOutput("Swerve/Drive/FeedForwardVolts", scaledFFVolts); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); - Logger.recordOutput("Swerve/Drive/ClosedLoopMode", m_DriveMotorClosedLoopOutput); - } - - /** - * Set the turn position of the module - * - * @param rotation Requested module Rotation2d position - */ - @Override - public void setTurnPosition(Rotation2d rotation) { - double busVoltage = RobotController.getBatteryVoltage(); - - // Scale feedforward voltage by battery voltage - double nominalFFVolts = - DrivebaseConstants.kNominalFFVolts; // replace with your feedforward calculation if needed - double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage; - - turnTalon.setControl( - switch (m_SteerMotorClosedLoopOutput) { - case Voltage -> - positionVoltageRequest - .withPosition(rotation.getRotations()) - .withFeedForward(scaledFFVolts); - case TorqueCurrentFOC -> - positionTorqueCurrentRequest.withPosition(rotation.getRotations()); - }); - - Logger.recordOutput("Swerve/Turn/TargetRotations", rotation.getRotations()); - Logger.recordOutput("Robot/BatteryVoltage", busVoltage); - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - driveConfig.Slot0.kP = kP; - driveConfig.Slot0.kI = kI; - driveConfig.Slot0.kD = kD; - PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - turnConfig.Slot0.kP = kP; - turnConfig.Slot0.kI = kI; - turnConfig.Slot0.kD = kD; - PhoenixUtil.tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); - } -} +// IMPORTANT: UNCOMMENT THIS MODULE IF YOU ARE USING FXS instead of FX +// COMMENT OUT the FX module + +// package frc.robot.subsystems.drive; + +// import static edu.wpi.first.units.Units.RotationsPerSecond; + +// import com.ctre.phoenix6.BaseStatusSignal; +// import com.ctre.phoenix6.CANBus; +// import com.ctre.phoenix6.StatusSignal; +// import com.ctre.phoenix6.configs.CANdiConfiguration; +// import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +// import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; +// import com.ctre.phoenix6.configs.Slot0Configs; +// import com.ctre.phoenix6.configs.TalonFXConfiguration; +// import com.ctre.phoenix6.configs.TalonFXSConfiguration; +// import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +// import com.ctre.phoenix6.controls.PositionVoltage; +// import com.ctre.phoenix6.controls.TorqueCurrentFOC; +// import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +// import com.ctre.phoenix6.controls.VelocityVoltage; +// import com.ctre.phoenix6.controls.VoltageOut; +// import com.ctre.phoenix6.hardware.CANdi; +// import com.ctre.phoenix6.hardware.ParentDevice; +// import com.ctre.phoenix6.hardware.TalonFXS; +// import com.ctre.phoenix6.signals.BrushedMotorWiringValue; +// import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; +// import com.ctre.phoenix6.signals.InvertedValue; +// import com.ctre.phoenix6.signals.MotorArrangementValue; +// import com.ctre.phoenix6.signals.NeutralModeValue; +// import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +// import com.ctre.phoenix6.swerve.SwerveModuleConstants; +// import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +// import edu.wpi.first.math.filter.Debouncer; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.units.measure.Angle; +// import edu.wpi.first.units.measure.AngularVelocity; +// import edu.wpi.first.units.measure.Current; +// import edu.wpi.first.units.measure.Voltage; +// import edu.wpi.first.wpilibj.RobotController; +// import frc.robot.Constants; +// import frc.robot.Constants.DrivebaseConstants; +// import frc.robot.generated.TunerConstants; +// import frc.robot.util.PhoenixUtil; +// import frc.robot.util.RBSICANBusRegistry; +// import java.util.Arrays; +// import java.util.Queue; +// import org.littletonrobotics.junction.Logger; + +// /** +// * Module IO implementation for Talon FXS drive motor controller, Talon FXS turn motor +// controller, +// * and CANdi (PWM 1). Configured using a set of module constants from Phoenix. +// * +// *

Device configuration and other behaviors not exposed by TunerConstants can be customized +// here. +// */ +// public class ModuleIOTalonFXS implements ModuleIO { +// private final SwerveModuleConstants< +// TalonFXConfiguration, TalonFXConfiguration, CANdiConfiguration> +// constants; + +// // This module number (for logging) +// private final int module; + +// // Hardware objects +// private final TalonFXS driveTalon; +// private final TalonFXS turnTalon; +// private final CANdi candi; +// private final ClosedLoopOutputType m_DriveMotorClosedLoopOutput = +// switch (Constants.getPhoenixPro()) { +// case LICENSED -> ClosedLoopOutputType.TorqueCurrentFOC; +// case UNLICENSED -> ClosedLoopOutputType.Voltage; +// }; +// private final ClosedLoopOutputType m_SteerMotorClosedLoopOutput = +// switch (Constants.getPhoenixPro()) { +// case LICENSED -> ClosedLoopOutputType.TorqueCurrentFOC; +// case UNLICENSED -> ClosedLoopOutputType.Voltage; +// }; + +// // Voltage control requests +// private final VoltageOut voltageRequest = new VoltageOut(0); +// private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); +// private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + +// // Torque-current control requests +// private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); +// private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = +// new PositionTorqueCurrentFOC(0.0); +// private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = +// new VelocityTorqueCurrentFOC(0.0); + +// // Timestamp inputs from Phoenix thread +// private final Queue timestampQueue; + +// // Inputs from drive motor +// private final StatusSignal drivePosition; +// private final StatusSignal drivePositionOdom; +// private final Queue drivePositionQueue; +// private final StatusSignal driveVelocity; +// private final StatusSignal driveAppliedVolts; +// private final StatusSignal driveCurrent; + +// // Inputs from turn motor +// private final StatusSignal turnAbsolutePosition; +// private final StatusSignal turnPosition; +// private final StatusSignal turnPositionOdom; +// private final Queue turnPositionQueue; +// private final StatusSignal turnVelocity; +// private final StatusSignal turnAppliedVolts; +// private final StatusSignal turnCurrent; + +// // Connection debouncers +// private final Debouncer driveConnectedDebounce = +// new Debouncer(0.5, Debouncer.DebounceType.kFalling); +// private final Debouncer turnConnectedDebounce = +// new Debouncer(0.5, Debouncer.DebounceType.kFalling); +// private final Debouncer turnEncoderConnectedDebounce = +// new Debouncer(0.5, Debouncer.DebounceType.kFalling); + +// // Config +// private final TalonFXSConfiguration driveConfig = new TalonFXSConfiguration(); +// private final TalonFXSConfiguration turnConfig = new TalonFXSConfiguration(); + +// // Values used for calculating feedforward from kS, kV, and kA +// private double lastVelocityRotPerSec = 0.0; +// private long lastTimestampNano = System.nanoTime(); + +// /* +// * TalonFXS I/O +// */ +// public ModuleIOTalonFXS(int module) { +// // Record the module number for logging purposes +// this.module = module; + +// constants = +// switch (module) { +// case 0 -> TunerConstants.FrontLeft; +// case 1 -> TunerConstants.FrontRight; +// case 2 -> TunerConstants.BackLeft; +// case 3 -> TunerConstants.BackRight; +// default -> throw new IllegalArgumentException("Invalid module index"); +// }; + +// CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName); +// driveTalon = new TalonFXS(constants.DriveMotorId, canBus); +// turnTalon = new TalonFXS(constants.SteerMotorId, canBus); +// candi = new CANdi(constants.EncoderId, canBus); + +// // Configure drive motor +// driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; +// driveConfig.Slot0 = +// new Slot0Configs() +// .withKP(DrivebaseConstants.kDriveP) +// .withKI(0.0) +// .withKD(DrivebaseConstants.kDriveD) +// .withKS(DrivebaseConstants.kDriveS) +// .withKV(DrivebaseConstants.kDriveV) +// .withKA(DrivebaseConstants.kDriveA); +// driveConfig.Commutation.MotorArrangement = +// switch (constants.DriveMotorType) { +// case TalonFXS_NEO_JST -> MotorArrangementValue.NEO_JST; +// case TalonFXS_VORTEX_JST -> MotorArrangementValue.VORTEX_JST; +// default -> MotorArrangementValue.Disabled; +// }; +// driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; +// driveConfig.Slot0 = constants.DriveMotorGains; +// driveConfig.ExternalFeedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; +// driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrent; +// driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; +// // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing +// OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs(); +// openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod; +// openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod; +// openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod; +// ClosedLoopRampsConfigs closedRamps = new ClosedLoopRampsConfigs(); +// closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod; +// closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod; +// closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod; +// // Apply the open- and closed-loop ramp configuration for current smoothing +// driveConfig.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps); +// // Set motor inversions +// driveConfig.MotorOutput.Inverted = +// constants.DriveMotorInverted +// ? InvertedValue.Clockwise_Positive +// : InvertedValue.CounterClockwise_Positive; +// // Apply everything to the motor controllers +// PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); +// PhoenixUtil.tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + +// // Configure turn motor +// turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; +// turnConfig.Slot0 = +// new Slot0Configs() +// .withKP(DrivebaseConstants.kSteerP) +// .withKI(0.0) +// .withKD(DrivebaseConstants.kSteerD) +// .withKS(DrivebaseConstants.kSteerS) +// .withKV(0.0) +// .withKA(0.0) +// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); +// turnConfig.Commutation.MotorArrangement = +// switch (constants.SteerMotorType) { +// case TalonFXS_Minion_JST -> MotorArrangementValue.Minion_JST; +// case TalonFXS_NEO_JST -> MotorArrangementValue.NEO_JST; +// case TalonFXS_VORTEX_JST -> MotorArrangementValue.VORTEX_JST; +// case TalonFXS_NEO550_JST -> MotorArrangementValue.NEO550_JST; +// case TalonFXS_Brushed_AB, TalonFXS_Brushed_AC, TalonFXS_Brushed_BC -> +// MotorArrangementValue.Brushed_DC; +// default -> MotorArrangementValue.Disabled; +// }; +// turnConfig.Commutation.BrushedMotorWiring = +// switch (constants.SteerMotorType) { +// case TalonFXS_Brushed_AC -> BrushedMotorWiringValue.Leads_A_and_C; +// case TalonFXS_Brushed_BC -> BrushedMotorWiringValue.Leads_B_and_C; +// default -> BrushedMotorWiringValue.Leads_A_and_B; +// }; +// turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; +// turnConfig.Slot0 = constants.SteerMotorGains; +// turnConfig.ExternalFeedback.FeedbackRemoteSensorID = constants.EncoderId; +// turnConfig.ExternalFeedback.ExternalFeedbackSensorSource = +// switch (constants.FeedbackSource) { +// case RemoteCANdiPWM1 -> ExternalFeedbackSensorSourceValue.RemoteCANdiPWM1; +// case FusedCANdiPWM1 -> ExternalFeedbackSensorSourceValue.FusedCANdiPWM1; +// case SyncCANdiPWM1 -> ExternalFeedbackSensorSourceValue.SyncCANdiPWM1; +// default -> +// throw new RuntimeException( +// "You have selected a turn feedback source that is not supported by the default +// implementation of ModuleIOTalonFXS (CANdi PWM 1). Please check the AdvantageKit documentation for +// more information on alternative configurations: +// https://docs.advantagekit.org/getting-started/template-projects/talonfx-swerve-template#custom-module-implementations"); +// }; +// turnConfig.ExternalFeedback.RotorToSensorRatio = constants.SteerMotorGearRatio; +// turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; +// turnConfig.MotionMagic.MotionMagicAcceleration = +// turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; +// turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; +// turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; +// turnConfig.ClosedLoopGeneral.ContinuousWrap = true; +// turnConfig.MotorOutput.Inverted = +// constants.SteerMotorInverted +// ? InvertedValue.Clockwise_Positive +// : InvertedValue.CounterClockwise_Positive; +// PhoenixUtil.tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + +// // Configure CANdi +// CANdiConfiguration candiConfig = constants.EncoderInitialConfigs; +// candiConfig.PWM1.AbsoluteSensorOffset = constants.EncoderOffset; +// candiConfig.PWM1.SensorDirection = constants.EncoderInverted; +// candi.getConfigurator().apply(candiConfig); + +// // Create timestamp queue +// timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + +// // Create drive status signals +// drivePosition = driveTalon.getPosition(); +// drivePositionOdom = drivePosition.clone(); // NEW +// drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom); + +// driveVelocity = driveTalon.getVelocity(); +// driveAppliedVolts = driveTalon.getMotorVoltage(); +// driveCurrent = driveTalon.getStatorCurrent(); + +// // Create turn status signals +// turnPosition = turnTalon.getPosition(); +// turnPositionOdom = turnPosition.clone(); // NEW +// turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPositionOdom); + +// turnAbsolutePosition = candi.getPWM1Position(); +// turnVelocity = turnTalon.getVelocity(); +// turnAppliedVolts = turnTalon.getMotorVoltage(); +// turnCurrent = turnTalon.getStatorCurrent(); + +// // Configure periodic frames (IMPORTANT: apply odometry rate to the *odom clones*) +// BaseStatusSignal.setUpdateFrequencyForAll( +// SwerveConstants.kOdometryFrequency, drivePositionOdom, turnPositionOdom); +// BaseStatusSignal.setUpdateFrequencyForAll( +// 50.0, +// drivePosition, +// turnPosition, +// driveVelocity, +// driveAppliedVolts, +// driveCurrent, +// turnAbsolutePosition, +// turnVelocity, +// turnAppliedVolts, +// turnCurrent); + +// ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); +// } + +// @Override +// public void updateInputs(ModuleIOInputs inputs) { +// // Refresh Phoenix signals +// var driveStatus = +// BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, +// driveCurrent); +// var turnStatus = +// BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); +// var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + +// // Optional: status logging +// if (!driveStatus.isOK()) { +// Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString()); +// } +// if (!turnStatus.isOK()) { +// Logger.recordOutput("CAN/Module" + module + "/TurnRefreshStatus", turnStatus.toString()); +// } +// if (!encStatus.isOK()) { +// Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString()); +// } + +// // Connectivity flags +// inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); +// inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); +// inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); + +// // Drive inputs +// inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); +// inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); +// inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); +// inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + +// // Turn inputs +// inputs.turnAbsolutePosition = +// Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); +// inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); +// inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); +// inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); +// inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + +// // Odometry queue drain (common prefix only) +// final int tsCount = timestampQueue.size(); +// final int driveCount = drivePositionQueue.size(); +// final int turnCount = turnPositionQueue.size(); +// final int sampleCount = Math.min(tsCount, Math.min(driveCount, turnCount)); + +// if (sampleCount <= 0) { +// inputs.odometryTimestamps = new double[0]; +// inputs.odometryDrivePositionsRad = new double[0]; +// inputs.odometryTurnPositions = new Rotation2d[0]; +// return; +// } + +// final double[] outTs = new double[sampleCount]; +// final double[] outDriveRad = new double[sampleCount]; +// final Rotation2d[] outTurn = new Rotation2d[sampleCount]; + +// for (int i = 0; i < sampleCount; i++) { +// final Double t = timestampQueue.poll(); +// final Double driveRot = drivePositionQueue.poll(); +// final Double turnRot = turnPositionQueue.poll(); + +// if (t == null || driveRot == null || turnRot == null) { +// inputs.odometryTimestamps = Arrays.copyOf(outTs, i); +// inputs.odometryDrivePositionsRad = Arrays.copyOf(outDriveRad, i); +// inputs.odometryTurnPositions = Arrays.copyOf(outTurn, i); +// return; +// } + +// outTs[i] = t.doubleValue(); +// outDriveRad[i] = Units.rotationsToRadians(driveRot.doubleValue()); +// outTurn[i] = Rotation2d.fromRotations(turnRot.doubleValue()); +// } + +// inputs.odometryTimestamps = outTs; +// inputs.odometryDrivePositionsRad = outDriveRad; +// inputs.odometryTurnPositions = outTurn; +// } + +// /** Module Action Functions ********************************************** */ +// /** +// * Set the drive motor to an open-loop voltage, scaled to battery voltage +// * +// * @param output Specified open-loop voltage requested +// */ +// @Override +// public void setDriveOpenLoop(double output) { +// // Scale by actual battery voltage to keep full output consistent +// double busVoltage = RobotController.getBatteryVoltage(); +// double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage; + +// driveTalon.setControl( +// switch (m_DriveMotorClosedLoopOutput) { +// case Voltage -> voltageRequest.withOutput(scaledOutput); +// case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput); +// }); + +// // Log output and battery +// Logger.recordOutput("Swerve/Drive/OpenLoopOutput", scaledOutput); +// Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); +// } + +// /** +// * Set the turn motor to an open-loop voltage, scaled to battery voltage +// * +// * @param output Specified open-loop voltage requested +// */ +// @Override +// public void setTurnOpenLoop(double output) { +// double busVoltage = RobotController.getBatteryVoltage(); +// double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage; + +// turnTalon.setControl( +// switch (m_SteerMotorClosedLoopOutput) { +// case Voltage -> voltageRequest.withOutput(scaledOutput); +// case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput); +// }); + +// // Log output and battery +// Logger.recordOutput("Swerve/Turn/OpenLoopOutput", scaledOutput); +// Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); +// } + +// /** +// * Set the velocity of the module +// * +// * @param velocityRadPerSec Requested module drive velocity in radians per second +// */ +// @Override +// public void setDriveVelocity(double velocityRadPerSec) { +// double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); +// double busVoltage = RobotController.getBatteryVoltage(); + +// // Compute the Feedforward voltage for CTRE UNLICENSED operation ***** +// // Compute acceleration +// long currentTimeNano = System.nanoTime(); +// double deltaTimeSec = (currentTimeNano - lastTimestampNano) * 1e-9; +// double accelerationRotPerSec2 = +// deltaTimeSec > 0 ? (velocityRotPerSec - lastVelocityRotPerSec) / deltaTimeSec : 0.0; +// // Update last values for next loop +// lastVelocityRotPerSec = velocityRotPerSec; +// lastTimestampNano = currentTimeNano; +// // Compute feedforward voltage: kS + kV*v + kA*a +// double nominalFFVolts = +// Math.signum(velocityRotPerSec) * DrivebaseConstants.kDriveS +// + DrivebaseConstants.kDriveV * velocityRotPerSec +// + DrivebaseConstants.kDriveA * accelerationRotPerSec2; +// double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage; + +// // Set the drive motor control based on CTRE LICENSED status +// driveTalon.setControl( +// switch (m_DriveMotorClosedLoopOutput) { +// case Voltage -> +// +// velocityVoltageRequest.withVelocity(velocityRotPerSec).withFeedForward(scaledFFVolts); +// case TorqueCurrentFOC -> +// +// velocityTorqueCurrentRequest.withVelocity(RotationsPerSecond.of(velocityRotPerSec)); +// }); + +// // AdvantageKit logging +// Logger.recordOutput("Swerve/Drive/VelocityRadPerSec", velocityRadPerSec); +// Logger.recordOutput("Swerve/Drive/VelocityRotPerSec", velocityRotPerSec); +// Logger.recordOutput("Swerve/Drive/AccelerationRotPerSec2", accelerationRotPerSec2); +// Logger.recordOutput("Swerve/Drive/FeedForwardVolts", scaledFFVolts); +// Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); +// Logger.recordOutput("Swerve/Drive/ClosedLoopMode", m_DriveMotorClosedLoopOutput); +// } + +// /** +// * Set the turn position of the module +// * +// * @param rotation Requested module Rotation2d position +// */ +// @Override +// public void setTurnPosition(Rotation2d rotation) { +// double busVoltage = RobotController.getBatteryVoltage(); + +// // Scale feedforward voltage by battery voltage +// double nominalFFVolts = DrivebaseConstants.kNominalFFVolts; +// double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage; + +// turnTalon.setControl( +// switch (m_SteerMotorClosedLoopOutput) { +// case Voltage -> +// positionVoltageRequest +// .withPosition(rotation.getRotations()) +// .withFeedForward(scaledFFVolts); +// case TorqueCurrentFOC -> +// positionTorqueCurrentRequest.withPosition(rotation.getRotations()); +// }); + +// Logger.recordOutput("Swerve/Turn/TargetRotations", rotation.getRotations()); +// Logger.recordOutput("Swerve/BatteryVoltage", busVoltage); +// } + +// @Override +// public void setDrivePID(double kP, double kI, double kD) { +// driveConfig.Slot0.kP = kP; +// driveConfig.Slot0.kI = kI; +// driveConfig.Slot0.kD = kD; +// PhoenixUtil.tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); +// } + +// @Override +// public void setTurnPID(double kP, double kI, double kD) { +// turnConfig.Slot0.kP = kP; +// turnConfig.Slot0.kI = kI; +// turnConfig.Slot0.kD = kD; +// PhoenixUtil.tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index f5f63152..854f7182 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -22,51 +22,95 @@ import frc.robot.util.VirtualSubsystem; public class Imu extends VirtualSubsystem { + + // Declare the io and inputs private final ImuIO io; private final ImuIO.ImuIOInputs inputs = new ImuIO.ImuIOInputs(); - // Optional per-cycle cached objects (avoid repeated allocations) + // Per-cycle cached objects (to avoid repeated allocations) private long cacheStampNs = -1L; private Rotation2d cachedYaw = Rotation2d.kZero; private Translation3d cachedAccel = Translation3d.kZero; private Translation3d cachedJerk = Translation3d.kZero; - // Constructor + /** Constructor */ public Imu(ImuIO io) { this.io = io; } - // Periodic function to read inputs + /** + * Priority value for this virtual subsystem + * + *

See `frc.robot.util.VirtualSubsystem` for a description of the suggested values for various + * virtual subsystems. + */ + @Override + protected int getPeriodPriority() { + return -30; + } + + /** Periodic function to read inputs */ public void rbsiPeriodic() { io.updateInputs(inputs); } - /** Hot-path access: primitive-only snapshot */ + /** + * Get the inputs objects + * + *

Hot-path access: primitive-only snapshot + * + * @return The inputs objects + */ public ImuIO.ImuIOInputs getInputs() { return inputs; } - /** Readable boundary: Rotation2d (alloc/cached per timestamp) */ + /** + * Get the current YAW + * + *

This function updates the caches if needed. + * + * @return The current YAW as a Rotation2d object + */ public Rotation2d getYaw() { refreshCachesIfNeeded(); return cachedYaw; } - /** Readable boundary: Translation3d accel (alloc/cached per timestamp) */ + /** + * Get the current linear acceleration + * + *

This function updates the caches as needed. + * + * @return The current linear acceleration as a Translation3d object + */ public Translation3d getLinearAccel() { refreshCachesIfNeeded(); return cachedAccel; } + /** + * Get the current jerk + * + *

This function updates the caches ad needed. + * + * @return The current jerk as a Translation3d object + */ public Translation3d getJerk() { refreshCachesIfNeeded(); return cachedJerk; } + /** + * Zero the YAW to this input value + * + * @param yaw Input YAW + */ public void zeroYaw(Rotation2d yaw) { io.zeroYawRad(yaw.getRadians()); } + /** Refresh the caches from the inputs, if needed */ private void refreshCachesIfNeeded() { final long stamp = inputs.timestampNs; if (stamp == cacheStampNs) return; diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index bf350c4f..8ee521e4 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -24,9 +24,6 @@ /** * Single IMU interface exposing all relevant state: orientation, rates, linear accel, jerk, and * odometry samples. - * - *

Primitive-only core: NO WPILib geometry objects, NO Units objects. Conversions happen at the - * boundary in the Imu subsystem (wrapper methods). */ public interface ImuIO extends RBSIIO { @@ -34,28 +31,21 @@ public interface ImuIO extends RBSIIO { class ImuIOInputs { public boolean connected = false; - /** FPGA-local timestamp when inputs were captured (ns) */ + // FPGA-local timestamp when inputs were captured (ns) public long timestampNs = 0L; - - /** Yaw angle (robot frame) in radians */ + // Yaw angle (robot frame) in radians public double yawPositionRad = 0.0; - - /** Yaw angular rate in radians/sec */ + // Yaw angular rate in radians/sec public double yawRateRadPerSec = 0.0; - - /** Linear acceleration in robot frame (m/s^2) */ + // Linear acceleration in robot frame (m/s^2) public Translation3d linearAccel = Translation3d.kZero; - - /** Linear jerk in robot frame (m/s^3) */ + // Linear jerk in robot frame (m/s^3) public Translation3d linearJerk = Translation3d.kZero; - - /** Time spent in the IO update call (seconds) */ + // Time spent in the IO update call (seconds) public double latencySeconds = 0.0; - - /** Optional odometry samples (timestamps in seconds) */ + // Odometry samples (timestamps in seconds) public double[] odometryYawTimestamps = new double[] {}; - - /** Optional odometry samples (yaw positions in radians) */ + // Odometry samples (yaw positions in radians) public double[] odometryYawPositionsRad = new double[] {}; } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java index 18fa3c23..97d47100 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java @@ -20,8 +20,10 @@ import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Constants; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; import java.util.Iterator; @@ -29,23 +31,23 @@ /** IMU IO for NavX. Primitive-only: yaw/rate in radians, accel in m/s^2, jerk in m/s^3. */ public class ImuIONavX implements ImuIO { - private static final double DEG_TO_RAD = Math.PI / 180.0; - private static final double G_TO_MPS2 = 9.80665; + // Define the NavX Hardware private final AHRS navx; - // Phoenix odometry queues (boxed Doubles, but we drain without streams) + // Queues private final Queue yawPositionDegQueue; private final Queue yawTimestampQueue; - // Previous accel (m/s^2) for jerk + // Previous accel for jerk calculation (m/s/s) private Translation3d prevAcc = Translation3d.kZero; private long prevTimestampNs = 0L; - // Reusable buffers for queue drain + // Reusable buffers for queue-drain (to avoid using streams) private double[] odomTsBuf = new double[8]; private double[] odomYawRadBuf = new double[8]; + /** Constructor */ public ImuIONavX() { // Initialize NavX over SPI navx = new AHRS(NavXComType.kMXP_SPI, (byte) SwerveConstants.kOdometryFrequency); @@ -63,10 +65,14 @@ public ImuIONavX() { yawPositionDegQueue = PhoenixOdometryThread.getInstance().registerSignal(navx::getYaw); } + /** Update the Inputs */ @Override public void updateInputs(ImuIOInputs inputs) { final long start = System.nanoTime(); + // Load the nanosecond timestamp + inputs.timestampNs = start; + inputs.connected = navx.isConnected(); // Your original sign convention: @@ -76,21 +82,17 @@ public void updateInputs(ImuIOInputs inputs) { // NavX: // - getAngle() is degrees (continuous) // - getRawGyroZ() is deg/sec - final double yawDeg = -navx.getAngle(); - final double yawRateDegPerSec = -navx.getRawGyroZ(); - - inputs.yawPositionRad = yawDeg * DEG_TO_RAD; - inputs.yawRateRadPerSec = yawRateDegPerSec * DEG_TO_RAD; + inputs.yawPositionRad = Units.degreesToRadians(-navx.getAngle()); + inputs.yawRateRadPerSec = Units.degreesToRadians(-navx.getRawGyroZ()); - // World linear accel (NavX returns "g" typically). Convert to m/s^2. + // World linear accel (NavX returns "g" typically); convert to m/s/s inputs.linearAccel = new Translation3d( - navx.getWorldLinearAccelX(), - navx.getWorldLinearAccelY(), - navx.getWorldLinearAccelZ()) - .times(G_TO_MPS2); + navx.getWorldLinearAccelX() * Constants.G_TO_MPS2, + navx.getWorldLinearAccelY() * Constants.G_TO_MPS2, + navx.getWorldLinearAccelZ() * Constants.G_TO_MPS2); - // Jerk + // Jerk computed as (delta accel) / dt if (prevTimestampNs != 0L) { final double dt = (start - prevTimestampNs) * 1e-9; if (dt > 1e-6) { @@ -98,14 +100,14 @@ public void updateInputs(ImuIOInputs inputs) { } } + // Load "previous values" for the next loop prevTimestampNs = start; prevAcc = inputs.linearAccel; - inputs.timestampNs = start; - - // Odometry history + // Drain odometry queues to primitive arrays (timestamps == doubles; yaws == degrees) final int n = drainOdomQueues(); if (n > 0) { + // If there's anything to drain... final double[] tsOut = new double[n]; final double[] yawOut = new double[n]; System.arraycopy(odomTsBuf, 0, tsOut, 0, n); @@ -113,10 +115,12 @@ public void updateInputs(ImuIOInputs inputs) { inputs.odometryYawTimestamps = tsOut; inputs.odometryYawPositionsRad = yawOut; } else { + // ...otherwise return empty arrays inputs.odometryYawTimestamps = new double[] {}; inputs.odometryYawPositionsRad = new double[] {}; } + // Compute how long this took in seconds final long end = System.nanoTime(); inputs.latencySeconds = (end - start) * 1e-9; } @@ -128,7 +132,7 @@ public void updateInputs(ImuIOInputs inputs) { */ @Override public void zeroYawRad(double yawRad) { - navx.setAngleAdjustment(yawRad / DEG_TO_RAD); + navx.setAngleAdjustment(Units.radiansToDegrees(yawRad)); navx.zeroYaw(); // Reset jerk history so you don't spike on the next frame @@ -136,6 +140,11 @@ public void zeroYawRad(double yawRad) { prevAcc = Translation3d.kZero; } + /** + * Drain the Odometry Queues into a Buffer + * + *

Private function that does the heavy lifting of draining the queues + */ private int drainOdomQueues() { final int nTs = yawTimestampQueue.size(); final int nYaw = yawPositionDegQueue.size(); @@ -157,7 +166,7 @@ private int drainOdomQueues() { // queue provides degrees (navx::getYaw). Apply your sign convention (-d) then rad. final double yawDeg = -itY.next(); - odomYawRadBuf[i] = yawDeg * DEG_TO_RAD; + odomYawRadBuf[i] = Units.degreesToRadians(yawDeg); i++; } @@ -167,6 +176,11 @@ private int drainOdomQueues() { return i; } + /** + * Check that buffer is big enough for this queue + * + *

Private function that ensures odometry buffer capacity + */ private void ensureOdomCapacity(int n) { if (odomTsBuf.length >= n) return; int newCap = odomTsBuf.length; diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index 0db40ef9..a1f708b0 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -27,6 +27,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearAcceleration; +import frc.robot.Constants; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.RBSICANBusRegistry; @@ -36,10 +37,7 @@ /** IMU IO for CTRE Pigeon2 */ public class ImuIOPigeon2 implements ImuIO { - // Constants - private static final double G_TO_MPS2 = 9.80665; - - // Define the Pigeon2 + // Define the Pigeon2 Hardware private final Pigeon2 pigeon = new Pigeon2( SwerveConstants.kPigeonId, RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName)); @@ -55,15 +53,15 @@ public class ImuIOPigeon2 implements ImuIO { private final Queue odomTimestamps; private final Queue odomYawsDeg; - // Previous accel for jerk (m/s^2) + // Previous accel for jerk calculation (m/s/s) private Translation3d prevAcc = Translation3d.kZero; private long prevTimestampNs = 0L; - // Reusable buffers for queue-drain (avoid streams) + // Reusable buffers for queue-drain (to avoid using streams) private double[] odomTsBuf = new double[8]; private double[] odomYawRadBuf = new double[8]; - // Constructor + /** Constructor */ public ImuIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); @@ -86,38 +84,40 @@ public ImuIOPigeon2() { public void updateInputs(ImuIOInputs inputs) { final long start = System.nanoTime(); + // Load the nanosecond timestamp + inputs.timestampNs = start; + StatusCode code = BaseStatusSignal.refreshAll(yawSignal, yawRateSignal, accelX, accelY, accelZ); inputs.connected = code.isOK(); // Yaw / rate: Phoenix returns degrees and deg/s here; convert to radians - final double yawDeg = yawSignal.getValueAsDouble(); - final double yawRateDegPerSec = yawRateSignal.getValueAsDouble(); + inputs.yawPositionRad = Units.degreesToRadians(yawSignal.getValueAsDouble()); + inputs.yawRateRadPerSec = Units.degreesToRadians(yawRateSignal.getValueAsDouble()); - inputs.yawPositionRad = Units.degreesToRadians(yawDeg); - inputs.yawRateRadPerSec = Units.degreesToRadians(yawRateDegPerSec); - - // Accel: Phoenix returns "g" for these signals (common for Pigeon2). Convert to m/s^2 + // Accel: Phoenix returns "g" for these signals; convert to m/s/s inputs.linearAccel = new Translation3d( - accelX.getValueAsDouble(), accelY.getValueAsDouble(), accelZ.getValueAsDouble()) - .times(G_TO_MPS2); + accelX.getValueAsDouble() * Constants.G_TO_MPS2, + accelY.getValueAsDouble() * Constants.G_TO_MPS2, + accelZ.getValueAsDouble() * Constants.G_TO_MPS2); - // Jerk from delta accel / dt + // Jerk computed as (delta accel) / dt if (prevTimestampNs != 0L) { final double dt = (start - prevTimestampNs) * 1e-9; + // Only compute if `dt` is larger than 1 ms. if (dt > 1e-6) { inputs.linearJerk = inputs.linearAccel.minus(prevAcc).div(dt); } } + // Load "previous values" for the next loop prevTimestampNs = start; prevAcc = inputs.linearAccel; - inputs.timestampNs = start; - - // Drain odometry queues to primitive arrays (timestamps are already doubles; yaws are degrees) + // Drain odometry queues to primitive arrays (timestamps == doubles; yaws == degrees) final int n = drainOdometryQueuesIntoBuffers(); if (n > 0) { + // If there's anything to drain... final double[] tsOut = new double[n]; final double[] yawOut = new double[n]; System.arraycopy(odomTsBuf, 0, tsOut, 0, n); @@ -125,10 +125,12 @@ public void updateInputs(ImuIOInputs inputs) { inputs.odometryYawTimestamps = tsOut; inputs.odometryYawPositionsRad = yawOut; } else { + // ...otherwise return empty arrays inputs.odometryYawTimestamps = new double[] {}; inputs.odometryYawPositionsRad = new double[] {}; } + // Compute how long this took in seconds final long end = System.nanoTime(); inputs.latencySeconds = (end - start) * 1e-9; } @@ -143,6 +145,11 @@ public void zeroYawRad(double yawRad) { pigeon.setYaw(Units.radiansToDegrees(yawRad)); } + /** + * Drain the Odometry Queues into a Buffer + * + *

Private function that does the heavy lifting of draining the queues + */ private int drainOdometryQueuesIntoBuffers() { final int nTs = odomTimestamps.size(); final int nYaw = odomYawsDeg.size(); @@ -171,6 +178,11 @@ private int drainOdometryQueuesIntoBuffers() { return i; } + /** + * Check that buffer is big enough for this queue + * + *

Private function that ensures odometry buffer capacity + */ private void ensureOdomCapacity(int n) { if (odomTsBuf.length >= n) return; int newCap = odomTsBuf.length; diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index a28212ce..1bf04a7d 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -19,7 +19,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; +import frc.robot.util.TimeUtil; import org.littletonrobotics.junction.Logger; /** Simulated IMU for full robot simulation & replay logging */ @@ -73,7 +73,7 @@ public void updateInputs(ImuIOInputs inputs) { inputs.linearJerk = Translation3d.kZero; // Maintain odometry history - pushOdomSample(Timer.getFPGATimestamp(), yawRad); + pushOdomSample(TimeUtil.now(), yawRad); // Export odometry arrays (copy out in chronological order) final int n = odomSize; @@ -94,7 +94,7 @@ public void updateInputs(ImuIOInputs inputs) { inputs.odometryYawTimestamps = tsOut; inputs.odometryYawPositionsRad = yawOut; - // Optional: SIM logging (primitive-friendly) + // SIM logging Logger.recordOutput("IMU/YawRad", yawRad); Logger.recordOutput("IMU/YawDeg", Units.radiansToDegrees(yawRad)); Logger.recordOutput("IMU/YawRateDps", Units.radiansToDegrees(yawRateRadPerSec)); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 74d318aa..fda7b4b5 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -25,12 +25,14 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.Constants; +import frc.robot.Constants.Cameras; import frc.robot.FieldConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; +import frc.robot.util.TimedPose; import frc.robot.util.VirtualSubsystem; import java.util.ArrayDeque; import java.util.ArrayList; @@ -43,29 +45,30 @@ public class Vision extends VirtualSubsystem { + // Declare the Vision IO + private final VisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + /** Vision Consumer definition */ @FunctionalInterface - public interface VisionConsumer { - void accept(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix stdDevs); + public interface PoseMeasurementConsumer { + void accept(TimedPose measurement); } - private record TimedEstimate(Pose2d pose, double ts, Matrix stdDevs) {} - - private final VisionConsumer consumer; + // Declare pose consumer, drivebase, and epoch reset + private final PoseMeasurementConsumer consumer; private final Drive drive; private long lastSeenPoseResetEpoch = -1; - private final VisionIO[] io; - private final VisionIOInputsAutoLogged[] inputs; - - private final Constants.Cameras.CameraConfig[] camConfigs = Constants.Cameras.ALL; + // Declare the camera configurations + private final Cameras.CameraConfig[] camConfigs = Cameras.ALL; // Per-camera monotonic and pose reset gates private final double[] lastAcceptedTsPerCam; private volatile double lastPoseResetTimestamp = Double.NEGATIVE_INFINITY; // Smoothing buffer (recent fused estimates) - private final ArrayDeque fusedBuffer = new ArrayDeque<>(); + private final ArrayDeque fusedBuffer = new ArrayDeque<>(); private final double smoothWindowSec = 0.25; private final int smoothMaxSize = 12; @@ -82,8 +85,18 @@ private record TimedEstimate(Pose2d pose, double ts, Matrix stdDevs) {} private volatile double yawGateLookbackSec = 0.30; private volatile double yawGateLimitRadPerSec = 5.0; + // Variance minimum for fusing poses to prevent divide-by-zero explosions + private static final double kMinVariance = 1e-12; + + // Last smoothed and fused poses -- used for debugging + private Pose2d lastFusedPose = new Pose2d(); + private Pose2d lastSmoothedPose = new Pose2d(); + private double lastFusedTs = Double.NaN; + private boolean lastFusedValid = false; + private boolean lastSmoothedValid = false; + /** Constructor */ - public Vision(Drive drive, VisionConsumer consumer, VisionIO... io) { + public Vision(Drive drive, PoseMeasurementConsumer consumer, VisionIO... io) { this.drive = drive; this.consumer = consumer; this.io = io; @@ -96,114 +109,190 @@ public Vision(Drive drive, VisionConsumer consumer, VisionIO... io) { this.lastAcceptedTsPerCam = new double[io.length]; Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); - // Log robot->camera transforms if available + // Log robot->camera transforms int n = Math.min(camConfigs.length, io.length); for (int i = 0; i < n; i++) { Logger.recordOutput("Vision/RobotToCamera" + i, camConfigs[i].robotToCamera()); } } + /** + * Priority value for this virtual subsystem + * + *

See `frc.robot.util.VirtualSubsystem` for a description of the suggested values for various + * virtual subsystems. + */ + @Override + protected int getPeriodPriority() { + return -10; + } + /** Periodic Function */ @Override public void rbsiPeriodic() { - long epoch = drive.getPoseResetEpoch(); - if (epoch != lastSeenPoseResetEpoch) { - lastSeenPoseResetEpoch = epoch; - resetPoseGate(drive.getLastPoseResetTimestamp()); // your existing method - Logger.recordOutput("Vision/PoseGateResetFromDrive", true); - } else { - Logger.recordOutput("Vision/PoseGateResetFromDrive", false); - } + // Debugging values + boolean hasAcceptedThisLoop = false; + boolean hasFusedThisLoop = false; + boolean hasSmoothedThisLoop = false; + + try { + + lastAlignDbg.reset(); + // Pose reset gate (clears smoothing state, resets per-cam monotonic gates) + long epoch = drive.getPoseResetEpoch(); + if (epoch != lastSeenPoseResetEpoch) { + lastSeenPoseResetEpoch = epoch; + resetPoseGate(drive.getLastPoseResetTimestamp()); + Logger.recordOutput("Vision/PoseGateResetFromDrive", true); + } else { + Logger.recordOutput("Vision/PoseGateResetFromDrive", false); + } - // Update/log camera inputs - for (int i = 0; i < io.length; i++) { - io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + i, inputs[i]); - } + // Read camera inputs + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + i, inputs[i]); + } - // Pick one best accepted estimate per camera for this loop - final ArrayList perCamAccepted = new ArrayList<>(io.length); + // Always-on “health” debug -- may consider removing this + Logger.recordOutput("Vision/Debug/ioLength", io.length); + int totalObs = 0; + for (int i = 0; i < io.length; i++) { + totalObs += (inputs[i].poseObservations != null) ? inputs[i].poseObservations.length : 0; + } + Logger.recordOutput("Vision/Debug/totalObsThisLoop", totalObs); + + // Choose best observation per camera for THIS loop + final ArrayList perCamAccepted = new ArrayList<>(io.length); - for (int cam = 0; cam < io.length; cam++) { - int seen = 0; - int accepted = 0; - int rejected = 0; + for (int cam = 0; cam < io.length; cam++) { - TimedEstimate best = null; - double bestTrustScale = Double.NaN; - int bestTrustedCount = 0; - int bestTagCount = 0; + // Count the number of seen, accepted, and rejected poses estimates + int seen = 0; + int accepted = 0; + int rejected = 0; - for (var obs : inputs[cam].poseObservations) { - seen++; + TimedPose best = null; + double bestTrustScale = Double.NaN; + int bestTrustedCount = 0; + int bestTagCount = 0; - GateResult gate = passesHardGatesAndYawGate(cam, obs); - if (!gate.accepted) { - rejected++; + final var obsArr = inputs[cam].poseObservations; + if (obsArr == null) { + // Log zeros and move along if we ain't seen nuthin' + Logger.recordOutput("Vision/Camera" + cam + "/ObsSeen", 0); + Logger.recordOutput("Vision/Camera" + cam + "/ObsAccepted", 0); + Logger.recordOutput("Vision/Camera" + cam + "/ObsRejected", 0); continue; } - BuiltEstimate built = buildEstimate(cam, obs); - if (built == null) { - rejected++; - continue; + // Loop over pose observations; move along if gating or pose-construction fail + for (var obs : obsArr) { + seen++; + + GateResult gate = passesScrutiny(cam, obs); + Logger.recordOutput("Vision/Camera" + cam + "/GateFail", gate.reason); + if (!gate.accepted) { + rejected++; + continue; + } + + BuiltEstimate built = buildEstimate(cam, obs); + if (built == null) { + rejected++; + continue; + } + + // Compare this estimate to current "best" -- score current estimate using `isBetter()` + if (best == null || isBetter(built.estimate, best)) { + best = built.estimate; + bestTrustScale = built.trustScale; + bestTrustedCount = built.trustedCount; + bestTagCount = obs.tagCount(); + } } - TimedEstimate est = built.estimate; - if (best == null || isBetter(est, best)) { - best = est; - bestTrustScale = built.trustScale; - bestTrustedCount = built.trustedCount; - bestTagCount = obs.tagCount(); + // Accept the "best" pose, if extant + if (best != null) { + accepted++; + lastAcceptedTsPerCam[cam] = best.timestampSeconds(); + perCamAccepted.add(best); + + // Log everything about the accepted pose + Logger.recordOutput("Vision/Camera" + cam + "/InjectedPose2d", best.pose()); + Logger.recordOutput( + "Vision/Camera" + cam + "/InjectedTimestamp", best.timestampSeconds()); + Logger.recordOutput( + "Vision/Camera" + cam + "/InjectedStdDevs", stdDevsToArray(best.stdDevs())); + Logger.recordOutput("Vision/Camera" + cam + "/LastAcceptedTrustScale", bestTrustScale); + Logger.recordOutput( + "Vision/Camera" + cam + "/LastAcceptedTrustedCount", bestTrustedCount); + Logger.recordOutput("Vision/Camera" + cam + "/LastAcceptedTagCount", bestTagCount); } - } - if (best != null) { - accepted++; - lastAcceptedTsPerCam[cam] = best.ts; - perCamAccepted.add(best); - - Logger.recordOutput("Vision/Camera" + cam + "/InjectedPose2d", best.pose); - Logger.recordOutput("Vision/Camera" + cam + "/InjectedTimestamp", best.ts); - Logger.recordOutput("Vision/Camera" + cam + "/InjectedStdDevs", best.stdDevs); - Logger.recordOutput("Vision/Camera" + cam + "/LastAcceptedTrustScale", bestTrustScale); - Logger.recordOutput("Vision/Camera" + cam + "/LastAcceptedTrustedCount", bestTrustedCount); - Logger.recordOutput("Vision/Camera" + cam + "/LastAcceptedTagCount", bestTagCount); + Logger.recordOutput("Vision/Camera" + cam + "/ObsSeen", seen); + Logger.recordOutput("Vision/Camera" + cam + "/ObsAccepted", accepted); + Logger.recordOutput("Vision/Camera" + cam + "/ObsRejected", rejected); } - Logger.recordOutput("Vision/Camera" + cam + "/ObsSeen", seen); - Logger.recordOutput("Vision/Camera" + cam + "/ObsAccepted", accepted); - Logger.recordOutput("Vision/Camera" + cam + "/ObsRejected", rejected); - } - - if (perCamAccepted.isEmpty()) return; - - // Fusion time is the newest timestamp among accepted per-camera samples - double tF = perCamAccepted.stream().mapToDouble(e -> e.ts).max().orElse(Double.NaN); - if (!Double.isFinite(tF)) return; - - // Time-align camera estimates to tF using odometry buffer, then inverse-variance fuse - TimedEstimate fused = fuseAtTime(perCamAccepted, tF); - if (fused == null) return; - - // Smooth by fusing recent fused estimates (also aligned to tF) - pushFused(fused); - TimedEstimate smoothed = smoothAtTime(tF); - if (smoothed == null) return; - - // Inject the pose - consumer.accept(smoothed.pose, smoothed.ts, smoothed.stdDevs); + Logger.recordOutput("Vision/Debug/perCamAcceptedSize", perCamAccepted.size()); - Logger.recordOutput("Vision/FusedPose", fused.pose); - Logger.recordOutput("Vision/SmoothedPose", smoothed.pose); - Logger.recordOutput("Vision/FusedTimestamp", tF); + if (perCamAccepted.isEmpty()) { + // No new vision accepted this loop; we still log cached outputs below (in finally). + return; + } + hasAcceptedThisLoop = true; + + // ===== + // Fuse all accepted cams at the newest timestamp among them + final double tFusion = + perCamAccepted.stream().mapToDouble(e -> e.timestampSeconds()).max().orElse(Double.NaN); + if (!Double.isFinite(tFusion)) return; + + final TimedPose fused = fuseAtTime(perCamAccepted, tFusion); + if (fused == null) return; + hasFusedThisLoop = true; + + // ===== + // Smooth by fusing recent fused estimates aligned to tFusion + pushFused(fused); + final TimedPose smoothed = smoothAtTime(tFusion); + if (smoothed == null) return; + hasSmoothedThisLoop = true; + + // Update caches & inject to drive + lastFusedPose = fused.pose(); + lastSmoothedPose = smoothed.pose(); + lastFusedTs = tFusion; + lastFusedValid = true; + lastSmoothedValid = true; + + consumer.accept(smoothed); + + } finally { + + // Log everything on our way out of this function + + // Always-present “outputs” + Logger.recordOutput("Vision/FusedPose", lastFusedPose); + Logger.recordOutput("Vision/SmoothedPose", lastSmoothedPose); + Logger.recordOutput("Vision/FusedTimestamp", lastFusedTs); + Logger.recordOutput("Vision/HasFused", lastFusedValid); + Logger.recordOutput("Vision/HasSmoothed", lastSmoothedValid); + + // Per-loop flags (never stale) + Logger.recordOutput("Vision/HasAcceptedThisLoop", hasAcceptedThisLoop); + Logger.recordOutput("Vision/HasFusedThisLoop", hasFusedThisLoop); + Logger.recordOutput("Vision/HasSmoothedThisLoop", hasSmoothedThisLoop); + } } + /************************************************************************* */ /** Runtime configuration hooks ****************************************** */ /** - * Call when you reset odometry (e.g. auto start, manual reset, etc). + * Call when odometry is reset (e.g. auto start, manual reset, etc). * * @param fpgaNowSeconds Timestamp for the pose gate reset */ @@ -223,7 +312,7 @@ public void setTrustedTags(Set tags) { } /** - * Set whether to requite trusted tags + * Set whether to require trusted tags * * @param require Boolean */ @@ -255,69 +344,89 @@ public void setSingleTagYawGate(boolean enabled, double lookbackSec, double limi yawGateLimitRadPerSec = limitRadPerSec; } + /************************************************************************* */ /** Gating + Scoring ***************************************************** */ + + /** GateResult Class */ private static final class GateResult { final boolean accepted; + final String reason; - GateResult(boolean accepted) { + GateResult(boolean accepted, String reason) { this.accepted = accepted; + this.reason = reason; } } - private GateResult passesHardGatesAndYawGate(int cam, VisionIO.PoseObservation obs) { + /** + * Gating Function -- checks all sorts of things! + * + * @param cam Camera index + * @param obs PoseObservation + */ + private GateResult passesScrutiny(int cam, VisionIO.PoseObservation obs) { final double ts = obs.timestamp(); // Monotonic per-camera time - if (ts <= lastAcceptedTsPerCam[cam]) return new GateResult(false); + if (ts <= lastAcceptedTsPerCam[cam]) return new GateResult(false, "monotonic time"); // Reject anything older than last pose reset - if (ts < lastPoseResetTimestamp) return new GateResult(false); + if (ts < lastPoseResetTimestamp) return new GateResult(false, "older than pose reset"); // Must have tags - if (obs.tagCount() <= 0) return new GateResult(false); + if (obs.tagCount() <= 0) return new GateResult(false, "no tags"); // Single-tag ambiguity gate - if (obs.tagCount() == 1 && obs.ambiguity() > maxAmbiguity) return new GateResult(false); + if (obs.tagCount() == 1 && obs.ambiguity() > maxAmbiguity) + return new GateResult(false, "highly ambiguous"); // Z sanity - if (Math.abs(obs.pose().getZ()) > maxZError) return new GateResult(false); + if (Math.abs(obs.pose().getZ()) > maxZError) return new GateResult(false, "z not sane"); // Field bounds Pose3d p = obs.pose(); if (p.getX() < 0.0 || p.getX() > FieldConstants.aprilTagLayout.getFieldLength()) - return new GateResult(false); + return new GateResult(false, "out of bounds field X"); if (p.getY() < 0.0 || p.getY() > FieldConstants.aprilTagLayout.getFieldWidth()) - return new GateResult(false); + return new GateResult(false, "out of bounds field Y"); - // Optional 254-style yaw gate: only meaningful for single-tag + // Yaw gate; only meaningful for single-tag if (enableSingleTagYawGate && obs.tagCount() == 1) { OptionalDouble maxYaw = drive.getMaxAbsYawRateRadPerSec(ts - yawGateLookbackSec, ts); if (maxYaw.isPresent() && maxYaw.getAsDouble() > yawGateLimitRadPerSec) { - return new GateResult(false); + return new GateResult(false, "YAW gate failed"); } } - return new GateResult(true); + return new GateResult(true, ""); } + /** Built Estimate Class */ private static final class BuiltEstimate { - final TimedEstimate estimate; + final TimedPose estimate; final double trustScale; final int trustedCount; - BuiltEstimate(TimedEstimate estimate, double trustScale, int trustedCount) { + BuiltEstimate(TimedPose estimate, double trustScale, int trustedCount) { this.estimate = estimate; this.trustScale = trustScale; this.trustedCount = trustedCount; } } + /** + * Build a pose esitmate + * + * @param cam Camera Index + * @param obs PoseObservation + */ private BuiltEstimate buildEstimate(int cam, VisionIO.PoseObservation obs) { - // Base uncertainty grows with distance^2 / tagCount (your 2486-style) + // Base uncertainty grows with distance^2 / tagCount final double tagCount = Math.max(1, obs.tagCount()); final double avgDist = Math.max(0.0, obs.averageTagDistance()); final double distFactor = (avgDist * avgDist) / tagCount; + // Camera uncertainty factor final double camFactor = (cam < camConfigs.length) ? camConfigs[cam].stdDevFactor() : 1.0; double linearStdDev = linearStdDevBaseline * camFactor * distFactor; @@ -331,29 +440,40 @@ private BuiltEstimate buildEstimate(int cam, VisionIO.PoseObservation obs) { // Trusted tag blending final Set kTrusted = trustedTags.get(); + final int[] usedIds = (obs.usedTagIds() != null) ? obs.usedTagIds() : new int[0]; + int trustedCount = 0; - for (int id : obs.usedTagIds()) { - if (kTrusted.contains(id)) trustedCount++; + for (int i = 0; i < usedIds.length; i++) { + if (kTrusted.contains(usedIds[i])) trustedCount++; } + // If no trusted tags, return null if (requireTrustedTag && trustedCount == 0) { return null; } - final int usedCount = obs.usedTagIds().size(); + // Build the trust scale + final int usedCount = usedIds.length; final double fracTrusted = (usedCount == 0) ? 0.0 : ((double) trustedCount / usedCount); - final double trustScale = untrustedTagStdDevScale + fracTrusted * (trustedTagStdDevScale - untrustedTagStdDevScale); linearStdDev *= trustScale; angularStdDev *= trustScale; + linearStdDev = Math.max(linearStdDev, linearStdDevBaseline); + angularStdDev = Math.max(angularStdDev, angularStdDevBaseline); + // Output logs for tuning Logger.recordOutput("Vision/Camera" + cam + "/InjectedFracTrusted", fracTrusted); + Logger.recordOutput("Vision/Camera" + cam + "/Dbg_linearStdDev", linearStdDev); + Logger.recordOutput("Vision/Camera" + cam + "/Dbg_angularStdDev", angularStdDev); + Logger.recordOutput("Vision/Camera" + cam + "/Dbg_avgDist", avgDist); + Logger.recordOutput("Vision/Camera" + cam + "/Dbg_tagCount", obs.tagCount()); + return new BuiltEstimate( - new TimedEstimate( + new TimedPose( obs.pose().toPose2d(), obs.timestamp(), VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)), @@ -361,81 +481,194 @@ private BuiltEstimate buildEstimate(int cam, VisionIO.PoseObservation obs) { trustedCount); } - private boolean isBetter(TimedEstimate a, TimedEstimate b) { + /** + * Evaluate whether a is better than b + * + * @param a Base pose + * @param b Competitor pose + */ + private boolean isBetter(TimedPose a, TimedPose b) { // Lower trace of stddev vector (x+y+theta) is better - double ta = a.stdDevs.get(0, 0) + a.stdDevs.get(1, 0) + a.stdDevs.get(2, 0); - double tb = b.stdDevs.get(0, 0) + b.stdDevs.get(1, 0) + b.stdDevs.get(2, 0); + double ta = a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0) + a.stdDevs().get(2, 0); + double tb = b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0) + b.stdDevs().get(2, 0); return ta < tb; } + /************************************************************************* */ + /** Debug snapshot for the most-recent successful alignment this loop. */ + private static final class AlignDebug { + double alignDt = Double.NaN; + double deltaTranslation = Double.NaN; + double deltaRotation = Double.NaN; + boolean alignFinite = false; + + void reset() { + alignDt = Double.NaN; + deltaTranslation = Double.NaN; + deltaRotation = Double.NaN; + alignFinite = false; + } + + void set(double dt, Transform2d tf) { + alignDt = dt; + deltaTranslation = tf.getTranslation().getNorm(); + deltaRotation = tf.getRotation().getRadians(); + alignFinite = + Double.isFinite(alignDt) + && Double.isFinite(deltaTranslation) + && Double.isFinite(deltaRotation); + } + } + + private final AlignDebug lastAlignDbg = new AlignDebug(); + + /************************************************************************* */ /** Time alignment & fusion ********************************************** */ - private TimedEstimate fuseAtTime(ArrayList estimates, double tF) { - final ArrayList aligned = new ArrayList<>(estimates.size()); + + /** + * Fuse poses at a specified timestamp + * + * @param estimates Array of TimedPose esitmates + * @param tFusion The timestamp at which to fuse the measurements + */ + private TimedPose fuseAtTime(ArrayList estimates, double tFusion) { + final ArrayList aligned = new ArrayList<>(estimates.size()); for (var e : estimates) { - Pose2d alignedPose = timeAlignPose(e.pose, e.ts, tF); + Pose2d alignedPose = timeAlignPose(e.pose(), e.timestampSeconds(), tFusion); if (alignedPose == null) return null; - aligned.add(new TimedEstimate(alignedPose, tF, e.stdDevs)); + aligned.add(new TimedPose(alignedPose, tFusion, e.stdDevs())); } - return inverseVarianceFuse(aligned, tF); + return inverseVarianceFuse(aligned, tFusion); } - private Pose2d timeAlignPose(Pose2d visionPoseAtTs, double ts, double tF) { + /** + * Align a pose to where it would have been at the fusion time + * + *

* + * + *

We compute: - dTrans = odomTF.translation - odomTs.translation (field frame) - dTheta = + * odomTF.rotation - odomTs.rotation (field frame / global heading delta) + * + *

Then apply those deltas directly to the vision pose at ts to estimate vision at tFusion. + * + *

Gets the odometric poses at ts and tFusion from the drivebase PoseEstimator, computes the + * transform between them, and applies that to the vision pose. The correction is applied by + * finding the field-frame deltas for both translation and rotation, then returning a new Pose2d + * object that consists of the vision pose adjusted by the field-frame deltas. + * + * @param visionPoseAtTs The pose at ts + * @param ts Timestamp of the pose + * @param tFusion Fusion timestamp + * @return Transformed Pose2d + */ + private Pose2d timeAlignPose(Pose2d visionPoseAtTs, double ts, double tFusion) { Optional odomAtTsOpt = drive.getPoseAtTime(ts); - Optional odomAtTFOpt = drive.getPoseAtTime(tF); + Optional odomAtTFOpt = drive.getPoseAtTime(tFusion); if (odomAtTsOpt.isEmpty() || odomAtTFOpt.isEmpty()) return null; - Pose2d odomAtTs = odomAtTsOpt.get(); - Pose2d odomAtTF = odomAtTFOpt.get(); + final Pose2d odomAtTs = odomAtTsOpt.get(); + final Pose2d odomAtTF = odomAtTFOpt.get(); - // Transform that takes odomAtTs -> odomAtTF - Transform2d ts_T_tf = odomAtTF.minus(odomAtTs); - - // Apply same motion to vision pose to bring it forward - return visionPoseAtTs.transformBy(ts_T_tf); - } + // FIELD-FRAME translation delta + final Translation2d dTrans = odomAtTF.getTranslation().minus(odomAtTs.getTranslation()); - private TimedEstimate inverseVarianceFuse(ArrayList alignedAtTF, double tF) { - if (alignedAtTF.isEmpty()) return null; - if (alignedAtTF.size() == 1) return alignedAtTF.get(0); + // Heading delta (Rotation2d handles wrapping) + final Rotation2d dTheta = odomAtTF.getRotation().minus(odomAtTs.getRotation()); - double sumWx = 0, sumWy = 0, sumWth = 0; - double sumX = 0, sumY = 0; - double sumCos = 0, sumSin = 0; + // Update debug ONCE per loop (first successful alignment wins) + if (!lastAlignDbg.alignFinite) { + // For debug parity with the other version, package deltas as a Transform2d + lastAlignDbg.set(tFusion - ts, new Transform2d(dTrans, dTheta)); + } - for (var e : alignedAtTF) { - double sx = e.stdDevs.get(0, 0); - double sy = e.stdDevs.get(1, 0); - double sth = e.stdDevs.get(2, 0); + // Apply field-frame deltas to the vision pose + return new Pose2d( + visionPoseAtTs.getTranslation().plus(dTrans), visionPoseAtTs.getRotation().plus(dTheta)); + } - double vx = sx * sx; - double vy = sy * sy; - double vth = sth * sth; + /** + * Fuse a list of poses using inverse variable weighting + * + * @param alignedAtTF List of aligned poses + * @param tFusion Fusion timestamp + * @return Fuesed TimedPose object + */ + private TimedPose inverseVarianceFuse(ArrayList alignedAtTF, double tFusion) { + // If size of alignedAtTF is 0 or 1, return null or the only value + if (alignedAtTF == null || alignedAtTF.isEmpty()) return null; + if (alignedAtTF.size() == 1) return alignedAtTF.get(0); - double wx = 1.0 / vx; - double wy = 1.0 / vy; - double wth = 1.0 / vth; + // Define summing values + double sumWx = 0.0, sumWy = 0.0, sumWth = 0.0; + double sumX = 0.0, sumY = 0.0; + double sumCos = 0.0, sumSin = 0.0; + + // Loop over poses in the list + for (int i = 0; i < alignedAtTF.size(); i++) { + final TimedPose e = alignedAtTF.get(i); + final Pose2d p = e.pose(); + final Matrix s = e.stdDevs(); + + final double sx = s.get(0, 0); + final double sy = s.get(1, 0); + final double sth = s.get(2, 0); + + // variance = std^2, clamp away from 0 + final double vx = Math.max(sx * sx, kMinVariance); + final double vy = Math.max(sy * sy, kMinVariance); + final double vth = Math.max(sth * sth, kMinVariance); + + final double wx = 1.0 / vx; + final double wy = 1.0 / vy; + final double wth = 1.0 / vth; + + // If any weight goes NaN/Inf, skip this measurement rather than poisoning the fuse. + if (!Double.isFinite(wx) || !Double.isFinite(wy) || !Double.isFinite(wth)) { + continue; + } sumWx += wx; sumWy += wy; sumWth += wth; - sumX += e.pose.getX() * wx; - sumY += e.pose.getY() * wy; + sumX += p.getX() * wx; + sumY += p.getY() * wy; - sumCos += e.pose.getRotation().getCos() * wth; - sumSin += e.pose.getRotation().getSin() * wth; + final Rotation2d rot = p.getRotation(); + sumCos += rot.getCos() * wth; + sumSin += rot.getSin() * wth; } - Pose2d fused = new Pose2d(sumX / sumWx, sumY / sumWy, new Rotation2d(sumCos, sumSin)); + // If everything got skipped; return null + if (sumWx <= 0.0 || sumWy <= 0.0 || sumWth <= 0.0) return null; + + // Construct the fused translation + final Translation2d fusedTranslation = new Translation2d(sumX / sumWx, sumY / sumWy); + + // Rotation2d(cos, sin) will normalize internally; if both are ~0, fall back to zero. + final Rotation2d fusedRotation = + (Math.abs(sumCos) < 1e-12 && Math.abs(sumSin) < 1e-12) + ? Rotation2d.kZero + : new Rotation2d(sumCos, sumSin); - Matrix fusedStd = + // The fused pose is the combination of translation and rotation + final Pose2d fusedPose = new Pose2d(fusedTranslation, fusedRotation); + + // Fused standard deviations + final Matrix fusedStdDevs = VecBuilder.fill(Math.sqrt(1.0 / sumWx), Math.sqrt(1.0 / sumWy), Math.sqrt(1.0 / sumWth)); - return new TimedEstimate(fused, tF, fusedStd); + // Construct and return the TimedPose objects + return new TimedPose(fusedPose, tFusion, fusedStdDevs); } + /************************************************************************* */ /** Smoothing buffer ***************************************************** */ - private void pushFused(TimedEstimate fused) { + + /** THIS LIKELY HAS PROBLEMS */ + + /** Push the fused pose */ + private void pushFused(TimedPose fused) { fusedBuffer.addLast(fused); while (fusedBuffer.size() > smoothMaxSize) { @@ -443,23 +676,32 @@ private void pushFused(TimedEstimate fused) { } // Trim by time window relative to newest - while (!fusedBuffer.isEmpty() && fused.ts - fusedBuffer.peekFirst().ts > smoothWindowSec) { + while (!fusedBuffer.isEmpty() + && fused.timestampSeconds() - fusedBuffer.peekFirst().timestampSeconds() + > smoothWindowSec) { fusedBuffer.removeFirst(); } } - private TimedEstimate smoothAtTime(double tF) { + private TimedPose smoothAtTime(double tFusion) { if (fusedBuffer.isEmpty()) return null; if (fusedBuffer.size() == 1) return fusedBuffer.peekLast(); - final ArrayList aligned = new ArrayList<>(fusedBuffer.size()); + final ArrayList aligned = new ArrayList<>(fusedBuffer.size()); for (var e : fusedBuffer) { - Pose2d alignedPose = timeAlignPose(e.pose, e.ts, tF); + Pose2d alignedPose = timeAlignPose(e.pose(), e.timestampSeconds(), tFusion); if (alignedPose == null) continue; - aligned.add(new TimedEstimate(alignedPose, tF, e.stdDevs)); + aligned.add(new TimedPose(alignedPose, tFusion, e.stdDevs())); + // Debugging Logging + Logger.recordOutput("Vision/Debug/deltaTime", tFusion - e.timestampSeconds()); } if (aligned.isEmpty()) return fusedBuffer.peekLast(); - return inverseVarianceFuse(aligned, tF); + return inverseVarianceFuse(aligned, tFusion); + } + + /** UTILITY FUNCTIONS **************************************************** */ + private static double[] stdDevsToArray(Matrix s) { + return new double[] {s.get(0, 0), s.get(1, 0), s.get(2, 0)}; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 7bc61ed2..4f11bf9f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import java.util.Set; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @@ -19,14 +18,14 @@ public interface VisionIO { class VisionIOInputs { public boolean connected = false; - /** Latest "camera to target" observation (optional) */ + // Latest "camera to target" observation (optional) public TargetObservation latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); - /** Pose observations generated by the camera pipeline (each has its own timestamp) */ + // Pose observations generated by the camera pipeline (each has its own timestamp) public PoseObservation[] poseObservations = new PoseObservation[0]; - /** Union of tag IDs seen this loop (for logging/UI only) */ + // Union of tag IDs seen this loop public int[] tagIds = new int[0]; } @@ -35,14 +34,18 @@ record TargetObservation(Rotation2d tx, Rotation2d ty) {} /** Represents a robot pose sample used for pose estimation. */ record PoseObservation( - double timestamp, // camera capture time (seconds, same timebase as FPGA) - Pose3d pose, // field->robot pose - double ambiguity, // single-tag ambiguity if available + double timestamp, + Pose3d pose, + double ambiguity, int tagCount, double averageTagDistance, PoseObservationType type, - Set usedTagIds // immutable per observation - ) {} + int[] usedTagIds) { + // Compact constructor to coalesce null to empty + public PoseObservation { + usedTagIds = (usedTagIds == null) ? new int[0] : usedTagIds; + } + } enum PoseObservationType { MEGATAG_1, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index 07c83c7b..c9bbc0d3 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -18,6 +18,7 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; +import java.util.Arrays; import java.util.HashSet; import java.util.LinkedList; import java.util.List; @@ -71,13 +72,24 @@ public void updateInputs(VisionIOInputs inputs) { .flush(); // Increases network traffic but recommended by Limelight // Read new pose observations from NetworkTables - Set tagIds = new HashSet<>(); + Set unionTagIds = new HashSet<>(); List poseObservations = new LinkedList<>(); + for (var rawSample : megatag1Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); + + int tagCount = (int) rawSample.value[7]; + + // Build used tag array for THIS observation only + int[] used = new int[tagCount]; + int u = 0; + + for (int i = 11; i < rawSample.value.length && u < tagCount; i += 7) { + int id = (int) rawSample.value[i]; + used[u++] = id; + unionTagIds.add(id); } + poseObservations.add( new PoseObservation( // Timestamp, based on server timestamp of publish and latency @@ -90,7 +102,7 @@ public void updateInputs(VisionIOInputs inputs) { rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, // Tag count - (int) rawSample.value[7], + tagCount, // Average tag distance rawSample.value[9], @@ -98,14 +110,24 @@ public void updateInputs(VisionIOInputs inputs) { // Observation type PoseObservationType.MEGATAG_1, - // Visible Tag IDs - tagIds)); + // Used tag IDs + used)); } + for (var rawSample : megatag2Subscriber.readQueue()) { if (rawSample.value.length == 0) continue; - for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); + + int tagCount = (int) rawSample.value[7]; + + int[] used = new int[tagCount]; + int u = 0; + + for (int i = 11; i < rawSample.value.length && u < tagCount; i += 7) { + int id = (int) rawSample.value[i]; + used[u++] = id; + unionTagIds.add(id); } + poseObservations.add( new PoseObservation( // Timestamp, based on server timestamp of publish and latency @@ -125,23 +147,20 @@ public void updateInputs(VisionIOInputs inputs) { // Observation type PoseObservationType.MEGATAG_2, - - // Visible Tag IDs - tagIds)); + used)); } // Save pose observations to inputs object - inputs.poseObservations = new PoseObservation[poseObservations.size()]; - for (int i = 0; i < poseObservations.size(); i++) { - inputs.poseObservations[i] = poseObservations.get(i); - } + inputs.poseObservations = poseObservations.toArray(new PoseObservation[0]); - // Save tag IDs to inputs objects - inputs.tagIds = new int[tagIds.size()]; + inputs.tagIds = new int[unionTagIds.size()]; int i = 0; - for (int id : tagIds) { + for (int id : unionTagIds) { inputs.tagIds[i++] = id; } + + // Sort list by TagID for clarity + Arrays.sort(inputs.tagIds); } /** Parses the 3D pose from a Limelight botpose array. */ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 14a2867b..4dac1e05 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -15,11 +15,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import java.util.ArrayList; +import java.util.Arrays; import java.util.HashSet; import java.util.Set; import org.photonvision.PhotonCamera; -/** IO implementation for real PhotonVision hardware (pose already solved by PV). */ +/** IO implementation for real PhotonVision hardware. */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; @@ -62,9 +63,11 @@ public void updateInputs(VisionIOInputs inputs) { bestPitch = Rotation2d.fromDegrees(result.getBestTarget().getPitch()); } - if (result.multitagResult.isPresent()) { + // Add pose observation + if (result.multitagResult.isPresent()) { // Multitag result var multitag = result.multitagResult.get(); + // Calculate robot pose Transform3d fieldToCamera = multitag.estimatedPose.best; Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); @@ -77,12 +80,15 @@ public void updateInputs(VisionIOInputs inputs) { double avgTagDistance = result.targets.isEmpty() ? 0.0 : (totalTagDistance / result.targets.size()); - Set used = new HashSet<>(); + // Build used tag list (loggable + replayable) + int[] used = new int[multitag.fiducialIDsUsed.size()]; + int u = 0; for (int id : multitag.fiducialIDsUsed) { - used.add(id); - unionTagIds.add(id); + used[u++] = id; + unionTagIds.add(id); // keep your union set for tagIds UI/log } + // Add observation poseObservations.add( new PoseObservation( ts, @@ -91,11 +97,12 @@ public void updateInputs(VisionIOInputs inputs) { multitag.fiducialIDsUsed.size(), avgTagDistance, PoseObservationType.PHOTONVISION, - Set.copyOf(used))); + used)); - } else if (!result.targets.isEmpty()) { + } else if (!result.targets.isEmpty()) { // Single tag result var target = result.targets.get(0); + // Calculate robot pose var tagPose = aprilTagLayout.getTagPose(target.fiducialId); if (tagPose.isEmpty()) continue; @@ -117,10 +124,11 @@ public void updateInputs(VisionIOInputs inputs) { 1, cameraToTarget.getTranslation().getNorm(), PoseObservationType.PHOTONVISION, - Set.of(target.fiducialId))); + new int[] {target.fiducialId})); } } + // Save pose observations to inputs object inputs.latestTargetObservation = (newestTargetTs > Double.NEGATIVE_INFINITY) ? new TargetObservation(bestYaw, bestPitch) @@ -128,8 +136,12 @@ public void updateInputs(VisionIOInputs inputs) { inputs.poseObservations = poseObservations.toArray(new PoseObservation[0]); + // Save tag IDs to inputs objects inputs.tagIds = new int[unionTagIds.size()]; int i = 0; for (int id : unionTagIds) inputs.tagIds[i++] = id; + + // Sort the AprilTag IDs for ease of use by dashboards, etc. + Arrays.sort(inputs.tagIds); } } diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java index 5f20d34a..202012d8 100644 --- a/src/main/java/frc/robot/util/Alert.java +++ b/src/main/java/frc/robot/util/Alert.java @@ -12,7 +12,6 @@ import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.Comparator; @@ -66,7 +65,7 @@ public Alert(String group, String text, AlertType type) { */ public void set(boolean active) { if (active && !this.active) { - activeStartTime = Timer.getFPGATimestamp(); + activeStartTime = TimeUtil.now(); switch (type) { case ERROR: DriverStation.reportError(text, false); diff --git a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java index df46efd2..a2e55e61 100644 --- a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java +++ b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Az-FIRST +// Copyright (c) 2026 Az-FIRST // http://github.com/AZ-First // Copyright (c) 2024 FRC 254 // https://github.com/team254 @@ -24,6 +24,7 @@ import edu.wpi.first.math.interpolation.Interpolator; import java.util.Map.Entry; import java.util.Optional; +import java.util.OptionalDouble; import java.util.concurrent.ConcurrentNavigableMap; import java.util.concurrent.ConcurrentSkipListMap; @@ -126,26 +127,27 @@ public Optional getSample(double timeSeconds) { var bottomBound = m_pastSnapshots.floorEntry(timeSeconds); var topBound = m_pastSnapshots.ceilingEntry(timeSeconds); - // Return null if neither sample exists, and the opposite bound if the other is - // null - if (topBound == null && bottomBound == null) { - return Optional.empty(); - } else if (topBound == null) { + if (topBound == null && bottomBound == null) return Optional.empty(); + if (topBound == null) return Optional.of(bottomBound.getValue()); + if (bottomBound == null) return Optional.of(topBound.getValue()); + + // If they are the same sample, no interpolation possible/needed + if (topBound.getKey().doubleValue() == bottomBound.getKey().doubleValue()) { return Optional.of(bottomBound.getValue()); - } else if (bottomBound == null) { - return Optional.of(topBound.getValue()); - } else { - // Otherwise, interpolate. Because T is between [0, 1], we want the ratio of - // (the difference - // between the current time and bottom bound) and (the difference between top - // and bottom - // bounds). - return Optional.of( - m_interpolatingFunc.interpolate( - bottomBound.getValue(), - topBound.getValue(), - (timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey()))); } + + double t0 = bottomBound.getKey(); + double t1 = topBound.getKey(); + double denom = t1 - t0; + + // If the samples are so close together as to be indistinguishable, they are the same + if (Math.abs(denom) < 1e-9) return Optional.of(bottomBound.getValue()); + + double ratio = (timeSeconds - t0) / denom; + ratio = MathUtil.clamp(ratio, 0.0, 1.0); + + return Optional.of( + m_interpolatingFunc.interpolate(bottomBound.getValue(), topBound.getValue(), ratio)); } public Entry getLatest() { @@ -161,4 +163,16 @@ public Entry getLatest() { public ConcurrentNavigableMap getInternalBuffer() { return m_pastSnapshots; } + + /** Return the oldest timestamp in the buffer */ + public OptionalDouble getOldestTimestamp() { + if (m_pastSnapshots.isEmpty()) return OptionalDouble.empty(); + return OptionalDouble.of(m_pastSnapshots.firstKey()); + } + + /** Return the newest timestamp in the buffer */ + public OptionalDouble getNewestTimestamp() { + if (m_pastSnapshots.isEmpty()) return OptionalDouble.empty(); + return OptionalDouble.of(m_pastSnapshots.lastKey()); + } } diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java new file mode 100644 index 00000000..f77c8b2f --- /dev/null +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -0,0 +1,165 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the AdvantageKit-License.md file +// at the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Translation2d translation) { + return new Transform2d(translation, Rotation2d.kZero); + } + + /** + * Creates a pure translating transform + * + * @param x The x coordinate of the translation + * @param y The y coordinate of the translation + * @return The resulting transform + */ + public static Transform2d toTransform2d(double x, double y) { + return new Transform2d(x, y, Rotation2d.kZero); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d toTransform2d(Rotation2d rotation) { + return new Transform2d(Translation2d.kZero, rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d toTransform2d(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + public static Pose2d inverse(Pose2d pose) { + Rotation2d rotationInverse = pose.getRotation().unaryMinus(); + return new Pose2d( + pose.getTranslation().unaryMinus().rotateBy(rotationInverse), rotationInverse); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d toPose2d(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Translation2d translation) { + return new Pose2d(translation, Rotation2d.kZero); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d toPose2d(Rotation2d rotation) { + return new Pose2d(Translation2d.kZero, rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiply(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d toTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d toPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * + * @param speeds The original translation + * @return The resulting translation + */ + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + + /** + * Creates a new pose from an existing one using a different translation value. + * + * @param pose The original pose + * @param translation The new translation to use + * @return The new pose with the new translation and original rotation + */ + public static Pose2d withTranslation(Pose2d pose, Translation2d translation) { + return new Pose2d(translation, pose.getRotation()); + } + + /** + * Creates a new pose from an existing one using a different rotation value. + * + * @param pose The original pose + * @param rotation The new rotation to use + * @return The new pose with the original translation and new rotation + */ + public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { + return new Pose2d(pose.getTranslation(), rotation); + } +} diff --git a/src/main/java/frc/robot/util/RBSICANBusRegistry.java b/src/main/java/frc/robot/util/RBSICANBusRegistry.java index b0707fc6..4619ec7f 100644 --- a/src/main/java/frc/robot/util/RBSICANBusRegistry.java +++ b/src/main/java/frc/robot/util/RBSICANBusRegistry.java @@ -15,15 +15,21 @@ import com.ctre.phoenix6.CANBus; import java.util.Map; +import java.util.Set; import java.util.concurrent.ConcurrentHashMap; -/** Centralized CAN bus singleton registry + SIM/REAL indirection. */ +/** Centralized CAN bus singleton registry */ public final class RBSICANBusRegistry { private static final Map realBuses = new ConcurrentHashMap<>(); private static final Map likeBuses = new ConcurrentHashMap<>(); private static volatile boolean initialized = false; private static volatile boolean sim = false; + /** + * Initialize the REAL CANBusses + * + * @param busNames The list of bus names to initialize + */ public static void initReal(String... busNames) { sim = false; for (String name : busNames) { @@ -33,6 +39,11 @@ public static void initReal(String... busNames) { initialized = true; } + /** + * Initialize the SIM CANBusses + * + * @param busNames The list of bus names to initialize + */ public static void initSim(String... busNames) { sim = true; for (String name : busNames) { @@ -41,7 +52,12 @@ public static void initSim(String... busNames) { initialized = true; } - /** For Phoenix device constructors (REAL/REPLAY only). */ + /** + * Get the CANBus for Phoenix device constructors + * + * @param name Name of the CAN bus to get + * @return CANBus object corresponding to the name + */ public static CANBus getBus(String name) { checkInit(); if (sim) { @@ -52,7 +68,12 @@ public static CANBus getBus(String name) { return bus; } - /** For health logging (REAL or SIM). */ + /** + * Get a CANBus-like object for health logging + * + * @param name Name of the bus to health log + * @return CANBusLike object + */ public static CANBusLike getLike(String name) { checkInit(); CANBusLike bus = likeBuses.get(name); @@ -60,52 +81,63 @@ public static CANBusLike getLike(String name) { return bus; } + /** Check that the Registry is initialized */ private static void checkInit() { if (!initialized) throw new IllegalStateException("RBSICANBusRegistry not initialized."); } - private static void throwUnknown(String name, java.util.Set known) { + /** Throw exception if the CAN bus name is not in the Registry */ + private static void throwUnknown(String name, Set known) { throw new IllegalArgumentException("Unknown CAN bus '" + name + "'. Known: " + known); } - // ---- nested types ---- + /** Nested types for Registry Function *********************************** */ + /** CANBusLike interface */ public interface CANBusLike { String getName(); CANBus.CANBusStatus getStatus(); } + /** Real CAN Bus Adapter */ static final class RealCANBusAdapter implements CANBusLike { private final CANBus bus; + /** Constructor */ RealCANBusAdapter(CANBus bus) { this.bus = bus; } + /** Get the name of this CANBus instance */ @Override public String getName() { return bus.getName(); } + /** Get the status of this CANBus instance */ @Override public CANBus.CANBusStatus getStatus() { return bus.getStatus(); } } + /** Simulated CAN Bus Stub */ static final class SimCANBusStub implements CANBusLike { private final String name; + /** Constructor */ SimCANBusStub(String name) { this.name = name; } + /** Get the name of this simulated CANBus */ @Override public String getName() { return name; } + /** Get the status of this simulated CANBus */ @Override public CANBus.CANBusStatus getStatus() { return new CANBus.CANBusStatus(); diff --git a/src/main/java/frc/robot/util/RBSICANHealth.java b/src/main/java/frc/robot/util/RBSICANHealth.java index df7a185a..8959020f 100644 --- a/src/main/java/frc/robot/util/RBSICANHealth.java +++ b/src/main/java/frc/robot/util/RBSICANHealth.java @@ -15,17 +15,27 @@ import org.littletonrobotics.junction.Logger; +/** + * Virtual subsystem to monitor CAN health + * + *

Instantiate one of these subsystems for each CAN bus on the robot in the RobotContainer. + */ public class RBSICANHealth extends VirtualSubsystem { private long loops = 0; private final RBSICANBusRegistry.CANBusLike bus; + /** Constructur */ public RBSICANHealth(String busName) { bus = RBSICANBusRegistry.getLike(busName); } + /** Periodic function */ @Override protected void rbsiPeriodic() { + // Only log CAN health every 5 loops if ((loops++ % 5) != 0) return; + + // Get status and log var status = bus.getStatus(); Logger.recordOutput("CAN/" + bus.getName() + "/Utilization", status.BusUtilization); Logger.recordOutput("CAN/" + bus.getName() + "/TxFullCount", status.TxFullCount); diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 3aa3b8b9..b90752fb 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -38,8 +38,8 @@ public abstract class RBSISubsystem extends SubsystemBase { public final void periodic() { long start = System.nanoTime(); rbsiPeriodic(); - long end = System.nanoTime(); - Logger.recordOutput("Loop/Mech/" + name + "_ms", (end - start) / 1e6); + // Log the timing for this subsystem + Logger.recordOutput("LogPeriodic/Subsystem/" + name + "MS", (System.nanoTime() - start) / 1e6); } /** Subclasses must implement this instead of periodic(). */ diff --git a/src/main/java/frc/robot/util/TimeUtil.java b/src/main/java/frc/robot/util/TimeUtil.java new file mode 100644 index 00000000..62f3cb39 --- /dev/null +++ b/src/main/java/frc/robot/util/TimeUtil.java @@ -0,0 +1,33 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import org.littletonrobotics.junction.Logger; + +/** Time utility that works for REAL, SIM, and REPLAY */ +public final class TimeUtil { + private TimeUtil() {} + + /** Always seconds, regardless of real/sim/replay timebase quirks. */ + public static double now() { + double t = Logger.getTimestamp(); + + // Empirical: in some environments, Logger timestamp is microseconds. + // If it looks like µs, convert to seconds. + if (t > 1.0e6) { + t *= 1.0e-6; + } + return t; + } +} diff --git a/src/main/java/frc/robot/util/TimedPose.java b/src/main/java/frc/robot/util/TimedPose.java new file mode 100644 index 00000000..440cd315 --- /dev/null +++ b/src/main/java/frc/robot/util/TimedPose.java @@ -0,0 +1,21 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public record TimedPose(Pose2d pose, double timestampSeconds, Matrix stdDevs) {} diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 2996aa33..bb99b7ea 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -10,6 +10,7 @@ package frc.robot.util; import java.util.ArrayList; +import java.util.Comparator; import java.util.List; import org.littletonrobotics.junction.Logger; @@ -18,14 +19,39 @@ */ public abstract class VirtualSubsystem { private static final List subsystems = new ArrayList<>(); + private static boolean needsSort = false; + private final String name = getClass().getSimpleName(); // Load all defined virtual subsystems into a list public VirtualSubsystem() { subsystems.add(this); + needsSort = true; // a new subsystem changed ordering + } + + /** + * Override to control ordering. Lower runs earlier. + * + *

Example: IMU inputs -30, Drive odometry -20, Vision -10, Coordinator 0. + */ + protected int getPeriodPriority() { + return 0; } + /** + * Run the periodic functions of each subsystem in the order determined by the getPeriodPriority() + * of each. + */ public static void periodicAll() { + // Sort once (and again only if new subsystems are constructed) + if (needsSort) { + subsystems.sort( + Comparator.comparingInt(VirtualSubsystem::getPeriodPriority) + // deterministic tie-break to avoid “random” order when priorities match + .thenComparingInt(System::identityHashCode)); + needsSort = false; + } + // Call each virtual subsystem during robotPeriodic() for (VirtualSubsystem subsystem : subsystems) { subsystem.periodic(); @@ -46,7 +72,9 @@ public static void periodicAll() { public final void periodic() { long start = System.nanoTime(); rbsiPeriodic(); - Logger.recordOutput("Loop/Virtual/" + name + "_ms", (System.nanoTime() - start) / 1e6); + // Log the timing for this subsystem + Logger.recordOutput( + "LogPeriodic/VirtualSubsystem/" + name + "MS", (System.nanoTime() - start) / 1e6); } /** Subclasses must implement this instead of periodic(). */ diff --git a/vendordeps/Phoenix5-replay-5.36.0.json b/vendordeps/Phoenix5-5.36.0.json similarity index 68% rename from vendordeps/Phoenix5-replay-5.36.0.json rename to vendordeps/Phoenix5-5.36.0.json index 7fbfcf5e..c60dd4cc 100644 --- a/vendordeps/Phoenix5-replay-5.36.0.json +++ b/vendordeps/Phoenix5-5.36.0.json @@ -1,31 +1,31 @@ { - "fileName": "Phoenix5-replay-5.36.0.json", + "fileName": "Phoenix5-5.36.0.json", "name": "CTRE-Phoenix (v5)", "version": "5.36.0", "frcYear": "2026", - "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-replay-frc2026-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-latest.json", "requires": [ { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6-replay-frc2026-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json" + "offlineFileName": "Phoenix6-frc2026-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json" } ], "conflictsWith": [ { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Users must use the regular Phoenix 5 vendordep when using the regular Phoenix 6 vendordep.", - "offlineFileName": "Phoenix6-frc2026-latest.json" + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" }, { - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - "offlineFileName": "Phoenix5-frc2026-latest.json" + "offlineFileName": "Phoenix5-replay-frc2026-latest.json" } ], "javaDependencies": [ @@ -47,22 +47,11 @@ "version": "5.36.0", "isJar": false, "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.replay", - "artifactId": "cci-replay", - "version": "5.36.0", - "isJar": false, - "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", "linuxarm64", - "osxuniversal" + "linuxathena" ], "simMode": "hwsim" }, @@ -91,6 +80,9 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -104,6 +96,9 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -116,56 +111,11 @@ "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.replay", - "artifactId": "wpiapi-cpp-replay", - "version": "5.36.0", - "libName": "CTRE_Phoenix_WPIReplay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", "linuxarm64", - "osxuniversal" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.replay", - "artifactId": "api-cpp-replay", - "version": "5.36.0", - "libName": "CTRE_PhoenixReplay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.replay", - "artifactId": "cci-replay", - "version": "5.36.0", - "libName": "CTRE_PhoenixCCIReplay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" + "linuxathena" ], "simMode": "hwsim" }, diff --git a/vendordeps/Phoenix6-replay-frc2026-latest.json b/vendordeps/Phoenix6-26.1.1.json similarity index 87% rename from vendordeps/Phoenix6-replay-frc2026-latest.json rename to vendordeps/Phoenix6-26.1.1.json index 75532e9d..c0a1c19f 100644 --- a/vendordeps/Phoenix6-replay-frc2026-latest.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,18 +1,18 @@ { - "fileName": "Phoenix6-replay-frc2026-latest.json", - "name": "CTRE-Phoenix (v6) Replay", + "fileName": "Phoenix6-26.1.1.json", + "name": "CTRE-Phoenix (v6)", "version": "26.1.1", "frcYear": "2026", - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", "conflictsWith": [ { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-frc2026-latest.json" + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" } ], "javaDependencies": [ @@ -29,39 +29,17 @@ "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "api-cpp-replay", - "version": "26.1.1", - "isJar": false, - "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", "linuxarm64", - "osxuniversal" + "linuxathena" ], "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "tools-replay", + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, @@ -69,7 +47,7 @@ "windowsx86-64", "linuxx86-64", "linuxarm64", - "osxuniversal" + "linuxathena" ], "simMode": "hwsim" }, @@ -252,6 +230,9 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -264,40 +245,11 @@ "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "wpiapi-cpp-replay", - "version": "26.1.1", - "libName": "CTRE_Phoenix6_WPIReplay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", "linuxarm64", - "osxuniversal" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.replay", - "artifactId": "tools-replay", - "version": "26.1.1", - "libName": "CTRE_PhoenixTools_Replay", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" + "linuxathena" ], "simMode": "hwsim" }, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index b0ac8fb4..6279e58e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1", + "version": "v2026.2.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.2.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1", + "version": "v2026.2.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.2.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1" + "version": "v2026.2.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1" + "version": "v2026.2.2" } ] } diff --git a/vendordeps/yagsl-2026.1.30.json b/vendordeps/yagsl-2026.2.3.1.json similarity index 64% rename from vendordeps/yagsl-2026.1.30.json rename to vendordeps/yagsl-2026.2.3.1.json index aaee5a3f..b783dcdb 100644 --- a/vendordeps/yagsl-2026.1.30.json +++ b/vendordeps/yagsl-2026.2.3.1.json @@ -1,7 +1,7 @@ { - "fileName": "yagsl-2026.1.30.json", + "fileName": "yagsl-2026.2.3.1.json", "name": "YAGSL", - "version": "2026.1.30", + "version": "2026.2.3.1", "frcYear": "2026", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2026.1.30" + "version": "2026.2.3.1" }, { "groupId": "org.dyn4j", @@ -34,18 +34,6 @@ "offlineFileName": "REVLib.json", "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json" }, - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Phoenix6 Replay is required!", - "offlineFileName": "Phoenix6-replay-26.1.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2026-latest.json" - }, - { - "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", - "errorMessage": "Phoenix5 Replay Compatibility is required!", - "offlineFileName": "Phoenix5-replay-5.36.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-replay-frc2026-latest.json" - }, { "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", "errorMessage": "ThriftyLib is required!",