(T value, String reason, double confidence) {}
+```
+
+Log the request, selected action, reason, confidence, rejected alternatives, and any override. The dashboard should show concise statements such as:
+
+```text
+PASS_FUEL: hub inactive for 14.2 s
+PRESET_SHOT: vision confidence below threshold
+FEED_INHIBITED: flywheel not ready
+RECOVERING: pitch exceeded beach threshold
+```
+
+## 8. Use driver assists for constrained motion
+
+The most useful drive assists preserve driver translation while automating a constrained dimension:
+
+- Aim the robot or turret at the hub while the driver translates.
+- Center laterally in the trench while preserving forward/backward control.
+- Snap to a safe heading for bump traversal.
+- Align the climber while preserving an escape input.
+- Point an intake toward the motion vector when that measurably improves collection.
+
+Team 4744 automatically enabled trench centering based on field position, without requiring a button. Team 581 also developed trench, wall-intake, bump-crossing, and intake-orientation assists. [Team 4744 trench automation](https://www.chiefdelphi.com/t/ninjas-4744-2026-build-thread-open-alliance/505741?page=4) [Team 581 driver-assist results](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+
+Each assist should blend or constrain driver input rather than unexpectedly taking full control. Entry and exit conditions need hysteresis so the assist does not chatter near a field-zone boundary.
+
+## 9. Select fuel targets with bounded autonomy
+
+Team 581 detected fuel clusters using a Limelight-hosted Python pipeline, converted observations into field poses, aggregated them into a cluster map, and selected among three known autonomous lanes. The team evaluated clusters using estimated balls collected per second of driving, which filtered out low-value targets. [Team 581 cluster-map system](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+
+Az-RBSI should follow the same bounded approach. Perception should rank known-safe tasks rather than produce unconstrained robot behavior.
+
+A starting utility function is:
+
+```text
+utility =
+ expectedFuel / estimatedCompletionTime
+ - collisionRisk
+ - localizationUncertainty
+ - routeSwitchingPenalty
+```
+
+Require a minimum confidence and improvement margin before changing targets. Once committed to a short acquisition maneuver, finish it unless a safety condition or driver override occurs.
+
+## 10. Manage electrical power as a robot resource
+
+Team 581 dynamically assigned current limits based on the active action, prioritizing shooter consistency during scoring and feeder throughput while passing. [Team 581 Power Manager](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+
+Az-RBSI should begin with discrete, tested power profiles:
+
+| Profile | Priority |
+| --- | --- |
+| `NORMAL` | Balanced driving and mechanism operation. |
+| `SCORING` | Shooter and feeder consistency. |
+| `TRAVERSAL` | Drivetrain control and mechanism stow. |
+| `CLIMBING` | Climber and stabilization. |
+| `LOW_VOLTAGE` | Preventing brownout and preserving control. |
+
+Select profiles from superstructure state, battery margin, and expected current demand. Begin with conservative fixed limits and logging before attempting frequent runtime reconfiguration.
+
+## 11. Encode coordination mechanically when appropriate
+
+If two mechanical actions should nearly always happen together, consider linking them mechanically before adding another actuator and control loop.
+
+Team 157 mechanically coupled intake deployment to hopper-wall extension, translating one rotational input into coordinated linear movement. Other 2026 designs used intake retraction to compact fuel toward the shooter. [Team 157 coupled intake and hopper](https://www.chiefdelphi.com/t/frc-157-the-aztechs-2026-build-thread-open-alliance/509425/25) [2026 intake and hopper discussion](https://www.chiefdelphi.com/t/frc-8575-offseason-cad-practice/520816)
+
+Mechanical coupling can remove sensors, motors, commands, and failure modes. It should still be reviewed for impact tolerance, binding, maintenance access, and whether independent motion is needed for recovery.
+
+## Suggested Az-RBSI components
+
+```text
+FieldState
+ MatchPhase, hub state, transition timing, field zones
+
+RobotWorldModel
+ Pose, velocity, mechanism state, fuel state, health, power
+
+RobotIntent
+ Acquire, score, pass, traverse, climb, safe, recover
+
+DecisionPolicy
+ Resolves intent into a target and action with reason/confidence
+
+SuperstructureCoordinator
+ Executes action states and coordinates subsystem goals
+
+AutomationReflexes
+ Safety interlocks, jam recovery, unbeach, sensor fallback
+
+AutomationOverrides
+ Per-feature disable, preset fallback, full manual recovery
+```
+
+Keep the world model and decision policy free of command-framework types so they can be used by both Commands V2 and V3 infrastructure.
+
+## Implementation phases
+
+### Phase 1: Deterministic automation
+
+1. Add match/hub-state tracking with manual correction.
+2. Define high-level driver intents.
+3. Implement a superstructure for intake, scoring, passing, and safe drive.
+4. Add automatic staging and basic mechanism interlocks.
+5. Log every state transition and rejected action.
+
+### Phase 2: Reflexes and assists
+
+1. Add hood/intake stow zones.
+2. Add jam detection with bounded recovery.
+3. Add trench and climb alignment assists.
+4. Add preset fallbacks for failed vision or localization.
+5. Measure whether each assist improves success rate or cycle time.
+
+### Phase 3: Predictive behavior
+
+1. Add projectile time-of-flight to scoring-window calculations.
+2. Add iterative shoot-on-the-move compensation.
+3. Add action-based power profiles.
+4. Add bounded fuel-cluster ranking from field observations.
+
+## Verification and acceptance criteria
+
+Every automation feature should demonstrate:
+
+- A measurable improvement in cycle time, success rate, or driver workload.
+- Deterministic behavior for identical inputs.
+- Hysteresis around thresholds and field-zone boundaries.
+- A timeout and retry limit for recovery actions.
+- A visible reason for its current decision.
+- Safe behavior when a required sensor becomes invalid.
+- An immediate driver or operator override.
+- Simulation coverage and logged practice-match evidence.
+
+Team 581 deprecated snake mode, wall-intake assist, and bump-crossing assist after practice showed that trained drivers performed as well or better, or that the automation hurt performance under defense. This is a useful standard for Az-RBSI: automation should remain only when evidence shows that it improves the robot in realistic play. [Team 581 deprecated automation findings](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+
+## References
+
+- [Mechanical Advantage 6328: match-aware scoring windows](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2026-build-thread/509595?page=28)
+- [Team 581: cluster mapping, filling, power management, recovery, and fallbacks](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+- [Team 11010: superstructure, simulation, shot adjustment, and climb alignment](https://www.chiefdelphi.com/t/team-bobcats-11010-2026-season-build-blog/508880)
+- [Team 2910: state-machine coordination](https://www.chiefdelphi.com/t/team-2910-code-release-2026/521778)
+- [Team 4096: mechanism protection and shoot-on-the-move groundwork](https://www.chiefdelphi.com/t/frc-team-4096-ctrl-z-2026-build-thread-open-alliance/512198)
+- [Team 4744: match-aware behavior and trench automation](https://www.chiefdelphi.com/t/ninjas-4744-2026-build-thread-open-alliance/505741?page=4)
+- [2026 driver-control automation discussion](https://www.chiefdelphi.com/t/driver-opperator-control-schemes-for-rebuilt/514681)
+- [Team 157: mechanically coupled intake and hopper](https://www.chiefdelphi.com/t/frc-157-the-aztechs-2026-build-thread-open-alliance/509425/25)
diff --git a/doc/recommendations/Top-Team-Common-Practices.md b/doc/recommendations/Top-Team-Common-Practices.md
new file mode 100644
index 00000000..4614997a
--- /dev/null
+++ b/doc/recommendations/Top-Team-Common-Practices.md
@@ -0,0 +1,753 @@
+# Common Practices of High-Performing FRC Teams
+
+**Research snapshot:** August 29, 2026
+
+**Scope:** recurring practices in 2026 Chief Delphi and Open Alliance material,
+with emphasis on teams that demonstrated strong competitive performance,
+well-developed engineering systems, or unusually candid post-event analysis.
+
+## Recommendation In Brief
+
+“Top teams all do X” is useful shorthand, but it is not literally true. Strong
+teams do not all use swerve, turrets, three cameras, custom scouting software,
+or a practice robot. They have different resources and win with different
+architectures.
+
+They do repeatedly converge on a more durable set of behaviors:
+
+1. Convert the game into explicit priorities before designing mechanisms.
+2. Spend complexity only where it creates a measurable match advantage.
+3. Resolve important uncertainty early through prototypes, simulation, and
+ integrated testing.
+4. Protect time for software, autonomous testing, and driver practice.
+5. Treat reliability, serviceability, observability, and fallback operation as
+ designed features.
+6. Run competition as a disciplined feedback loop: prepare, execute, inspect,
+ record, learn, and adapt.
+
+The common denominator is **management of uncertainty**, not a particular
+robot archetype. Az-RBSI should encode the technical parts of that discipline
+and provide lightweight templates for the human processes around it.
+
+## How To Read The List
+
+Each practice has an adoption classification:
+
+- **Foundation:** broadly transferable and worth treating as a default.
+- **Multiplier:** common among strong programs, but its implementation should
+ scale to team resources.
+- **Conditional:** valuable only when it answers a specific strategic or
+ technical need.
+
+The evidence is observational. Open Alliance posts overrepresent teams willing
+to publish, and a strong process can be invisible when a team writes mainly
+about hardware. This document therefore avoids claiming that a practice is
+universal merely because one elite team uses it.
+
+## The Short List
+
+Read each line as “high-performing teams tend to,” not as a universal claim:
+
+### Strategy and design
+
+- Study the whole game before choosing mechanisms.
+- Define the robot's intended match role and rank requirements.
+- Decline features that do not justify their time, mass, power, and risk.
+- Study proven robots and reuse understood ideas.
+- Prototype the important unknowns against measurable pass/fail criteria.
+- Integrate early enough for mechanisms, wiring, software, and drivers to
+ expose one another's problems.
+- Track mass, volume, power, schedule, and service clearance with margin.
+- Design exposed systems for collision and predictable repair.
+- Preserve representative hardware access for programmers and drivers.
+
+### Electrical and software
+
+- Treat wire routing, retention, and motion as mechanical design.
+- Standardize connectors, labeling, configurations, firmware, and spares.
+- Monitor power, CAN, network, device, and loop health.
+- Start production software before the competition robot is complete.
+- Maintain authoritative robot state instead of scattered flags.
+- Log observations, decisions, outputs, timing, and faults.
+- Run production logic in simulation and replay where practical.
+- Automate useful execution while retaining practiced manual fallbacks.
+- Test autonomous routines against realistic errors and field interactions.
+- Keep complexity inside the team's compute, schedule, and maintenance budget.
+
+### Practice and competition
+
+- Protect substantial driver practice and use representative match drills.
+- Practice communication, defense, degraded modes, and recovery.
+- Use robot-specific pre-match and post-match checklists with named owners.
+- Make pit and robot readiness visible to the whole team.
+- Record failures before repairing them, then verify and sign off.
+- Self-inspect early and after consequential changes.
+- Bring verified spares and a configuration/software recovery path.
+- Control event changes and retain a known-good rollback.
+- Collect scouting data that changes strategy, validate it, and supplement it
+ with direct observation.
+- Turn scouting into a short, shared match plan with contingencies.
+- Document decisions, train backups, run retrospectives, and preserve human as
+ well as technical margin.
+
+## 1. Strategy And Scope
+
+### 1.1 Understand the game before inventing the robot
+
+**Classification: Foundation**
+
+Strong teams read the complete manual, enumerate scoring and movement tasks,
+play or simulate the game, and test rules knowledge before selecting an
+architecture. Team 581 began with the full manual, explicit goals, and
+Must/Won't/Could design criteria. Team 7691 used a rules quiz and a human-scale
+version of the game. Team 1792 similarly studied the rules and listed every
+scoring and acquisition action before discussing mechanisms.
+
+Sources: [Team 581 2026 CAD and code release](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762),
+[Team 7691 Open Alliance thread](https://www.chiefdelphi.com/t/frc-7691-2025-2026-build-thread-open-alliance/506859),
+[Team 1792 Open Alliance thread](https://www.chiefdelphi.com/t/1792-round-table-robotics-2026-open-alliance-thread/509877)
+
+**Az-RBSI recommendation:** add a season-planning template containing a rules
+quiz link, task inventory, field-zone diagram, and unresolved-rules register.
+Keep it outside robot code, but link it from the project documentation.
+
+### 1.2 Define the robot's job and rank requirements
+
+**Classification: Foundation**
+
+Competitive teams distinguish requirements from attractive features. A useful
+version is Must/Should/Could/Won't or Needs/Wants/Wishes, tied to the level of
+play the team intends to reach. The result should say what the robot must do,
+how well it must do it, and what the team has deliberately declined.
+
+Team 6328 summarized its target as a reliable, relatively simple robot that
+leaned on its software strengths. Team 5119 explicitly rejected a turret that
+did not match its resources and experience, in part to preserve driver-practice
+time. That is strategic discipline, not lack of ambition.
+
+Sources: [Team 6328 2026 build thread](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2026-build-thread/509595),
+[Team 5119 Open Alliance thread](https://www.chiefdelphi.com/t/frc-5119-team-steam-2026-build-thread-open-alliance/507554),
+[2026 technical binder](https://www.chiefdelphi.com/uploads/short-url/aiIYU0Pv6kleLK5yW5mDDAaE2n9.pdf)
+
+**Team routine:** every added feature must name the match advantage, owner,
+test, schedule cost, weight cost, power cost, and fallback. If those cannot be
+stated, the feature is not ready to enter the plan.
+
+### 1.3 Reuse proven ideas and innovate selectively
+
+**Classification: Foundation**
+
+High-performing teams study past robots and current prototypes. They reuse
+known-good geometry, controls, and construction methods, then reserve invention
+for the few unknowns that differentiate their strategy. Team 5119 explicitly
+looked to successful historical robots rather than reinventing every mechanism;
+Team 353 credited designs from teams 6328, 4481, and 2017 while adapting them to
+its own package.
+
+Sources: [Team 5119 Open Alliance thread](https://www.chiefdelphi.com/t/frc-5119-team-steam-2026-build-thread-open-alliance/507554),
+[Team 353 Open Alliance thread](https://www.chiefdelphi.com/t/pobots-353-2026-open-alliance-build-blog/509221)
+
+This is not “copy without understanding.” Record the source, governing
+dimensions, expected operating range, and what changed in the adaptation.
+
+## 2. Design And Mechanical Execution
+
+### 2.1 Prototype the unknown, with a pass/fail question
+
+**Classification: Foundation**
+
+A prototype should answer a decision: required compression, achievable range,
+cycle time, impact survival, centering reliability, or packaging envelope.
+Team 581 built an early Alpha Bot and several mechanism prototypes, then
+discarded a visually plausible dumper archetype when testing showed it was not
+consistent enough. Team 4096 moved prototypes into CAD and laser-cut parts to
+raise their fidelity and shorten the path into the final design.
+
+Sources: [Team 581 2026 CAD and code release](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762),
+[Team 4096 Open Alliance thread](https://www.chiefdelphi.com/t/frc-team-4096-ctrl-z-2026-build-thread-open-alliance/512198)
+
+**Team routine:** every prototype card should include its question, variables,
+measurement method, decision threshold, owner, and deadline. Video without a
+decision is documentation, not validation.
+
+### 2.2 Integrate early enough to discover system failures
+
+**Classification: Foundation**
+
+Mechanisms that work alone can still fail because of frame flex, ball flow,
+software timing, battery sag, cable motion, collisions, or driver behavior.
+Strong teams seek those interactions before competition. Team 5010 delivered a
+second robot iteration to software and drive after learning from early testing.
+Team 4561 used practice-field cycles to improve both autonomous consistency and
+its intake. Team 5667's retrospective provides the inverse lesson: late
+mechanical completion left roughly a week and a half for programming, exposed
+intake failures, and forced a late climber removal during a weight scare.
+
+Sources: [Team 5010 Open Alliance thread](https://www.chiefdelphi.com/t/frc-5010-tiger-dynasty-2026-build-thread-open-alliance/510219),
+[Team 4561 Open Alliance thread](https://www.chiefdelphi.com/t/4561-the-terrorbytes-2026-build-thread-open-alliance/508623),
+[Team 5667 season wrap-up](https://www.chiefdelphi.com/t/digital-eagles-5667-2026-build-blog-open-alliance/509725?page=2)
+
+**Team routine:** set a driveable-robot deadline, then a competition-feature
+freeze. Protect the interval between them for cycling, autos, tuning, repair,
+and practice.
+
+### 2.3 Budget mass, volume, power, and schedule continuously
+
+**Classification: Foundation**
+
+Resource limits are design inputs, not end-of-build surprises. CAD assemblies
+should carry realistic materials and masses; electrical design should estimate
+simultaneous current; mechanism owners should include wiring, fasteners,
+guards, and repair clearance. Team 2102 publicly tracked full-assembly mass and
+reserved approximately ten pounds for cable chain, wiring, and later changes
+rather than treating the modeled mechanism mass as finished weight.
+
+Source: [Team 2102 2026-2027 Open Alliance thread](https://www.chiefdelphi.com/t/2102-build-thread-2026-2027-open-alliance/521559)
+
+Recommended release gates:
+
+- **Architecture review:** every required function fits in space and power.
+- **Detailed-design review:** interfaces, fasteners, wire paths, sensors,
+ service access, and realistic mass are present.
+- **Manufacturing release:** drawings, stock, tooling, owners, and inspection
+ criteria are ready.
+
+### 2.4 Design exposed mechanisms to survive contact
+
+**Classification: Foundation**
+
+Intakes, sensors, cameras, cable runs, and low mechanisms should be evaluated as
+collision systems. Use load paths, hard stops, crash bars, compliant elements,
+and replaceable wear parts. Teams 2102 and 2826 described explicit crash
+protection in their 2026 designs; several post-event reports changed intakes
+only after realistic drive and defense exposure.
+
+Sources: [Team 2102 Open Alliance thread](https://www.chiefdelphi.com/t/2102-build-thread-2026-2027-open-alliance/521559),
+[Team 2826 Open Alliance thread](https://www.chiefdelphi.com/t/frc-2826-wave-robotics-2026-open-alliance-build-thread/508460)
+
+### 2.5 Design for diagnosis and repair
+
+**Classification: Foundation**
+
+Strong competition robots are not merely durable; predictable failures can be
+found and repaired inside a match turnaround. Critical assemblies need visible
+inspection points, accessible connectors and fasteners, consistent hardware,
+replaceable modules, and staged spares. Add witness marks where fasteners can
+move and make wear limits measurable.
+
+**Az-RBSI recommendation:** pair each mechanism class with a documented health
+view and pit test command. A replacement should require calibration or homing
+through one named workflow, not ad-hoc code edits.
+
+### 2.6 Provide representative hardware access; do not fetishize robot count
+
+**Classification: Multiplier**
+
+Many strong programs build an alpha robot, practice robot, spare mechanisms,
+or retain an older chassis. The underlying practice is to let programmers and
+drivers work while the competition machine is still being built or serviced.
+Team 581 used Alpha, Practice, and Competition robots; Team 5406 kept an
+earlier robot available for practice. A lower-resource team can meet the same
+goal with a kit chassis, simulator, mechanism rig, or scheduled access blocks.
+
+Sources: [Team 581 2026 CAD and code release](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762),
+[Team 5406 Open Alliance thread](https://www.chiefdelphi.com/t/celt-x-5406-2026-build-thread-open-alliance/511200)
+
+Three robots are conditional. Early, representative access is the transferable
+practice.
+
+## 3. Electrical And Controls Hardware
+
+### 3.1 Treat wiring as a mechanical subsystem
+
+**Classification: Foundation**
+
+Electrical reliability comes from connector retention, strain relief, service
+loops, abrasion protection, mounting, bend radius, labeling, and planned cable
+motion—not merely a correct schematic. Team 5406 responded to a controller
+disconnect by improving strain relief and service loops. Team 4322 attributed a
+long run without radio, CAN, vision-power, USB, or code failures to a deliberate
+electrical-reliability focus, until a main-breaker event exposed the next weak
+link.
+
+Sources: [Team 5406 Open Alliance thread](https://www.chiefdelphi.com/t/celt-x-5406-2026-build-thread-open-alliance/511200),
+[Team 4322 Open Alliance thread](https://www.chiefdelphi.com/t/4322-clockwork-2026-build-thread-open-alliance/511196)
+
+Required design-review questions:
+
+- Can a connector carry tension, vibrate loose, or be struck?
+- Can every high-current termination be inspected and torque-checked?
+- Does every moving mechanism have a defined cable path at both limits?
+- Can the radio, main controller, switch, and vision system be independently
+ identified, rebooted, and replaced?
+
+### 3.2 Standardize, label, inspect, and stock known-good spares
+
+**Classification: Foundation**
+
+Use a small set of connector, wire, fastener, sensor, and controller practices.
+Label both ends. Store exported device configurations and known-good firmware
+versions. Build critical spares before they are needed and verify them on the
+robot or a fixture.
+
+At events, use a fresh, charged, load-tested, tracked battery and inspect the
+main power path after every match. A battery-management system can be a sheet
+or tags; consistency matters more than sophistication.
+
+### 3.3 Design for power and network degradation
+
+**Classification: Foundation**
+
+Current limits, brownout behavior, CAN utilization, loop timing, camera
+bandwidth, controller temperature, and radio health should be visible before
+they become driver symptoms. Mechanisms need priorities so a transient load
+does not turn into a whole-robot failure.
+
+Team 581 implemented a power manager. Team 6328 reported that an accumulation
+of software and vision work caused loop overruns, illustrating that compute and
+network budgets must be managed like mass and current.
+
+Sources: [Team 581 2026 CAD and code release](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762),
+[Team 6328 2026 build thread](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2026-build-thread/509595)
+
+## 4. Software And Automation
+
+### 4.1 Start software before the final robot exists
+
+**Classification: Foundation**
+
+Strong programs prepare the repository, dependency versions, simulated IO,
+logging, controller mappings, and baseline drive code before mechanisms arrive.
+Team 157 had its repository and template ready at kickoff, used simulation in
+AdvantageScope, and created early prototype hardware so software could start.
+
+Source: [Team 157 Open Alliance thread](https://www.chiefdelphi.com/t/frc-157-the-aztechs-2026-build-thread-open-alliance/509425)
+
+**Az-RBSI recommendation:** a generated project should boot in simulation,
+exercise every configured subsystem, and show expected telemetry before any
+CAN device is connected.
+
+### 4.2 Keep one authoritative state model
+
+**Classification: Foundation**
+
+The robot should have authoritative sources for pose, mechanism position,
+match state, and current intent. Commands consume that state instead of
+reconstructing it through scattered booleans. Global field localization should
+support broad navigation; a tag-relative or mechanism-local observation can
+take over for the last precise alignment when appropriate.
+
+Teams 6328 and 1360 have described that global-plus-local pattern. It reduces
+dependence on a globally perfect estimate while retaining field-wide autonomy.
+
+Sources: [Team 6328 localization discussion](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/476206),
+[Team 1360 Open Alliance thread](https://www.chiefdelphi.com/t/frc-1360-2026-build-thread/507908)
+
+See [Pose, Odometry, and Vision Integration](Pose-Odometry-Vision-Integration.md)
+for the Az-RBSI implementation details.
+
+### 4.3 Log facts, decisions, and health—not just setpoints
+
+**Classification: Foundation**
+
+Top-level debugging depends on reconstructing what the robot observed and why
+it acted. Log sensor inputs, outputs, pose observations and acceptance reasons,
+command transitions, driver requests, safety inhibits, loop timing, power, and
+faults. Team 9280 describes logs as the source of truth and uses the same
+software paths for real operation, simulation, and replay.
+
+Source: [Team 9280 2026 Open Alliance software post](https://www.chiefdelphi.com/t/high-altitude-9280-build-thread-2026-open-alliance/509708?page=2)
+
+**Az-RBSI recommendation:** define a stable telemetry contract and a standard
+AdvantageScope layout. A pit crew should be able to answer “sensor, wiring,
+configuration, mechanism, or decision logic?” from one match log.
+
+### 4.4 Automate execution while preserving obvious fallbacks
+
+**Classification: Foundation**
+
+Driver assists should aim, stage, coordinate, protect, and recover mechanisms,
+but a failed camera or estimator must not make the robot unusable. Team 6328
+added fixed-distance presets and a force-launch control after vision and
+auto-aim failures at Week Zero. Team 581 provided preset-shot and dashboard
+fallbacks alongside more advanced automation.
+
+Sources: [Team 6328 override controls](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2026-build-thread/509595/606),
+[Team 581 2026 CAD and code release](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+
+Every automated action should define:
+
+- its required sensors and confidence;
+- a timeout and interruption path;
+- a lower-capability fallback;
+- a direct driver override; and
+- a logged explanation of why it was blocked or changed.
+
+See [Robot Decision Automation](Robot-Decision-Automation.md) for the proposed
+intent, world-state, and superstructure model.
+
+### 4.5 Test the same code in simulation, replay, and hardware
+
+**Classification: Multiplier**
+
+Simulation is most valuable when it runs production commands and subsystem
+logic, not a parallel demonstration. Replay is most valuable when logged inputs
+can reproduce a decision without energizing hardware. Pair both with hardware
+tests for timing, friction, impacts, radio behavior, and sensor limitations.
+
+Do not confuse a convincing field animation with validation. Maintain explicit
+acceptance tests for pose error, cycle time, shot error, autonomous completion,
+recovery, and degraded modes.
+
+### 4.6 Keep complexity inside measured compute and maintenance budgets
+
+**Classification: Foundation**
+
+Advanced teams remove features that cost more reliability or practice than they
+return in points. Profile periodic-loop time, network load, log volume, garbage
+collection, and vision processing. Prefer bounded algorithms, explicit state,
+and inspectable commands over opaque layers that only one student understands.
+
+Repository discipline supports this: use version control, review important
+changes, protect the deployable branch, tag event releases, and retain a known-
+good rollback point. The goal is quick recovery, not ceremony.
+
+### 4.7 Test autonomous routines on a representative field
+
+**Classification: Foundation**
+
+An autonomous routine is not complete when it traces correctly once. Test
+start-pose error, game pieces, boundaries, bumps, battery variation, missed
+acquisition, obstruction, and interruption. Team 4561 attributed a consistent
+2.5-cycle autonomous routine to practice-field testing. Team 581 added
+automatic unbeaching so a field interaction did not necessarily end the rest
+of the routine.
+
+Sources: [Team 4561 Open Alliance thread](https://www.chiefdelphi.com/t/4561-the-terrorbytes-2026-build-thread-open-alliance/508623),
+[Team 581 2026 CAD and code release](https://www.chiefdelphi.com/t/581-blazing-bulldogs-2026-cad-and-code-release/521762)
+
+## 5. Driver Development
+
+### 5.1 Protect substantial, purposeful drive practice
+
+**Classification: Foundation**
+
+Strong teams do not treat practice as whatever time remains. They schedule it,
+provide field access, measure repeatable tasks, and feed driver observations
+back into mechanical and software changes. Team 4096 reported that dedicated
+code and drive practice exposed strengths and weaknesses that shaped its next
+robot iteration. Team 1730 hosted “Practice with a Purpose” on a shared full
+field, and Team 360 described community collaboration around a full practice
+field.
+
+Sources: [Team 4096 Open Alliance thread](https://www.chiefdelphi.com/t/frc-team-4096-ctrl-z-2026-build-thread-open-alliance/512198),
+[Team 1730 Open Alliance thread](https://www.chiefdelphi.com/t/1730-team-driven-2026-build-thread-open-alliance/508101),
+[Team 360 Open Alliance thread](https://www.chiefdelphi.com/t/frc-360-the-revolution-2026-build-thread-open-alliance/510290)
+
+Practice should include:
+
+- repeatable acquisition, scoring, traversal, and endgame drills;
+- full match cycles with realistic traffic and game-piece depletion;
+- defense from and against the robot;
+- degraded vision, failed automation, jams, and manual recovery;
+- autonomous-to-teleop handoff and controller reconnects; and
+- concise driver feedback after each block.
+
+### 5.2 Make controls stable, sparse, and explainable
+
+**Classification: Foundation**
+
+Essential controls should fit the drive team's practiced mental model. Prefer
+high-level intent controls, consistent physical locations, tactile separation,
+and deliberate confirmation for dangerous actions. Avoid remapping controls at
+an event unless the expected benefit exceeds the new execution risk.
+
+Az-RBSI controller APIs should name inputs by physical position while robot
+bindings name the intent. This preserves reusable controller code without
+hiding the action assigned in a particular robot project.
+
+### 5.3 Practice communications and contingencies as part of driving
+
+**Classification: Foundation**
+
+The drive team should have short calls for role changes, defense, passing,
+failed mechanisms, endgame timing, and emergency disable. Pre-match strategy
+must identify the alliance plan, starting locations, autonomous interactions,
+traffic lanes, likely defense, and what changes if a partner or mechanism
+fails.
+
+## 6. Competition Operations
+
+### 6.1 Use specific pre-match and post-match checklists
+
+**Classification: Foundation**
+
+Memory is unreliable under event pressure. A checklist should be ordered,
+specific to the robot, practiced, and updated after failures. It should assign
+who inspects structure, mechanisms, electrical, software, battery, and final
+functional state.
+
+Team 7691's 2026 pit system records issues and fixes, divides post-match work
+among programming, electrical, and mechanical roles, then performs a power
+cycle, system check, and signoff. Chief Delphi's 2026 checklist discussion also
+emphasized concrete inspection items and evolving the list from observed
+failures.
+
+Sources: [Team 7691 pit-system post](https://www.chiefdelphi.com/t/frc-7691-2025-2026-build-thread-open-alliance/506859?page=3),
+[pre-match checklist discussion](https://www.chiefdelphi.com/t/pre-match-checklists/473008)
+
+Minimum post-match loop:
+
+1. Isolate and power down safely.
+2. Record driver symptoms before changing anything.
+3. Inspect known wear and impact points.
+4. Review faults and the relevant log interval.
+5. Repair with one accountable owner.
+6. Power, calibrate, and run a short functional test.
+7. Install and record the next battery.
+8. Obtain a final ready/not-ready signoff.
+
+### 6.2 Make robot and pit status visible
+
+**Classification: Foundation**
+
+Everyone should know whether the robot is safe to approach, under repair,
+awaiting software, ready for functional test, or ready to queue. Team 7691 used
+a color-coded pit status and a digital system for schedule, notes, checklists,
+parts, batteries, and break logs. The exact app is conditional; a visible state
+and one responsible coordinator are not.
+
+Source: [Team 7691 pit-system post](https://www.chiefdelphi.com/t/frc-7691-2025-2026-build-thread-open-alliance/506859?page=3)
+
+### 6.3 Self-inspect early and keep the robot inspection-ready
+
+**Classification: Foundation**
+
+Use the current official checklist before load-in, after consequential changes,
+and before leaving the shop for the next event. Ask for pre-inspection when the
+event permits it. Keep evidence such as component documentation accessible.
+PRECHECK was offered in 2026 as a friendlier self-inspection interface with
+links to the underlying rules.
+
+Source: [2026 inspection-checklist discussion](https://www.chiefdelphi.com/t/2026-team-update-07/513642)
+
+### 6.4 Record failures before fixing them
+
+**Classification: Foundation**
+
+Capture the match, timestamp, symptom, driver observation, fault/log evidence,
+root cause, repair, and verification. This converts event chaos into reliability
+engineering and prevents the same symptom from receiving a different guess
+each time.
+
+Use a short post-match debrief, not a long design meeting. Schedule deeper
+analysis away from queue pressure.
+
+### 6.5 Bring staged spares, tools, and configuration recovery
+
+**Classification: Multiplier**
+
+Prioritize spares by probability, consequence, and repair time. Complete
+mechanism or wiring modules are valuable when they reduce turnaround, but
+unverified spares can create a second failure. Include controller configuration,
+calibration data, deployable software, firmware, and network settings in the
+recovery kit.
+
+### 6.6 Control event changes
+
+**Classification: Foundation**
+
+At competition, define the problem before modifying the robot. Change one
+causal variable when practical, record it, test it, and retain a rollback path.
+Risky feature work should not displace a functioning match configuration
+without an explicit strategy decision.
+
+## 7. Scouting And Match Strategy
+
+### 7.1 Collect the smallest trustworthy dataset that changes decisions
+
+**Classification: Foundation**
+
+Strong scouting systems are designed around match strategy and alliance
+selection, not around collecting every observable fact. Combine quantitative
+production with qualitative observations such as preferred paths, failure
+modes, defense behavior, recovery, driver control, and compatibility.
+
+Team 9483—drawing on experience at high levels of play—described pre-scouting
+prior matches, combining pit and match scouting, and managing scout fatigue and
+data quality. Team 5572's 2026 retrospective similarly identified training,
+rotation, missing submissions, and data freshness as operational constraints.
+
+Sources: [Team 9483 scouting thread](https://www.chiefdelphi.com/t/9483-scouting-saga-from-vibes-and-paper-to-predictive-data/521985),
+[Team 5572 scouting overview](https://www.chiefdelphi.com/t/frc-5572-scouting-system-data-and-strategy-overview-2026/519045)
+
+### 7.2 Validate data and retain human observations
+
+**Classification: Foundation**
+
+Automated rankings, EPA, and dashboards are inputs, not decisions. Flag
+impossible values, missing matches, defense-distorted data, and role changes.
+Watch candidates directly before alliance selection and record evidence for
+disagreements. An alliance partner should be selected for the intended playoff
+role and compatibility, not merely the largest aggregate number.
+
+### 7.3 Collaborate when it improves coverage or quality
+
+**Classification: Multiplier**
+
+Scouting alliances can reduce labor and improve coverage if teams agree on
+definitions, training, validation, data ownership, and failure recovery. A 2026
+Archimedes alliance used shared ScoutRadioz forms and a second pit-scouting
+round before alliance selection. Team 1676 also maintained a public 2026
+Championship pre-scouting database to avoid duplicated work.
+
+Sources: [2026 Archimedes scouting alliance](https://www.chiefdelphi.com/t/2026-archimedes-scouting-alliance/519248),
+[2026 Championship pre-scouting database](https://www.chiefdelphi.com/t/2026-world-championship-public-pre-scouting-database/519278)
+
+### 7.4 Turn scouting into an executable match plan
+
+**Classification: Foundation**
+
+Before every match, agree on autonomous positions, collision risks, scoring or
+passing roles, traffic lanes, expected hub timing, defensive assignments,
+endgame responsibilities, and contingency calls. The output must be short
+enough for all three drive teams to remember.
+
+## 8. Team Process And Learning
+
+### 8.1 Assign ownership without creating knowledge silos
+
+**Classification: Foundation**
+
+Every deliverable and event role needs an owner, a reviewer or backup, a due
+date, and an acceptance condition. Train at least one backup for deploy,
+electrical diagnosis, calibration, drive operation, scouting administration,
+and critical repairs.
+
+### 8.2 Document decisions and publish enough to be accountable
+
+**Classification: Multiplier**
+
+Design reviews, technical binders, code releases, and Open Alliance posts force
+teams to explain what they chose and what happened. Team 1710 published code,
+CAD, and a technical binder; many of the strongest 2026 sources used here also
+included candid failure reports rather than only reveal videos.
+
+Source: [Team 1710 resources](https://www.chiefdelphi.com/t/frc-1710-resources/520754)
+
+Public release is conditional. Internal decision records, setup instructions,
+and post-event retrospectives are foundational.
+
+### 8.3 Run short feedback loops
+
+**Classification: Foundation**
+
+After prototypes, practice blocks, matches, and events, ask:
+
+1. What outcome did we expect?
+2. What actually happened?
+3. What evidence distinguishes possible causes?
+4. What is the smallest useful change?
+5. How will we verify it and prevent regression?
+
+Keep the record blame-free and technically specific. A failure found during
+practice is useful schedule information; the same failure rediscovered in
+eliminations is process debt.
+
+### 8.4 Preserve margin and team sustainability
+
+**Classification: Foundation**
+
+Time, weight, power, money, attention, and student energy are all finite.
+Schedule meals and rest at events, rotate scouts, constrain late-night heroics,
+and maintain technical scope the team can explain and repair. Sustainable
+execution across an event is a competitive capability.
+
+## Practices That Top Teams Do **Not** All Share
+
+Do not convert visible correlation into a universal requirement. The source
+material does not support “top teams all” doing any of the following:
+
+- building two or three complete robots;
+- using swerve rather than tank drive;
+- using a turret or shooting while moving;
+- using a particular motor, camera, controller, or vendordep;
+- writing a custom scouting application;
+- using full-field global localization for every precision action;
+- maximizing mechanism count;
+- using one command framework or software architecture; or
+- having a large team, large shop, or large budget.
+
+The transferable lesson behind most of these is access, testability,
+observability, strategic fit, or reduced cognitive load. Implement that lesson
+at the scale the team can sustain.
+
+## What Az-RBSI Should Encode
+
+The following should become opinionated defaults or supported templates in the
+RBSI toolchain:
+
+| Area | Recommended RBSI Capability |
+| --- | --- |
+| Bring-up | Simulated boot, subsystem self-tests, device/configuration inventory, and explicit calibration status. |
+| Reliability | Health signals for power, CAN, loop timing, controllers, cameras, sensors, and mechanism progress. |
+| Observability | Stable log schema, command/intent transitions, vision accept/reject reasons, faults, and standard AdvantageScope layouts. |
+| Degraded operation | Named sensor dependencies, timeouts, preset/manual fallbacks, driver overrides, and dashboard disable switches. |
+| Autonomous | Start-pose validation, path/field overlays, event-marker logging, interruption tests, and realistic simulation/replay. |
+| Controls | Physical controller names in reusable controller classes; semantic intent names in robot bindings. |
+| Pit support | Safe mechanism test commands, calibration workflow, configuration export/restore, and a one-page health summary. |
+| Process templates | Strategy priorities, prototype card, design-review checklist, software release record, pre/post-match checklist, and failure report. |
+
+Az-RBSI should **not** prescribe a competitive archetype, mechanism count,
+vendor ecosystem, scouting platform, or number of robots. Those are team and
+game decisions.
+
+## Suggested Adoption Sequence
+
+### Before kickoff
+
+1. Establish repository, simulation, logging, deployment, and rollback.
+2. Build controller, battery, wiring, inspection, and pit-checklist standards.
+3. Train backups and rehearse diagnosis on an existing robot.
+4. Prepare strategy, prototype, design-review, and retrospective templates.
+
+### Weeks 1–2
+
+1. Complete game analysis and prioritized requirements.
+2. Prototype only the decisions that can change architecture.
+3. Select a deliberately bounded robot role.
+4. Release chassis and mechanism interfaces with mass and service margin.
+
+### Weeks 3–4
+
+1. Integrate the first driveable system.
+2. Give software and drivers representative access.
+3. Establish baseline autos and manual fallbacks.
+4. Start full-system reliability cycling and log review.
+
+### Before the first event
+
+1. Freeze competition functionality early enough to practice it.
+2. Run full matches, defense, degraded modes, and repair turnarounds.
+3. Self-inspect and verify spares, batteries, configuration recovery, and
+ checklists.
+4. Train scouts and rehearse the pre-match strategy workflow.
+
+### At every event
+
+1. Execute the pre-match plan and checklist.
+2. Capture symptoms and logs immediately after the match.
+3. Inspect, repair, test, and sign off through named roles.
+4. Update scouting and strategy from current evidence.
+5. Make controlled changes with a rollback path.
+
+## Bottom Line
+
+The strongest recurring 2026 pattern is not “build the most advanced robot.”
+It is: **make explicit choices, expose uncertainty early, preserve practice
+time, measure the real system, degrade gracefully, and learn faster than the
+event schedule changes.**
+
+Az-RBSI can help by making a well-instrumented, testable, recoverable robot the
+easy default. The team must supply the strategic restraint and operational
+discipline that turn those capabilities into match performance.
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
index a4b76b95..d997cfc6 100644
Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
index 8e975a5f..8a5d8935 100644
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,6 +1,6 @@
distributionBase=GRADLE_USER_HOME
distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-9.4.1-bin.zip
networkTimeout=10000
validateDistributionUrl=true
zipStoreBase=GRADLE_USER_HOME
diff --git a/gradlew b/gradlew
index f5feea6d..739907df 100755
--- a/gradlew
+++ b/gradlew
@@ -1,7 +1,7 @@
#!/bin/sh
#
-# Copyright © 2015-2021 the original authors.
+# Copyright © 2015 the original authors.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -57,7 +57,7 @@
# Darwin, MinGW, and NonStop.
#
# (3) This script is generated from the Groovy template
-# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+# https://github.com/gradle/gradle/blob/2d6327017519d23b96af35865dc997fcb544fb40/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
# within the Gradle project.
#
# You can find Gradle at https://github.com/gradle/gradle/.
@@ -86,8 +86,7 @@ done
# shellcheck disable=SC2034
APP_BASE_NAME=${0##*/}
# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
-APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
-' "$PWD" ) || exit
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s\n' "$PWD" ) || exit
# Use the maximum available, or set MAX_FD != -1 to use that value.
MAX_FD=maximum
@@ -115,7 +114,6 @@ case "$( uname )" in #(
NONSTOP* ) nonstop=true ;;
esac
-CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
# Determine the Java command to use to start the JVM.
@@ -173,7 +171,6 @@ fi
# For Cygwin or MSYS, switch paths to Windows format before running java
if "$cygwin" || "$msys" ; then
APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
- CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
JAVACMD=$( cygpath --unix "$JAVACMD" )
@@ -206,15 +203,14 @@ fi
DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
# Collect all arguments for the java command:
-# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
# and any embedded shellness will be escaped.
# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
# treated as '${Hostname}' itself on the command line.
set -- \
"-Dorg.gradle.appname=$APP_BASE_NAME" \
- -classpath "$CLASSPATH" \
- org.gradle.wrapper.GradleWrapperMain \
+ -jar "$APP_HOME/gradle/wrapper/gradle-wrapper.jar" \
"$@"
# Stop when "xargs" is not available.
diff --git a/gradlew.bat b/gradlew.bat
index 9b42019c..e509b2dd 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -70,11 +70,10 @@ goto fail
:execute
@rem Setup the command line
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -jar "%APP_HOME%\gradle\wrapper\gradle-wrapper.jar" %*
:end
@rem End local scope for the variables with windows NT shell
diff --git a/settings.gradle b/settings.gradle
index 25f6f6e8..e2626a81 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -2,27 +2,27 @@ import org.gradle.internal.os.OperatingSystem
pluginManagement {
repositories {
- mavenLocal()
- gradlePluginPortal()
- String frcYear = '2026'
- File frcHome
+ String wpilibYear = '2027_alpha5'
+ File wpilibHome
if (OperatingSystem.current().isWindows()) {
String publicFolder = System.getenv('PUBLIC')
if (publicFolder == null) {
publicFolder = "C:\\Users\\Public"
}
def homeRoot = new File(publicFolder, "wpilib")
- frcHome = new File(homeRoot, frcYear)
+ wpilibHome = new File(homeRoot, wpilibYear)
} else {
def userFolder = System.getProperty("user.home")
def homeRoot = new File(userFolder, "wpilib")
- frcHome = new File(homeRoot, frcYear)
+ wpilibHome = new File(homeRoot, wpilibYear)
}
- def frcHomeMaven = new File(frcHome, 'maven')
+ def wpilibHomeMaven = new File(wpilibHome, 'maven')
maven {
- name = 'frcHome'
- url = frcHomeMaven
+ name = 'wpilibHome'
+ url = wpilibHomeMaven
}
+ mavenLocal()
+ gradlePluginPortal()
}
}
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/first/Main.java
similarity index 50%
rename from src/main/java/frc/robot/Main.java
rename to src/main/java/first/Main.java
index f417e18b..8fd1dabb 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/first/Main.java
@@ -1,23 +1,10 @@
-// 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;
+package first;
-import edu.wpi.first.wpilibj.RobotBase;
+import org.wpilib.framework.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
@@ -33,6 +20,6 @@ private Main() {}
* If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
+ RobotBase.startRobot(frc.robot.Robot.class);
}
}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index bf7af0ec..b64019b5 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -17,7 +17,7 @@
package frc.robot;
-import static edu.wpi.first.units.Units.*;
+import static org.wpilib.units.Units.*;
import com.pathplanner.lib.config.ModuleConfig;
import com.pathplanner.lib.config.PIDConstants;
@@ -25,21 +25,10 @@
import com.therekrab.autopilot.APConstraints;
import com.therekrab.autopilot.APProfile;
import com.therekrab.autopilot.Autopilot;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-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;
import frc.robot.FieldConstants.AprilTagLayoutType;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.SwerveConstants;
import frc.robot.util.Alert;
-import frc.robot.util.RBSIController.Axis;
-import frc.robot.util.RBSIController.Button;
import frc.robot.util.RBSIEnum.AutoType;
import frc.robot.util.RBSIEnum.CTREPro;
import frc.robot.util.RBSIEnum.DriveStyle;
@@ -49,9 +38,17 @@
import frc.robot.util.RBSIEnum.VisionType;
import frc.robot.util.RobotDeviceId;
import java.util.Set;
+import java.util.stream.Stream;
import org.littletonrobotics.junction.Logger;
import org.photonvision.simulation.SimCameraProperties;
-import swervelib.math.Matter;
+import org.wpilib.framework.RobotBase;
+import org.wpilib.hardware.power.PowerDistribution;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.system.DCMotor;
+import org.wpilib.math.util.Units;
+import org.wpilib.units.measure.Mass;
/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -118,8 +115,6 @@ public static void disableHAL() {
public static final class RobotConstants {
public static final Mass kMass = Pounds.of(100.);
- public static final Matter kChassisMatter =
- new Matter(new Translation3d(0, 0, Inches.of(8).in(Meters)), kMass.in(Kilograms));
// Robot moment of inertia; this can be obtained from a CAD model of your drivetrain. Usually,
// this is between 3 and 8 kg*m^2.
public static final double kMomentOfInertiaKgMetersSq = 6.8;
@@ -154,8 +149,9 @@ public static final class RobotConstants {
public static final class PowerConstants {
// Power Distribution Module Configuration
- public static final PowerDistribution.ModuleType kPdmType = PowerDistribution.ModuleType.kRev;
+ public static final PowerDistribution.ModuleType kPdmType = PowerDistribution.ModuleType.REV;
public static final int kPdmCanId = 1;
+ public static final int kPdmChannelCount = 24;
// Current Limits
public static final double kTotalMaxCurrentAmps = 120.;
@@ -171,10 +167,39 @@ public static final class PowerConstants {
/************************************************************************* */
/** List of Robot CAN Busses ********************************************* */
public static final class CANBuses {
- public static final String RIO = "rio";
- public static final String DRIVE = "DriveTrain";
-
- public static final String[] ALL = {RIO, DRIVE};
+ // SystemCore's five native CAN interfaces, ordered left to right.
+ public static final String SYSTEMCORE_0 = "can_s0";
+ public static final String SYSTEMCORE_1 = "can_s1";
+ public static final String SYSTEMCORE_2 = "can_s2";
+ public static final String SYSTEMCORE_3 = "can_s3";
+ public static final String SYSTEMCORE_4 = "can_s4";
+ // The default mechanism bus. This is a SystemCore port, not a roboRIO CAN bus.
+ public static final String RIO = SYSTEMCORE_1;
+
+ // Register and health-monitor every bus actually selected below.
+ public static final String[] ALL =
+ Stream.of(
+ RIO,
+ DrivebaseConstants.kPigeonCANBus,
+ DrivebaseConstants.kFLCANBus,
+ DrivebaseConstants.kFRCANBus,
+ DrivebaseConstants.kBLCANBus,
+ DrivebaseConstants.kBRCANBus)
+ .distinct()
+ .toArray(String[]::new);
+
+ /** REV's 2027 constructors use a numbered SystemCore port rather than a Phoenix bus name. */
+ public static int revPort(String bus) {
+ return switch (bus) {
+ case SYSTEMCORE_0 -> 0;
+ case SYSTEMCORE_1 -> 1;
+ case SYSTEMCORE_2 -> 2;
+ case SYSTEMCORE_3 -> 3;
+ case SYSTEMCORE_4 -> 4;
+ default ->
+ throw new IllegalArgumentException("REV device requires a SystemCore CAN bus: " + bus);
+ };
+ }
}
/************************************************************************* */
@@ -215,7 +240,7 @@ public static class RobotDevices {
new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null);
// Pigeon
public static final RobotDeviceId PIGEON =
- new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kCANbusName, null);
+ new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kPigeonCanbus, null);
/* SUBSYSTEM CAN DEVICE IDS */
// This is where mechanism subsystem devices are defined (Including ID, bus, and power port)
@@ -284,42 +309,19 @@ public static class OperatorConstants {
}
/************************************************************************* */
- /** Driver Controller Button Constants *********************************** */
- public static final class ControllerButtonConstants {
- // Face buttons are named by physical position. RBSIController maps these to Xbox, PS4, or PS5
- // names for the detected controller.
-
- // DRIVER BUTTONS
- public static final Button ROBOT_RELATIVE = Button.EAST_FACE; // Xbox B / PS Circle
- public static final Button BRAKE = Button.SOUTH_FACE; // Xbox A / PS Cross
- public static final Button X_LOCK = Button.WEST_FACE; // Xbox X / PS Square
- public static final Button ZERO_GYRO = Button.NORTH_FACE; // Xbox Y / PS Triangle
-
- // Bumpers
- public static final Button RUN_FLYWHEEL = Button.RIGHT_BUMPER; // Xbox RB / PS R1
- public static final Button AUTOPILOT_DEMO = Button.LEFT_BUMPER; // Xbox LB / PS L1
-
- // Triggers
- public static final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER;
- public static final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER;
-
- // D-PAD POV Buttons
- public static final Button NUDGE_LEFT = Button.POV_LEFT;
- public static final Button NUDGE_RIGHT = Button.POV_RIGHT;
- public static final Button NUDGE_FORWARD = Button.POV_UP;
- public static final Button NUDGE_BACK = Button.POV_DOWN;
-
- // Pressing the joysticks
- public static final Button LEFT_STICK_BUTTON = Button.LEFT_STICK;
- public static final Button RIGHT_STICK_BUTTON = Button.RIGHT_STICK;
-
- public static final double kTriggerPressedThreshold = 0.5;
- }
-
/************************************************************************* */
/** Drive Base Constants ************************************************* */
public static final class DrivebaseConstants {
+ // One bus per complete module (drive, steer, encoder), plus an independent Pigeon bus.
+ // Default: front on port 2, back on port 3, Pigeon on port 4, mechanisms on port 1.
+ // Assign any of these five values to another SYSTEMCORE_* bus to change the split.
+ public static final String kPigeonCANBus = CANBuses.SYSTEMCORE_4;
+ public static final String kFLCANBus = CANBuses.SYSTEMCORE_2;
+ public static final String kFRCANBus = CANBuses.SYSTEMCORE_2;
+ public static final String kBLCANBus = CANBuses.SYSTEMCORE_3;
+ public static final String kBRCANBus = CANBuses.SYSTEMCORE_3;
+
// Theoretical free speed (m/s) at 12v applied output;
// IMPORTANT: Follow the AdvantageKit instructions for measuring the ACTUAL maximum linear speed
// of YOUR ROBOT, and replace the estimate here with your measured value!
@@ -339,7 +341,7 @@ public static final class DrivebaseConstants {
// For Profiled PID Motion -- NEED TUNING!
// Used in a variety of contexts, including PathPlanner and AutoPilot
// Chassis (not module) across-the-field strafing motion
- public static final double kStrafeP = 5.0;
+ public static final double kStrafeP = 3.0;
public static final double kStrafeI = 0.0;
public static final double kStrafeD = 0.0;
// Chassis (not module) solid-body rotation
@@ -379,13 +381,15 @@ public static final class DrivebaseConstants {
public static final double kDriveA = 0.0;
public static final double kDriveS = 0.20;
public static final double kDriveT =
- SwerveConstants.kDriveGearRatio / DCMotor.getKrakenX60Foc(1).KtNMPerAmp;
+ SwerveConstants.kDriveGearRatio / DCMotor.getKrakenX60Foc(1).Kt;
public static final double kSteerP = 100.0;
public static final double kSteerD = 0.5;
public static final double kSteerS = 0.1;
// Odometry-related constants ==================================
public static final double kPoseBufferHistorySecs = 1.5;
+ public static final double kPathPlannerVisionFreshnessSec = 0.5;
+ public static final double kPathPlannerStartToleranceMeters = 0.5;
// 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;
diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java
index 85283c38..9780a989 100644
--- a/src/main/java/frc/robot/FieldConstants.java
+++ b/src/main/java/frc/robot/FieldConstants.java
@@ -19,15 +19,14 @@
package frc.robot;
-import static edu.wpi.first.units.Units.*;
+import static org.wpilib.units.Units.*;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.wpilibj.Filesystem;
import java.io.IOException;
+import java.nio.file.Files;
import java.nio.file.Path;
import lombok.Getter;
+import org.wpilib.system.Filesystem;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
/**
* Contains various field dimensions and useful reference points. All units are in meters and poses
@@ -62,22 +61,22 @@ public enum AprilTagLayoutType {
NONE_ANDYMARK("none-andymark");
AprilTagLayoutType(String name) {
+ Path layoutPath =
+ Constants.disableHAL
+ ? Path.of("src", "main", "deploy", "apriltags", name + ".json")
+ : Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json");
+
try {
- layout =
- new AprilTagFieldLayout(
- Constants.disableHAL
- ? Path.of("src", "main", "deploy", "apriltags", name + ".json")
- : Path.of(
- Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json"));
+ layout = new AprilTagFieldLayout(layoutPath);
} catch (IOException e) {
throw new RuntimeException(e);
}
try {
- layoutString = new ObjectMapper().writeValueAsString(layout);
- } catch (JsonProcessingException e) {
+ layoutString = Files.readString(layoutPath);
+ } catch (IOException e) {
throw new RuntimeException(
- "Failed to serialize AprilTag layout JSON " + toString() + "for PhotonVision");
+ "Failed to read AprilTag layout JSON " + toString() + " for PhotonVision", e);
}
}
diff --git a/src/main/java/frc/robot/FieldState.java b/src/main/java/frc/robot/FieldState.java
index 54322c6c..f4ce0e92 100644
--- a/src/main/java/frc/robot/FieldState.java
+++ b/src/main/java/frc/robot/FieldState.java
@@ -17,11 +17,21 @@
package frc.robot;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.util.Alert;
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.driverstation.RobotState;
-public class FieldState {
+public final class FieldState {
+ private static final double TRANSITION_END_SECONDS = 130.0;
+ private static final double SHIFT_DURATION_SECONDS = 25.0;
+ private static final double ENDGAME_START_SECONDS = 30.0;
+
+ private static final Alert missingHubDataAlert =
+ new Alert(
+ "No HUB data from FMS! HUB is listed as ACTIVE! Driver BEWARE!", Alert.AlertType.WARNING);
+
+ private FieldState() {}
/**
* FOR 2026 - REBUILT, store the data from the FMS about the TeleOp shifts here
@@ -52,55 +62,45 @@ public class FieldState {
* @return Whether the team's alliance's HUB is active right now
*/
public static boolean isHubActive() {
+ missingHubDataAlert.set(false);
// The HUB is active for both alliances in AUTO
- if (DriverStation.isAutonomous()) {
+ if (RobotState.isAutonomous()) {
return true;
}
// The HUB is not active when not in AUTO or TELEOP
- if (!DriverStation.isTeleop()) {
+ if (!RobotState.isTeleop()) {
return false;
}
// Read the approximate match time and alliance. The FMS does not send an official match time to
- // robots; DriverStation.getMatchTime() is approximate and should not be used to dispute ref
+ // robots; MatchState.getMatchTime() is approximate and should not be used to dispute ref
// calls or guarantee that a function will trigger before the match ends.
- double timeRemaining = DriverStation.getMatchTime();
- Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue);
+ double timeRemaining = MatchState.getMatchTime();
+ Alliance alliance = MatchState.getAlliance().orElse(Alliance.BLUE);
// If the FMS has not provided an alliance yet, set to TRUE and kick an Alert!
- if (timeRemaining < 130.0 && wonAuto == null) {
- new Alert(
- "No HUB data from FMS! HUB is listed as ACTIVE! Driver BEWARE!",
- Alert.AlertType.WARNING)
- .set(true);
+ boolean isMissingHubData = timeRemaining < TRANSITION_END_SECONDS && wonAuto == null;
+ missingHubDataAlert.set(isMissingHubData);
+ if (isMissingHubData) {
return true;
}
- if (timeRemaining >= 130.0) {
- // TRANSITION SHIFT -- Both HUBs active
+ // Both HUBs are active during transition and endgame.
+ if (timeRemaining >= TRANSITION_END_SECONDS || timeRemaining < ENDGAME_START_SECONDS) {
return true;
+ }
- } else if (timeRemaining >= 105.0) {
- // SHIFT 1 -- Non-winning alliance gets a first go
- return !(wonAuto == alliance);
-
- } else if (timeRemaining >= 80.0) {
- // SHIFT 2 -- Winning alliance gets a chance
- return (wonAuto == alliance);
-
- } else if (timeRemaining >= 55.0) {
- // SHIFT 3 -- Trade off
- return !(wonAuto == alliance);
-
- } else if (timeRemaining >= 30.0) {
- // SHIFT 4 -- Trade off again
- return (wonAuto == alliance);
+ // The non-winning alliance is active in odd shifts; the winning alliance is active in even
+ // shifts.
+ return isAllianceActiveDuringShift(timeRemaining, alliance, wonAuto);
+ }
- } else {
- // ENDGAME -- Both HUBs active
- return true;
- }
+ static boolean isAllianceActiveDuringShift(
+ double timeRemaining, Alliance alliance, Alliance autoWinner) {
+ int shift = (int) Math.ceil((TRANSITION_END_SECONDS - timeRemaining) / SHIFT_DURATION_SECONDS);
+ boolean wonAutoIsActive = shift % 2 == 0;
+ return wonAutoIsActive == (autoWinner == alliance);
}
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1f5f73cc..56df2fe7 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -17,14 +17,11 @@
package frc.robot;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.util.FlippingUtil;
import com.revrobotics.util.StatusLogger;
-import edu.wpi.first.wpilibj.DriverStation;
-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;
@@ -34,6 +31,13 @@
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
import org.littletonrobotics.urcl.URCL;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.CommandScheduler;
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.system.Threads;
+import org.wpilib.system.Timer;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -96,7 +100,8 @@ public Robot() {
// Initialize URCL
Logger.registerURCL(URCL.startExternal());
StatusLogger.disableAutoLogging(); // Disable REVLib's built-in logging
- LoggedPowerDistribution.getInstance(PowerConstants.kPdmCanId, PowerConstants.kPdmType);
+ LoggedPowerDistribution.getInstance(
+ PowerConstants.kPdmCanId, PowerConstants.kPdmChannelCount, PowerConstants.kPdmType);
// Start AdvantageKit logger
Logger.start();
@@ -111,7 +116,7 @@ public Robot() {
// Switch thread to high priority to improve loop timing
if (isReal()) {
- Threads.setCurrentThreadPriority(true, 99);
+ setCurrentThreadPriorityFor2027Migration(99);
}
}
@@ -122,7 +127,7 @@ public void robotPeriodic() {
if (isReal()) {
// Switch thread to high priority to improve loop timing
- Threads.setCurrentThreadPriority(true, 99);
+ setCurrentThreadPriorityFor2027Migration(99);
}
final long t1 = System.nanoTime();
@@ -140,7 +145,7 @@ public void robotPeriodic() {
if (isReal()) {
// Return thread to normal priority
- Threads.setCurrentThreadPriority(false, 10);
+ setCurrentThreadPriorityFor2027Migration(10);
}
final long t4 = System.nanoTime();
@@ -180,7 +185,6 @@ public void autonomousInit() {
CommandScheduler.getInstance().cancelAll();
m_robotContainer.getDrivebase().setMotorBrake(true);
m_robotContainer.getDrivebase().resetHeadingController();
- m_robotContainer.getVision().resetPoseGate(TimeUtil.now());
// Do not zero the gyro here. PathPlanner and Choreo reset through Drive.resetPose(...), which
// aligns the pose estimator to the selected auto's start while preserving the gyro reference.
@@ -194,6 +198,19 @@ public void autonomousInit() {
"Incorrect AUTO type selected in Constants: " + Constants.getAutoType());
};
+ if (m_autonomousCommand instanceof PathPlannerAuto pathPlannerAuto) {
+ Pose2d startingPose = pathPlannerAuto.getStartingPose();
+ if (startingPose != null) {
+ if (AutoBuilder.shouldFlip()) {
+ startingPose = FlippingUtil.flipFieldPose(startingPose);
+ }
+ Logger.recordOutput("Auto/StartingPose", startingPose);
+ if (!m_robotContainer.getDrivebase().validatePathPlannerAutoStart(startingPose)) {
+ m_autonomousCommand = null;
+ }
+ }
+ }
+
if (m_autonomousCommand != null) {
CommandScheduler.getInstance().schedule(m_autonomousCommand);
}
@@ -232,37 +249,38 @@ public void teleopPeriodic() {
//
// https://docs.wpilib.org/en/stable/docs/yearly-overview/2026-game-data.html
if (FieldState.wonAuto == null) {
- // Only call this code block if the signal from FMS has not yet arrived
- String gameData = DriverStation.getGameSpecificMessage();
- if (gameData.length() > 0) {
- switch (gameData.charAt(0)) {
- case 'B':
- // Blue case code
- FieldState.wonAuto = DriverStation.Alliance.Blue;
- break;
- case 'R':
- // Red case code
- FieldState.wonAuto = DriverStation.Alliance.Red;
- break;
- default:
- // This is corrupt data, do nothing
- break;
- }
- }
+ FieldState.wonAuto = parseAutoWinner(MatchState.getGameData().orElse(""));
}
- // Anything else for the teleopPeriodic() function
+ }
+ private static Alliance parseAutoWinner(String gameData) {
+ if (gameData.isEmpty()) {
+ return null;
+ }
+ return switch (gameData.charAt(0)) {
+ case 'B' -> Alliance.BLUE;
+ case 'R' -> Alliance.RED;
+ default -> null;
+ };
}
- /** This function is called once when test mode is enabled. */
+ /** This function is called once when utility mode is enabled. */
@Override
- public void testInit() {
- // Cancels all running commands at the start of test mode.
+ public void utilityInit() {
+ // Cancels all running commands at the start of utility mode.
CommandScheduler.getInstance().cancelAll();
m_robotContainer.getDrivebase().resetHeadingController();
}
- /** This function is called periodically during test mode. */
+ /** This function is called periodically during utility mode. */
@Override
- public void testPeriodic() {}
+ public void utilityPeriodic() {}
+
+ @SuppressWarnings("deprecation")
+ private static void setCurrentThreadPriorityFor2027Migration(int priority) {
+ // TODO(2027): Revisit this as WPILib 2027 moves through beta and release. The current
+ // Threads API still works in alpha-6, but is deprecated because real-time priority misuse can
+ // lock up the system. Replace with the final WPILib guidance or remove the priority adjustment.
+ Threads.setCurrentThreadPriority(priority);
+ }
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f3b8f1df..8a4a71cf 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,23 +19,7 @@
package frc.robot;
-import static frc.robot.Constants.ControllerButtonConstants.*;
-
-import choreo.auto.AutoFactory;
-import choreo.auto.AutoRoutine;
-import choreo.auto.AutoTrajectory;
import com.pathplanner.lib.auto.AutoBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants.CANBuses;
import frc.robot.Constants.Cameras;
import frc.robot.Constants.OperatorConstants;
@@ -77,6 +61,17 @@
import org.photonvision.PhotonCamera;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.VisionSystemSim;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.command2.button.CommandJoystick;
+import org.wpilib.command2.sysid.SysIdRoutine;
+import org.wpilib.driverstation.DriverStationErrors;
+import org.wpilib.driverstation.GenericHID;
+import org.wpilib.driverstation.NiDsXboxController;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform2d;
+import org.wpilib.system.Filesystem;
/** This is the location for defining robot hardware, commands, and controller button bindings. */
public class RobotContainer {
@@ -90,7 +85,7 @@ public class RobotContainer {
final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches
// These two are needed for the Sweep evaluator for camera FOV simulation
- final CommandJoystick joystick3 = new CommandJoystick(3); // Joystick for CamersSweepEvaluator
+ final CommandJoystick joystick3 = new CommandJoystick(3); // Joystick for CameraSweepEvaluator
private final CameraSweepEvaluator sweep;
/** Declare the robot subsystems here ************************************ */
@@ -121,8 +116,9 @@ public class RobotContainer {
// AutoChoosers for both supported path planning types
private final LoggedDashboardChooser autoChooserPathPlanner;
- private final LoggedDashboardChooser autoChooserChoreo;
- private final AutoFactory autoFactoryChoreo;
+ // TODO(2027): Re-enable Choreo chooser/factory when ChoreoLib supports 2027 WPILib.
+ // private final LoggedDashboardChooser autoChooserChoreo;
+ // private final AutoFactory autoFactoryChoreo;
private final LoggedDashboardChooser driveStyle =
new LoggedDashboardChooser<>("Drive Style");
@@ -138,7 +134,7 @@ public class RobotContainer {
private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO);
/**
- * Constructor for the Robot Container. This container holds subsystems, opertator interface
+ * Constructor for the Robot Container. This container holds subsystems, operator interface
* devices, and commands.
*/
public RobotContainer() {
@@ -149,7 +145,7 @@ public RobotContainer() {
// Real robot, instantiate hardware IO implementations
// Register the CANBus
- RBSICANBusRegistry.initReal(CANBuses.RIO, CANBuses.DRIVE);
+ RBSICANBusRegistry.initReal(CANBuses.ALL);
// YAGSL drivebase, get config from deploy directory
// Get the IMU instance
@@ -166,7 +162,7 @@ public RobotContainer() {
break;
case SIM:
- RBSICANBusRegistry.initSim(CANBuses.RIO, CANBuses.DRIVE);
+ RBSICANBusRegistry.initSim(CANBuses.ALL);
m_imu = new Imu(new ImuIOSim());
m_drivebase = new Drive(m_imu);
@@ -200,7 +196,7 @@ public RobotContainer() {
default:
// Replayed robot, disable IO implementations
- RBSICANBusRegistry.initSim(CANBuses.RIO, CANBuses.DRIVE);
+ RBSICANBusRegistry.initSim(CANBuses.ALL);
m_imu = new Imu(new ImuIO() {});
m_drivebase = new Drive(m_imu);
m_driveOdometry = new DriveOdometry(m_drivebase, m_imu, m_drivebase.getModules());
@@ -217,7 +213,7 @@ public RobotContainer() {
// Init all CAN busses specified in the `Constants.CANBuses` class
canHealth = Arrays.stream(Constants.CANBuses.ALL).map(RBSICANHealth::new).toList();
- // In addition to the initial battery capacity from the Dashbaord, ``RBSIPowerMonitor`` takes
+ // In addition to the initial battery capacity from the dashboard, ``RBSIPowerMonitor`` takes
// all the non-drivebase subsystems for which you wish to have power monitoring; DO NOT
// include ``m_drivebase``, as that is automatically monitored.
m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel);
@@ -232,32 +228,32 @@ public RobotContainer() {
// ...
// Set the others to null
autoChooserPathPlanner = null;
- autoChooserChoreo = null;
- autoFactoryChoreo = null;
+ // TODO(2027): Restore Choreo fields when ChoreoLib supports 2027 WPILib.
+ // autoChooserChoreo = null;
+ // autoFactoryChoreo = null;
break;
case PATHPLANNER:
autoChooserPathPlanner =
new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
// Set the others to null
- autoChooserChoreo = null;
- autoFactoryChoreo = null;
+ // TODO(2027): Restore Choreo fields when ChoreoLib supports 2027 WPILib.
+ // autoChooserChoreo = null;
+ // autoFactoryChoreo = null;
break;
case CHOREO:
- autoFactoryChoreo =
- new AutoFactory(
- m_drivebase::getPose, // A function that returns the current robot pose
- m_drivebase::resetPose, // A function that resets the current robot pose to the
- // provided Pose2d
- m_drivebase::followTrajectory, // The drive subsystem trajectory follower
- true, // If alliance flipping should be enabled
- m_drivebase // The drive subsystem
- );
- autoChooserChoreo = new LoggedDashboardChooser<>("Choreo Auto Choices");
- autoChooserChoreo.addDefaultOption("Nothing", Commands.none());
- autoChooserChoreo.addOption("twoPieceAuto", twoPieceAuto().cmd());
- // Set the others to null
+ // TODO(2027): Re-enable this Choreo AutoFactory setup when ChoreoLib supports 2027 WPILib.
+ // autoFactoryChoreo =
+ // new AutoFactory(
+ // m_drivebase::getPose,
+ // m_drivebase::resetPose,
+ // m_drivebase::followTrajectory,
+ // true,
+ // m_drivebase);
+ // autoChooserChoreo = new LoggedDashboardChooser<>("Choreo Auto Choices");
+ // autoChooserChoreo.addDefaultOption("Nothing", Commands.none());
+ // autoChooserChoreo.addOption("twoPieceAuto", twoPieceAuto().cmd());
autoChooserPathPlanner = null;
break;
@@ -292,8 +288,8 @@ private void defineAutoCommands() {
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
- * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ * org.wpilib.driverstation.Joystick} or {@link NiDsXboxController}), and then passing it to a
+ * {@link org.wpilib.command2.button.JoystickButton}.
*/
private void configureBindings() {
@@ -305,7 +301,7 @@ private void configureBindings() {
// ** Example Commands -- Remap, remove, or change as desired **
// Press B / Circle button while driving --> ROBOT-CENTRIC
driverController
- .button(ROBOT_RELATIVE)
+ .eastFaceButton()
.whileTrue(
DriveCommands.robotRelativeDrive(
m_drivebase,
@@ -314,17 +310,17 @@ private void configureBindings() {
() -> -getTurnStickX()));
// Press A / Cross button -> BRAKE
- driverController.button(BRAKE).onTrue(DriveCommands.setBrakeMode(m_drivebase, true));
+ driverController.southFaceButton().onTrue(DriveCommands.setBrakeMode(m_drivebase, true));
// Press X / Square button --> Stop with wheels in X-Lock position
- driverController.button(X_LOCK).whileTrue(DriveCommands.stopWithX(m_drivebase));
+ driverController.westFaceButton().whileTrue(DriveCommands.stopWithX(m_drivebase));
// Press Y / Triangle button --> Manually Re-Zero the Gyro
- driverController.button(ZERO_GYRO).onTrue(DriveCommands.zeroHeadingForAlliance(m_drivebase));
+ driverController.northFaceButton().onTrue(DriveCommands.zeroHeadingForAlliance(m_drivebase));
// Press RIGHT BUMPER / R1 --> Run the example flywheel
driverController
- .button(RUN_FLYWHEEL)
+ .rightBumper()
.whileTrue(
Commands.startEnd(
() -> m_flywheel.runVelocity(flywheelSpeedInput.get()),
@@ -333,7 +329,7 @@ private void configureBindings() {
// Press LEFT BUMPER / L1 --> Drive to a demo pose offset defined in OperatorConstants
driverController
- .button(AUTOPILOT_DEMO)
+ .leftBumper()
.whileTrue(
Commands.defer(
() -> {
@@ -359,22 +355,22 @@ private void configureBindings() {
// Press POV LEFT to nudge the robot left
driverController
- .button(NUDGE_LEFT)
+ .povLeft()
.whileTrue(
DriveCommands.robotRelativeNudge(
m_drivebase, 0.0, OperatorConstants.kRobotRelativeNudgeSpeedMetersPerSec, 0.0));
driverController
- .button(NUDGE_RIGHT)
+ .povRight()
.whileTrue(
DriveCommands.robotRelativeNudge(
m_drivebase, 0.0, -OperatorConstants.kRobotRelativeNudgeSpeedMetersPerSec, 0.0));
driverController
- .button(NUDGE_FORWARD)
+ .povUp()
.whileTrue(
DriveCommands.robotRelativeNudge(
m_drivebase, OperatorConstants.kRobotRelativeNudgeSpeedMetersPerSec, 0.0, 0.0));
driverController
- .button(NUDGE_BACK)
+ .povDown()
.whileTrue(
DriveCommands.robotRelativeNudge(
m_drivebase, -OperatorConstants.kRobotRelativeNudgeSpeedMetersPerSec, 0.0, 0.0));
@@ -396,7 +392,7 @@ private void configureBindings() {
.resolve("camera_sweep.csv")
.toString());
} catch (Exception e) {
- DriverStation.reportError("Camera sweep failed", e.getStackTrace());
+ DriverStationErrors.reportError("Camera sweep failed", e.getStackTrace());
}
}));
}
@@ -434,7 +430,8 @@ public Command getAutonomousCommandPathPlanner() {
* @return the command to run in autonomous
*/
public Command getAutonomousCommandChoreo() {
- return autoChooserChoreo.get();
+ // TODO(2027): Return autoChooserChoreo.get() when ChoreoLib supports 2027 WPILib.
+ return Commands.none();
}
/** Updates the alerts. */
@@ -573,35 +570,15 @@ public void updateInputs(VisionIOInputs inputs) {
return ios;
}
- /**
- * Example Choreo auto command
- *
- * NOTE: This would normally be in a spearate file.
- */
- private AutoRoutine twoPieceAuto() {
- AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto");
-
- // Load the routine's trajectories
- AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece");
- AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece");
-
- // When the routine begins, reset odometry and start the first trajectory
- routine.active().onTrue(Commands.sequence(pickupTraj.resetOdometry(), pickupTraj.cmd()));
-
- // Starting at the event marker named "intake", run the intake
- // pickupTraj.atTime("intake").onTrue(intakeSubsystem.intake());
-
- // When the trajectory is done, start the next trajectory
- pickupTraj.done().onTrue(scoreTraj.cmd());
-
- // While the trajectory is active, prepare the scoring subsystem
- // scoreTraj.active().whileTrue(scoringSubsystem.getReady());
-
- // When the trajectory is done, score
- // scoreTraj.done().onTrue(scoringSubsystem.score());
-
- return routine;
- }
+ // TODO(2027): Re-enable this example Choreo auto when ChoreoLib supports 2027 WPILib.
+ // private AutoRoutine twoPieceAuto() {
+ // AutoRoutine routine = autoFactoryChoreo.newRoutine("twoPieceAuto");
+ // AutoTrajectory pickupTraj = routine.trajectory("pickupGamepiece");
+ // AutoTrajectory scoreTraj = routine.trajectory("scoreGamepiece");
+ // routine.active().onTrue(Commands.sequence(pickupTraj.resetOdometry(), pickupTraj.cmd()));
+ // pickupTraj.done().onTrue(scoreTraj.cmd());
+ // return routine;
+ // }
private DriveStyle getSelectedDriveStyle() {
DriveStyle selected = driveStyle.get();
diff --git a/src/main/java/frc/robot/commands/AutopilotCommands.java b/src/main/java/frc/robot/commands/AutopilotCommands.java
index 3b800eba..02e52522 100644
--- a/src/main/java/frc/robot/commands/AutopilotCommands.java
+++ b/src/main/java/frc/robot/commands/AutopilotCommands.java
@@ -17,19 +17,19 @@
package frc.robot.commands;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static org.wpilib.units.Units.RadiansPerSecond;
import com.therekrab.autopilot.APTarget;
import com.therekrab.autopilot.Autopilot.APResult;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.AutoConstants;
import frc.robot.subsystems.drive.Drive;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.ChassisVelocities;
+import org.wpilib.units.measure.Distance;
public final class AutopilotCommands {
private AutopilotCommands() {}
@@ -178,7 +178,7 @@ private static Command autopilotToTarget(Drive drive, APTarget target) {
return Commands.run(
() -> {
- ChassisSpeeds robotRelativeSpeeds = drive.getChassisSpeeds();
+ ChassisVelocities robotRelativeSpeeds = drive.getChassisSpeeds();
Pose2d pose = drive.getPose();
boolean atTarget = AutoConstants.kAutopilot.atTarget(pose, target);
@@ -197,8 +197,8 @@ private static Command autopilotToTarget(Drive drive, APTarget target) {
Logger.recordOutput("Autopilot/atTarget", atTarget);
// Output is field relative
- ChassisSpeeds speeds =
- new ChassisSpeeds(
+ ChassisVelocities speeds =
+ new ChassisVelocities(
output.vx(),
output.vy(),
RadiansPerSecond.of(
@@ -208,7 +208,7 @@ private static Command autopilotToTarget(Drive drive, APTarget target) {
drive.getHeading().getRadians(),
output.targetAngle().getRadians())));
- drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, drive.getHeading()));
+ drive.runVelocity(speeds.toRobotRelative(drive.getHeading()));
},
drive)
diff --git a/src/main/java/frc/robot/commands/ChoreoAutoController.java b/src/main/java/frc/robot/commands/ChoreoAutoController.java
index 6b41f69f..c7bb2b42 100644
--- a/src/main/java/frc/robot/commands/ChoreoAutoController.java
+++ b/src/main/java/frc/robot/commands/ChoreoAutoController.java
@@ -9,61 +9,10 @@
package frc.robot.commands;
-import choreo.trajectory.SwerveSample;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import frc.robot.Constants.AutoConstants;
-import frc.robot.subsystems.drive.Drive;
-import java.util.function.Consumer;
-
-public final class ChoreoAutoController implements Consumer {
- private final Drive drive; // drive subsystem
- private final PIDController xController =
- new PIDController(
- AutoConstants.kChoreoDrivePID.kP,
- AutoConstants.kChoreoDrivePID.kI,
- AutoConstants.kChoreoDrivePID.kD);
- private final PIDController yController =
- new PIDController(
- AutoConstants.kChoreoDrivePID.kP,
- AutoConstants.kChoreoDrivePID.kI,
- AutoConstants.kChoreoDrivePID.kD);
- private final PIDController headingController =
- new PIDController(
- AutoConstants.kChoreoSteerPID.kP,
- AutoConstants.kChoreoSteerPID.kI,
- AutoConstants.kChoreoSteerPID.kD);
-
- public ChoreoAutoController(Drive drive) {
- this.drive = drive;
- headingController.enableContinuousInput(-Math.PI, Math.PI);
- }
-
- public void reset() {
- Pose2d pose = drive.getPose();
- xController.reset();
- yController.reset();
- headingController.reset();
- headingController.setSetpoint(pose.getRotation().getRadians());
- }
-
- @Override
- public void accept(SwerveSample referenceState) {
- Pose2d pose = drive.getPose();
- double xFF = referenceState.vx;
- double yFF = referenceState.vy;
- double rotationFF = referenceState.omega;
-
- double xFeedback = xController.calculate(pose.getX(), referenceState.x);
- double yFeedback = yController.calculate(pose.getY(), referenceState.y);
- double rotationFeedback =
- headingController.calculate(pose.getRotation().getRadians(), referenceState.heading);
-
- // Convert to field relative speeds & send command
- ChassisSpeeds out =
- ChassisSpeeds.fromFieldRelativeSpeeds(
- xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation());
- drive.runVelocity(out);
- }
+/**
+ * TODO(2027): Restore the Choreo auto controller when ChoreoLib publishes a 2027-compatible
+ * vendordep.
+ */
+public final class ChoreoAutoController {
+ private ChoreoAutoController() {}
}
diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
index 626c1932..1483e915 100644
--- a/src/main/java/frc/robot/commands/DriveCommands.java
+++ b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -9,27 +9,28 @@
package frc.robot.commands;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.DrivebaseConstants;
import frc.robot.Constants.OperatorConstants;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.SwerveConstants;
+import frc.robot.util.MathUtil;
import java.text.DecimalFormat;
import java.text.NumberFormat;
import java.util.ArrayList;
import java.util.List;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.Commands;
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.DriverStationErrors;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.math.filter.SlewRateLimiter;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.ChassisVelocities;
+import org.wpilib.math.util.Units;
+import org.wpilib.system.Timer;
public final class DriveCommands {
@@ -57,13 +58,13 @@ public static Command fieldRelativeDrive(
double omega = getOmega(omegaLimiter.calculate(omegaSupplier.getAsDouble()));
// Convert to field relative speeds & send command
- ChassisSpeeds speeds =
- new ChassisSpeeds(
+ ChassisVelocities speeds =
+ new ChassisVelocities(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec());
- drive.runVelocity(
- ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getAllianceAwareHeading(drive)));
+ Rotation2d heading = getAllianceRelativeHeading(drive.getHeading());
+ drive.runVelocity(speeds.toRobotRelative(heading));
},
drive)
.beforeStarting(() -> resetLimiters(xLimiter, yLimiter, omegaLimiter));
@@ -92,7 +93,7 @@ public static Command robotRelativeDrive(
// Run with straight-up velocities w.r.t. the robot!
drive.runVelocity(
- new ChassisSpeeds(
+ new ChassisVelocities(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec()));
@@ -131,13 +132,13 @@ public static Command fieldRelativeDriveAtAngle(
drive.getHeading().getRadians(), rotationSupplier.get().getRadians());
// Convert to field relative speeds & send command
- ChassisSpeeds speeds =
- new ChassisSpeeds(
+ ChassisVelocities speeds =
+ new ChassisVelocities(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega);
- drive.runVelocity(
- ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getAllianceAwareHeading(drive)));
+ Rotation2d heading = getAllianceRelativeHeading(drive.getHeading());
+ drive.runVelocity(speeds.toRobotRelative(heading));
},
drive)
@@ -173,7 +174,8 @@ public static Command zeroHeadingForAlliance(Drive drive) {
/** Drives robot-relative at a fixed velocity while scheduled. */
public static Command robotRelativeNudge(
Drive drive, double vxMetersPerSec, double vyMetersPerSec, double omegaRadPerSec) {
- ChassisSpeeds speeds = new ChassisSpeeds(vxMetersPerSec, vyMetersPerSec, omegaRadPerSec);
+ ChassisVelocities speeds =
+ new ChassisVelocities(vxMetersPerSec, vyMetersPerSec, omegaRadPerSec);
return Commands.run(() -> drive.runVelocity(speeds), drive).finallyDo(drive::stop);
}
@@ -219,10 +221,10 @@ private static void resetLimiters(SlewRateLimiter... limiters) {
}
}
- private static Rotation2d getAllianceAwareHeading(Drive drive) {
- return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red
- ? drive.getHeading().plus(Rotation2d.k180deg)
- : drive.getHeading();
+ private static Rotation2d getAllianceRelativeHeading(Rotation2d heading) {
+ return MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.RED
+ ? heading.plus(Rotation2d.k180deg)
+ : heading;
}
/***************************************************************************/
@@ -284,7 +286,7 @@ public static Command feedforwardCharacterization(Drive drive) {
}
double denominator = n * sumX2 - sumX * sumX;
if (n < 2 || Math.abs(denominator) < 1e-9) {
- DriverStation.reportWarning(
+ DriverStationErrors.reportWarning(
"Drive FF characterization did not collect enough usable samples.",
false);
drive.stop();
@@ -324,7 +326,7 @@ public static Command wheelRadiusCharacterization(Drive drive) {
double speed =
limiter.calculate(
DrivebaseConstants.kWheelRadiusCharacterizationMaxVelocityRadPerSec);
- drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed));
+ drive.runVelocity(new ChassisVelocities(0.0, 0.0, speed));
},
drive)),
@@ -358,7 +360,7 @@ public static Command wheelRadiusCharacterization(Drive drive) {
wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0;
}
if (wheelDelta <= 1e-9) {
- DriverStation.reportWarning(
+ DriverStationErrors.reportWarning(
"Wheel radius characterization did not measure wheel movement.",
false);
drive.stop();
diff --git a/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java b/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java
index 91b48bfe..c0989ad2 100644
--- a/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java
+++ b/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java
@@ -1,6 +1,6 @@
package frc.robot.generated;
-import static edu.wpi.first.units.Units.*;
+import static org.wpilib.units.Units.*;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.*;
@@ -8,10 +8,10 @@
import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.swerve.*;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.units.measure.*;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
+import org.wpilib.units.measure.*;
// import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -74,9 +74,8 @@ public class COMPBOTTunerConstants {
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null;
- // CAN bus that the devices are located on;
- // All swerve devices must share the same CAN bus
- public static final CANBus kCANBus = new CANBus("DriveTrain", "./logs/example.hoot");
+ // Phoenix Tuner generation bus. Runtime wiring is selected in Constants.DrivebaseConstants.
+ public static final CANBus kCANBus = new CANBus("can_s2", "./logs/example.hoot");
// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
diff --git a/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java b/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java
index 0942cc39..21c1a759 100755
--- a/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java
+++ b/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java
@@ -1,6 +1,6 @@
package frc.robot.generated;
-import static edu.wpi.first.units.Units.*;
+import static org.wpilib.units.Units.*;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.*;
@@ -8,10 +8,10 @@
import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.swerve.*;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.units.measure.*;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
+import org.wpilib.units.measure.*;
// import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -74,9 +74,8 @@ public class DEVBOT1TunerConstants {
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null;
- // CAN bus that the devices are located on;
- // All swerve devices must share the same CAN bus
- public static final CANBus kCANBus = new CANBus("DriveTrain", "./logs/example.hoot");
+ // Phoenix Tuner generation bus. Runtime wiring is selected in Constants.DrivebaseConstants.
+ public static final CANBus kCANBus = new CANBus("can_s2", "./logs/example.hoot");
// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
diff --git a/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java b/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java
index 31b86369..94b9424c 100644
--- a/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java
+++ b/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java
@@ -1,13 +1,13 @@
package frc.robot.generated;
-import static edu.wpi.first.units.Units.*;
+import static org.wpilib.units.Units.*;
import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.*;
import com.ctre.phoenix6.signals.*;
import com.ctre.phoenix6.swerve.*;
import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
-import edu.wpi.first.units.measure.*;
+import org.wpilib.units.measure.*;
// import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -70,9 +70,8 @@ public class DEVBOT2TunerConstants {
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null;
- // CAN bus that the devices are located on;
- // All swerve devices must share the same CAN bus
- public static final CANBus kCANBus = new CANBus("DriveTrain", "./logs/example.hoot");
+ // Phoenix Tuner generation bus. Runtime wiring is selected in Constants.DrivebaseConstants.
+ public static final CANBus kCANBus = new CANBus("can_s2", "./logs/example.hoot");
// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
index 5b7b4e25..29273d25 100644
--- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
+++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
@@ -13,7 +13,6 @@
package frc.robot.subsystems.accelerometer;
-import edu.wpi.first.math.geometry.Translation3d;
import frc.robot.Constants;
import frc.robot.Constants.RobotConstants;
import frc.robot.Constants.SensorConstants;
@@ -21,6 +20,7 @@
import frc.robot.util.TimeUtil;
import frc.robot.util.VirtualSubsystem;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.geometry.Translation3d;
/**
* Accelerometer subsystem (VirtualSubsystem)
diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java
index 6cf6ef33..5d5ff886 100644
--- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java
+++ b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java
@@ -13,41 +13,15 @@
package frc.robot.subsystems.accelerometer;
-import edu.wpi.first.wpilibj.BuiltInAccelerometer;
-import edu.wpi.first.wpilibj.Notifier;
-
public class RioAccelIORoboRIO implements RioAccelIO {
- private final BuiltInAccelerometer accel = new BuiltInAccelerometer();
-
- private volatile long stampNs = 0L;
- private volatile double xG = 0.0, yG = 0.0, zG = 0.0;
-
- private final Notifier sampler;
-
- public RioAccelIORoboRIO(double sampleHz) {
- sampler =
- new Notifier(
- () -> {
- xG = accel.getX();
- yG = accel.getY();
- zG = accel.getZ();
- stampNs = System.nanoTime();
- });
- sampler.setName("RioAccelSampler");
- sampler.startPeriodic(1.0 / sampleHz);
- }
+ public RioAccelIORoboRIO(double sampleHz) {}
@Override
public void updateInputs(Inputs inputs) {
- inputs.connected = true;
- inputs.timestampNs = stampNs;
- inputs.xG = xG;
- inputs.yG = yG;
- inputs.zG = zG;
- }
-
- @Override
- public void close() {
- sampler.stop();
+ inputs.connected = false;
+ inputs.timestampNs = 0L;
+ inputs.xG = 0.0;
+ inputs.yG = 0.0;
+ inputs.zG = 0.0;
}
}
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index ea54345d..d5595853 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -17,45 +17,24 @@
package frc.robot.subsystems.drive;
-import static edu.wpi.first.units.Units.Volts;
import static frc.robot.subsystems.drive.SwerveConstants.*;
+import static org.wpilib.units.Units.Volts;
-import choreo.trajectory.SwerveSample;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.pathfinding.Pathfinding;
import com.pathplanner.lib.util.PathPlannerLogging;
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.trajectory.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.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
import frc.robot.Constants.AutoConstants;
import frc.robot.Constants.DrivebaseConstants;
import frc.robot.Constants.RobotConstants;
import frc.robot.subsystems.imu.Imu;
+import frc.robot.util.Alert;
+import frc.robot.util.Alert.AlertType;
import frc.robot.util.ConcurrentTimeInterpolatableBuffer;
import frc.robot.util.LocalADStarAK;
+import frc.robot.util.MathUtil;
import frc.robot.util.RBSIEnum.Mode;
import frc.robot.util.RBSIParsing;
import frc.robot.util.RBSISubsystem;
@@ -67,6 +46,26 @@
import java.util.concurrent.locks.ReentrantLock;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.sysid.SysIdRoutine;
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.DriverStationErrors;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.driverstation.RobotState;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.controller.ProfiledPIDController;
+import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.ChassisVelocities;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
+import org.wpilib.math.trajectory.TrapezoidProfile;
+import org.wpilib.smartdashboard.Field2d;
+import org.wpilib.smartdashboard.SmartDashboard;
/**
* Drive subsystem (RBSISubsystem)
@@ -93,7 +92,18 @@ public class Drive extends RBSISubsystem {
// Declare an alert
private final Alert gyroDisconnectedAlert =
- new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError);
+ new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.ERROR);
+ private final Alert pathPlannerStartPoseAlert =
+ new Alert("PathPlanner auto start pose is outside the allowed radius.", AlertType.ERROR);
+ private boolean pathPlannerStartBlocked = false;
+ private PathPlannerStartAction pendingPathPlannerStartAction;
+ private Pose2d pendingPathPlannerStartPose;
+
+ enum PathPlannerStartAction {
+ RESET_TO_PATH_START,
+ USE_VISION_POSE,
+ BLOCK_AUTO
+ }
// Declare odometry and pose-related variables
// This one is package-private; used in DriveOdometry, PhoenixOdometryThread, and
@@ -118,6 +128,8 @@ public class Drive extends RBSISubsystem {
// Pose reset gate (vision + anything latency-sensitive)
private volatile long poseResetEpoch = 0; // monotonic counter
private volatile double lastPoseResetTimestamp = Double.NEGATIVE_INFINITY;
+ private volatile double lastAcceptedVisionReceiptTimestamp = Double.NEGATIVE_INFINITY;
+ private volatile double lastAcceptedVisionMeasurementTimestamp = Double.NEGATIVE_INFINITY;
// Pose Regimes (ENABLED, DISABLED_COAST, DISABLE_STATIONARY)
private boolean lastEnabled = false;
@@ -131,6 +143,9 @@ public class Drive extends RBSISubsystem {
private boolean disabledVisionInitialized = false;
private Pose2d lastDisabledVisionPose = new Pose2d();
private double lastDisabledVisionTs = Double.NaN;
+ private final LatestSampleQueue pendingVisionMeasurements =
+ new LatestSampleQueue<>(16);
+ private long droppedVisionMeasurements = 0;
/** Constructor */
public Drive(Imu imu) {
@@ -211,7 +226,7 @@ public Drive(Imu imu) {
}
// Usage reporting for swerve template
- HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit);
+ HAL.reportUsage("RobotDrive", "Swerve_AdvantageKit");
// Configure Autonomous Path Building for PathPlanner based on `AutoType`
switch (Constants.getAutoType()) {
@@ -220,7 +235,7 @@ public Drive(Imu imu) {
// Configure AutoBuilder for PathPlanner
AutoBuilder.configure(
this::getPose,
- this::resetPose,
+ this::resetPoseFromPathPlanner,
this::getChassisSpeeds,
(speeds, feedforwards) -> runVelocity(speeds),
new PPHolonomicDriveController(
@@ -233,10 +248,10 @@ public Drive(Imu imu) {
DrivebaseConstants.kSpinI,
DrivebaseConstants.kSpinD)),
AutoConstants.kPathPlannerConfig,
- () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+ () -> MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.RED,
this);
} catch (Exception e) {
- DriverStation.reportError(
+ DriverStationErrors.reportError(
"Failed to load PathPlanner config and configure AutoBuilder", e.getStackTrace());
}
Pathfinding.setPathfinder(new LocalADStarAK());
@@ -280,11 +295,11 @@ public Drive(Imu imu) {
public void rbsiPeriodic() {
// The only function of the drive periodic() is to stop the modules if the DriverStation is
- // diabled.
- if (DriverStation.isDisabled()) {
+ // disabled.
+ if (RobotState.isDisabled()) {
for (var module : modules) module.stop();
- Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
- Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+ Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleVelocity[] {});
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleVelocity[] {});
}
field.setRobotPose(m_PoseEstimator.getEstimatedPosition());
@@ -301,7 +316,9 @@ public void rbsiPeriodic() {
public void simulationPeriodic() {
// IMPORTANT: do not run sim physics during REPLAY
- if (Constants.getMode() != Mode.SIM) return;
+ if (Constants.getMode() != Mode.SIM) {
+ return;
+ }
final double dt = Constants.kLoopPeriodSecs;
@@ -311,7 +328,7 @@ public void simulationPeriodic() {
}
// Get module states from modules (ok to allocate; can be cached later if desired)
- final SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length];
+ final SwerveModuleVelocity[] moduleStates = new SwerveModuleVelocity[modules.length];
for (int i = 0; i < modules.length; i++) {
moduleStates[i] = modules[i].getState();
}
@@ -346,16 +363,14 @@ public void simulationPeriodic() {
* @param brake True to set motors to brake mode, false for coast.
*/
public void setMotorBrake(boolean brake) {
- {
- for (Module swerveModule : modules) {
- swerveModule.setBrakeMode(brake);
- }
+ for (Module swerveModule : modules) {
+ swerveModule.setBrakeMode(brake);
}
}
/** Stop the drive. */
public void stop() {
- runVelocity(new ChassisSpeeds());
+ runVelocity(new ChassisVelocities());
}
/**
@@ -363,9 +378,10 @@ public void stop() {
* return to their normal orientations the next time a nonzero velocity is requested.
*/
public void stopWithX() {
- Rotation2d[] headings = new Rotation2d[4];
- for (int i = 0; i < 4; i++) {
- headings[i] = getModuleTranslations()[i].getAngle();
+ Translation2d[] moduleTranslations = getModuleTranslations();
+ Rotation2d[] headings = new Rotation2d[moduleTranslations.length];
+ for (int i = 0; i < moduleTranslations.length; i++) {
+ headings[i] = moduleTranslations[i].getAngle();
}
kinematics.resetHeadings(headings);
stop();
@@ -376,18 +392,29 @@ public void stopWithX() {
*
* @param speeds Speeds in meters/sec
*/
- public void runVelocity(ChassisSpeeds speeds) {
+ public void runVelocity(ChassisVelocities speeds) {
+ if (pathPlannerStartBlocked && RobotState.isAutonomousEnabled()) {
+ for (Module module : modules) {
+ module.stop();
+ }
+ Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleVelocity[] {});
+ Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleVelocity[] {});
+ return;
+ }
+
// Calculate module setpoints
- ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.kLoopPeriodSecs);
- SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
- SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, getMaxLinearSpeedMetersPerSec());
+ ChassisVelocities discreteSpeeds = speeds.discretize(Constants.kLoopPeriodSecs);
+ SwerveModuleVelocity[] setpointStates = kinematics.toSwerveModuleVelocities(discreteSpeeds);
+ setpointStates =
+ SwerveDriveKinematics.desaturateWheelVelocities(
+ setpointStates, getMaxLinearSpeedMetersPerSec());
// Log unoptimized setpoints and setpoint speeds
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds);
// Send setpoints to modules
- for (int i = 0; i < 4; i++) {
+ for (int i = 0; i < modules.length; i++) {
modules[i].runSetpoint(setpointStates[i]);
}
@@ -401,8 +428,8 @@ public void runVelocity(ChassisSpeeds speeds) {
* @param output Specified drive output for characterization
*/
public void runCharacterization(double output) {
- for (int i = 0; i < 4; i++) {
- modules[i].runCharacterization(output);
+ for (Module module : modules) {
+ module.runCharacterization(output);
}
}
@@ -458,12 +485,12 @@ public void updateDisabledCoastState(
}
// If coast already expired, nothing to do.
- if (!(now < disabledCoastUntilTs)) {
+ if (now >= disabledCoastUntilTs) {
return;
}
// Need odometry positions to detect motion
- if (odomPositions == null || odomPositions.length < 4) {
+ if (odomPositions == null || odomPositions.length < modules.length) {
return;
}
@@ -472,16 +499,16 @@ public void updateDisabledCoastState(
double maxDelta = 0.0;
boolean hadLastWheelDist = haveLastWheelDist;
if (haveLastWheelDist) {
- for (int i = 0; i < 4; i++) {
- double dist = odomPositions[i].distanceMeters;
+ for (int i = 0; i < modules.length; i++) {
+ double dist = odomPositions[i].distance;
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;
+ for (int i = 0; i < modules.length; i++) {
+ lastWheelDistM[i] = odomPositions[i].distance;
}
haveLastWheelDist = true;
@@ -532,16 +559,16 @@ public Module[] getModules() {
return modules;
}
- /** Return the prodiledPID angle controller */
+ /** Return the profiled PID 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() {
- SwerveModuleState[] states = new SwerveModuleState[4];
- for (int i = 0; i < 4; i++) {
+ private SwerveModuleVelocity[] getModuleStates() {
+ SwerveModuleVelocity[] states = new SwerveModuleVelocity[modules.length];
+ for (int i = 0; i < modules.length; i++) {
states[i] = modules[i].getState();
}
return states;
@@ -550,8 +577,8 @@ private SwerveModuleState[] getModuleStates() {
/** Returns the module positions (turn angles and drive positions) for all of the modules. */
@AutoLogOutput(key = "SwerveStates/Positions")
SwerveModulePosition[] getModulePositions() {
- SwerveModulePosition[] states = new SwerveModulePosition[4];
- for (int i = 0; i < 4; i++) {
+ SwerveModulePosition[] states = new SwerveModulePosition[modules.length];
+ for (int i = 0; i < modules.length; i++) {
states[i] = modules[i].getPosition();
}
return states;
@@ -559,8 +586,8 @@ SwerveModulePosition[] getModulePositions() {
/** Returns the measured chassis speeds of the robot. */
@AutoLogOutput(key = "SwerveChassisSpeeds/Measured")
- public ChassisSpeeds getChassisSpeeds() {
- return kinematics.toChassisSpeeds(getModuleStates());
+ public ChassisVelocities getChassisSpeeds() {
+ return kinematics.toChassisVelocities(getModuleStates());
}
/**
@@ -576,9 +603,17 @@ public Pose2d getPose() {
return m_PoseEstimator.getEstimatedPosition();
}
- /** Returns the current odometry YAW. */
+ /** Returns the field-aligned heading from the pose estimator. */
@AutoLogOutput(key = "Odometry/Yaw")
public Rotation2d getHeading() {
+ if (Constants.isPureSim()) {
+ return simPhysics.getYaw();
+ }
+ return m_PoseEstimator.getEstimatedPosition().getRotation();
+ }
+
+ /** Returns the physical gyro reading used as the estimator's sensor reference. */
+ private Rotation2d getRawGyroHeading() {
if (Constants.isPureSim()) {
return simPhysics.getYaw();
}
@@ -591,11 +626,11 @@ public Rotation2d getHeading() {
* +X = field forward +Y = field left CCW+ = counterclockwise
*/
@AutoLogOutput(key = "SwerveChassisSpeeds/FieldMeasured")
- public ChassisSpeeds getFieldRelativeSpeeds() {
+ public ChassisVelocities getFieldRelativeSpeeds() {
// Robot-relative measured speeds from modules
- ChassisSpeeds robotRelative = getChassisSpeeds();
+ ChassisVelocities robotRelative = getChassisSpeeds();
// Convert to field-relative using authoritative yaw
- return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelative, getHeading());
+ return robotRelative.toFieldRelative(getHeading());
}
/**
@@ -605,8 +640,8 @@ public ChassisSpeeds getFieldRelativeSpeeds() {
*/
@AutoLogOutput(key = "Drive/FieldLinearVelocity")
public Translation2d getFieldLinearVelocity() {
- ChassisSpeeds fieldSpeeds = getFieldRelativeSpeeds();
- return new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond);
+ ChassisVelocities fieldSpeeds = getFieldRelativeSpeeds();
+ return new Translation2d(fieldSpeeds.vx, fieldSpeeds.vy);
}
/** Returns interpolated odometry pose at a given timestamp. */
@@ -614,12 +649,12 @@ public Optional getPoseAtTime(double timestampSeconds) {
return poseBuffer.getSample(timestampSeconds);
}
- /** Returns the oldest timetamp in the current pose buffer */
+ /** Returns the oldest timestamp in the current pose buffer. */
public double getPoseBufferOldestTime() {
return poseBuffer.getOldestTimestamp().orElse(Double.NaN);
}
- /** Returns the newest timetamp in the current pose buffer */
+ /** Returns the newest timestamp in the current pose buffer. */
public double getPoseBufferNewestTime() {
return poseBuffer.getNewestTimestamp().orElse(Double.NaN);
}
@@ -640,14 +675,10 @@ public OptionalDouble getMaxAbsYawRateRadPerSec(double t0, double t1) {
if (sub.isEmpty()) return OptionalDouble.empty();
double maxAbs = 0.0;
- boolean any = false;
- for (double v : sub.values()) {
- any = true;
- double a = Math.abs(v);
- if (a > maxAbs) maxAbs = a;
+ for (double value : sub.values()) {
+ maxAbs = Math.max(maxAbs, Math.abs(value));
}
- // Return a value if there's anything to report, else empty
- return any ? OptionalDouble.of(maxAbs) : OptionalDouble.empty();
+ return OptionalDouble.of(maxAbs);
}
/** Get the last EPOCH of a pose reset */
@@ -697,7 +728,7 @@ public boolean isDisabledCoast() {
/** Returns whether the robot was in the DISABLED_COAST state at time `timestamp` */
public boolean isDisabledCoast(double timestamp) {
- return DriverStation.isDisabled() && (timestamp < disabledCoastUntilTs);
+ return RobotState.isDisabled() && (timestamp < disabledCoastUntilTs);
}
/** Returns the disabledCoastStartTs variable */
@@ -707,8 +738,8 @@ public double getDisabledCoastStartTs() {
/** Returns the position of each module in radians. */
public double[] getWheelRadiusCharacterizationPositions() {
- double[] values = new double[4];
- for (int i = 0; i < 4; i++) {
+ double[] values = new double[modules.length];
+ for (int i = 0; i < modules.length; i++) {
values[i] = modules[i].getWheelRadiusCharacterizationPosition();
}
return values;
@@ -717,10 +748,10 @@ public double[] getWheelRadiusCharacterizationPositions() {
/** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */
public double getFFCharacterizationVelocity() {
double output = 0.0;
- for (int i = 0; i < 4; i++) {
- output += modules[i].getFFCharacterizationVelocity() / 4.0;
+ for (Module module : modules) {
+ output += module.getFFCharacterizationVelocity();
}
- return output;
+ return output / modules.length;
}
/************************************************************************* */
@@ -729,17 +760,110 @@ public double getFFCharacterizationVelocity() {
/**
* Resets the current odometry pose
*
- * @param pose The specified pose to which to reset the poseEsitmator
+ * @param pose The specified pose to which to reset the pose estimator
*/
public void resetPose(Pose2d pose) {
- m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), pose);
- markPoseReset(TimeUtil.now());
+ final double now = TimeUtil.now();
+ m_PoseEstimator.resetPosition(getRawGyroHeading(), getModulePositions(), pose);
+ lastAcceptedVisionReceiptTimestamp = Double.NEGATIVE_INFINITY;
+ markPoseReset(now);
+ poseBufferAddSample(now, pose);
+ }
+
+ /** Validates PathPlanner's starting pose and returns whether autonomous may run. */
+ public boolean validatePathPlannerAutoStart(Pose2d pathStartPose) {
+ pendingPathPlannerStartAction = evaluatePathPlannerStart(pathStartPose);
+ pendingPathPlannerStartPose = pathStartPose;
+ return pendingPathPlannerStartAction != PathPlannerStartAction.BLOCK_AUTO;
+ }
+
+ private PathPlannerStartAction evaluatePathPlannerStart(Pose2d pathStartPose) {
+ final double now = TimeUtil.now();
+ final double visionAge = now - lastAcceptedVisionReceiptTimestamp;
+ final boolean hasRecentVision =
+ Double.isFinite(visionAge)
+ && visionAge >= 0.0
+ && visionAge <= DrivebaseConstants.kPathPlannerVisionFreshnessSec;
+ final Pose2d currentPose = getPose();
+ final double startDistanceMeters =
+ currentPose.getTranslation().getDistance(pathStartPose.getTranslation());
+ final PathPlannerStartAction action =
+ determinePathPlannerStartAction(
+ currentPose,
+ pathStartPose,
+ hasRecentVision,
+ DrivebaseConstants.kPathPlannerStartToleranceMeters);
+
+ pathPlannerStartBlocked = action == PathPlannerStartAction.BLOCK_AUTO;
+ pathPlannerStartPoseAlert.setText(
+ String.format(
+ "PathPlanner auto blocked: robot is %.2f m from the requested start (limit %.2f m).",
+ startDistanceMeters, DrivebaseConstants.kPathPlannerStartToleranceMeters));
+ pathPlannerStartPoseAlert.set(pathPlannerStartBlocked);
+
+ Logger.recordOutput("Auto/NominalStartingPose", pathStartPose);
+ Logger.recordOutput("Auto/PoseBeforeResetDecision", currentPose);
+ Logger.recordOutput("Auto/VisionMeasurementAgeSec", visionAge);
+ Logger.recordOutput(
+ "Auto/LastVisionMeasurementTimestamp", lastAcceptedVisionMeasurementTimestamp);
+ Logger.recordOutput("Auto/StartPoseDistanceMeters", startDistanceMeters);
+ Logger.recordOutput("Auto/StartPoseAction", action.toString());
+ Logger.recordOutput("Auto/StartPoseBlocked", pathPlannerStartBlocked);
+ Logger.recordOutput(
+ "Auto/PoseResetSkippedForVision", action == PathPlannerStartAction.USE_VISION_POSE);
+
+ return action;
+ }
+
+ static PathPlannerStartAction determinePathPlannerStartAction(
+ Pose2d currentPose, Pose2d pathStartPose, boolean hasRecentVision, double toleranceMeters) {
+ boolean estimatorUninitialized = currentPose.getTranslation().getNorm() <= 1e-6;
+ if (!hasRecentVision || estimatorUninitialized) {
+ return PathPlannerStartAction.RESET_TO_PATH_START;
+ }
+
+ double distance = currentPose.getTranslation().getDistance(pathStartPose.getTranslation());
+ return Double.isFinite(distance) && distance <= toleranceMeters
+ ? PathPlannerStartAction.USE_VISION_POSE
+ : PathPlannerStartAction.BLOCK_AUTO;
+ }
+
+ /** Applies PathPlanner's starting-pose policy from AutoBuilder's reset callback. */
+ private void resetPoseFromPathPlanner(Pose2d pose) {
+ PathPlannerStartAction action;
+ boolean usingPreflightDecision =
+ pendingPathPlannerStartAction != null
+ && pendingPathPlannerStartPose != null
+ && pendingPathPlannerStartPose.getTranslation().getDistance(pose.getTranslation())
+ <= 1e-6
+ && Math.abs(
+ pendingPathPlannerStartPose
+ .getRotation()
+ .minus(pose.getRotation())
+ .getRadians())
+ <= 1e-6;
+
+ if (usingPreflightDecision) {
+ action = pendingPathPlannerStartAction;
+ } else {
+ action = evaluatePathPlannerStart(pose);
+ }
+ pendingPathPlannerStartAction = null;
+ pendingPathPlannerStartPose = null;
+
+ boolean resetSuppressed = action != PathPlannerStartAction.RESET_TO_PATH_START;
+ Logger.recordOutput("Auto/PathPlannerResetUsedPreflightDecision", usingPreflightDecision);
+ Logger.recordOutput("Auto/PathPlannerResetSuppressed", resetSuppressed);
+
+ if (!resetSuppressed) {
+ resetPose(pose);
+ }
}
/** Zeros the gyro based on alliance color */
public void zeroHeadingForAlliance() {
imu.zeroYaw(
- DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+ MatchState.getAlliance().orElse(Alliance.BLUE) == Alliance.BLUE
? Rotation2d.kZero
: Rotation2d.k180deg);
resetHeadingController();
@@ -758,127 +882,114 @@ public void zeroHeading() {
*
* @param measurement The pose @ timestamp to add to the pose estimator
*/
- // Called by Vision via consumer.accept(TimedPose)
+ // Called by Vision via consumer.accept(TimedPose). Odometry applies queued measurements while it
+ // already owns the estimator lock.
public void addVisionMeasurement(TimedPose meas) {
- odometryLock.lock();
- try {
- // 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);
+ if (pendingVisionMeasurements.offerLatest(meas)) {
+ droppedVisionMeasurements++;
+ }
+ }
- // 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,
- DrivebaseConstants.kDisabledVisionCoastBlendAlpha)
- : 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;
+ /** Applies a bounded number of queued vision measurements. Caller must own odometryLock. */
+ int applyQueuedVisionMeasurements(int maxMeasurements) {
+ int processed = 0;
+ while (processed < maxMeasurements) {
+ TimedPose measurement = pendingVisionMeasurements.poll();
+ if (measurement == null) {
+ break;
}
+ applyVisionMeasurementLocked(measurement);
+ processed++;
+ }
- // If not initialized AND not coasting: snap hard to vision once
- if (!disabledVisionInitialized && !coast) {
- disabledVisionInitialized = true;
- lastDisabledVisionPose = vision;
- lastDisabledVisionTs = t;
+ Logger.recordOutput("Vision/EstimatorQueueDepth", pendingVisionMeasurements.size());
+ Logger.recordOutput("Vision/EstimatorQueueDropped", droppedVisionMeasurements);
+ Logger.recordOutput("Vision/EstimatorQueueProcessed", processed);
+ return processed;
+ }
- m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), vision);
- markPoseReset(t);
- poseBufferAddSample(t, vision);
+ private void applyVisionMeasurementLocked(TimedPose meas) {
+ final double t = meas.timestampSeconds();
+ final Pose2d vision = meas.pose();
- 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);
+ if (!RobotState.isDisabled()) {
+ disabledVisionInitialized = false;
+ lastDisabledVisionTs = Double.NaN;
+ m_PoseEstimator.addVisionMeasurement(vision, t, meas.stdDevs());
+ markVisionMeasurementAccepted(t);
+ return;
+ }
+
+ final boolean coast = isDisabledCoast(t);
+ if (coast) {
+ final double coastAge = t - getDisabledCoastStartTs();
+ if (coastAge >= 0.0 && coastAge < DrivebaseConstants.kDisabledVisionIgnoreAfterDisableSec) {
return;
}
- Logger.recordOutput("Vision/DisabledReject", false);
+ }
- // Accept this vision frame as the new reference
+ final double alpha =
+ coast
+ ? Math.min(
+ DrivebaseConstants.kDisabledVisionBlendAlpha,
+ DrivebaseConstants.kDisabledVisionCoastBlendAlpha)
+ : DrivebaseConstants.kDisabledVisionBlendAlpha;
+ final boolean stale =
+ Double.isFinite(lastDisabledVisionTs)
+ && (t - lastDisabledVisionTs) > DrivebaseConstants.kDisabledVisionStale;
+
+ if (coast) {
+ disabledVisionInitialized = false;
+ }
+
+ if (!disabledVisionInitialized && !coast) {
+ disabledVisionInitialized = true;
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);
+ m_PoseEstimator.resetPosition(getRawGyroHeading(), getModulePositions(), vision);
markPoseReset(t);
- poseBufferAddSample(t, blended);
+ poseBufferAddSample(t, vision);
+ markVisionMeasurementAccepted(t);
- Logger.recordOutput("Vision/DisabledBlendedPose", blended);
+ Logger.recordOutput("Vision/DisabledInitSnap", true);
+ Logger.recordOutput("Vision/DisabledReject", false);
Logger.recordOutput("Vision/DisabledBlendAlphaUsed", alpha);
+ return;
+ }
+ Logger.recordOutput("Vision/DisabledInitSnap", false);
+
+ 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());
- } finally {
- odometryLock.unlock();
+ if (!stale
+ && (deltaTranslation > DrivebaseConstants.kDisabledVisionMaxJumpM
+ || deltaRotation > DrivebaseConstants.kDisabledVisionMaxJumpRad)) {
+ Logger.recordOutput("Vision/DisabledReject", true);
+ Logger.recordOutput("Vision/DisabledBlendAlphaUsed", alpha);
+ return;
}
+ Logger.recordOutput("Vision/DisabledReject", false);
+
+ lastDisabledVisionPose = vision;
+ lastDisabledVisionTs = t;
+
+ // Preserve estimator history after the one initialization snap so delayed measurements remain
+ // meaningful and Vision does not have to reset its smoothing gate on every frame.
+ m_PoseEstimator.addVisionMeasurement(vision, t, meas.stdDevs());
+ final Pose2d fusedPose = m_PoseEstimator.getEstimatedPosition();
+ poseBufferAddSample(t, fusedPose);
+ markVisionMeasurementAccepted(t);
+
+ Logger.recordOutput("Vision/DisabledBlendedPose", fusedPose);
+ Logger.recordOutput("Vision/DisabledBlendAlphaUsed", 0.0);
+ }
+
+ private void markVisionMeasurementAccepted(double measurementTimestamp) {
+ lastAcceptedVisionMeasurementTimestamp = measurementTimestamp;
+ lastAcceptedVisionReceiptTimestamp = TimeUtil.now();
}
/**
@@ -996,35 +1107,26 @@ public void resetOdometry(Pose2d pose) {
AutoConstants.kChoreoSteerPID.kI,
AutoConstants.kChoreoSteerPID.kD);
- /**
- * Follows the given field-centric path sample with PID for Choreo
- *
- * @param pose Current pose of the robot
- * @param sample Sample along the path to follow
- */
- public void choreoController(Pose2d pose, SwerveSample sample) {
- var targetSpeeds = sample.getChassisSpeeds();
- targetSpeeds.vxMetersPerSecond += m_pathXController.calculate(pose.getX(), sample.x);
- targetSpeeds.vyMetersPerSecond += m_pathYController.calculate(pose.getY(), sample.y);
- targetSpeeds.omegaRadiansPerSecond +=
- m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading);
-
- runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(targetSpeeds, getHeading()));
- }
-
- public void followTrajectory(SwerveSample sample) {
- // Get the current pose of the robot
- Pose2d pose = getPose();
-
- // Choreo samples are field-relative; convert to robot-relative before sending to modules.
- ChassisSpeeds fieldRelativeSpeeds =
- new ChassisSpeeds(
- sample.vx + m_pathXController.calculate(pose.getX(), sample.x),
- sample.vy + m_pathYController.calculate(pose.getY(), sample.y),
- sample.omega
- + m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading));
-
- // Apply the generated speeds
- runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation()));
- }
+ // TODO(2027): Re-enable these Choreo trajectory followers when ChoreoLib supports 2027 WPILib.
+ // public void choreoController(Pose2d pose, SwerveSample sample) {
+ // var targetSpeeds = sample.getChassisSpeeds();
+ // targetSpeeds.vx += m_pathXController.calculate(pose.getX(), sample.x);
+ // targetSpeeds.vy += m_pathYController.calculate(pose.getY(), sample.y);
+ // targetSpeeds.omega +=
+ // m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading);
+ //
+ // runVelocity(targetSpeeds.toRobotRelative(getHeading()));
+ // }
+ //
+ // public void followTrajectory(SwerveSample sample) {
+ // Pose2d pose = getPose();
+ // ChassisVelocities fieldRelativeSpeeds =
+ // new ChassisVelocities(
+ // sample.vx + m_pathXController.calculate(pose.getX(), sample.x),
+ // sample.vy + m_pathYController.calculate(pose.getY(), sample.y),
+ // sample.omega
+ // + m_pathThetaController.calculate(pose.getRotation().getRadians(),
+ // sample.heading));
+ // runVelocity(fieldRelativeSpeeds.toRobotRelative(pose.getRotation()));
+ // }
}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java
index 6f4b49bb..e66b4cc0 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java
@@ -17,22 +17,28 @@
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 com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
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 java.util.ArrayList;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.driverstation.RobotState;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
public final class DriveOdometry extends VirtualSubsystem {
+ private static final int MAX_VISION_MEASUREMENTS_PER_LOOP = 2;
// Declare the io and inputs
private final Drive drive;
private final Imu imu;
private final Module[] modules;
+ private final BaseStatusSignal[] bulkRefreshSignals;
+ private final BaseStatusSignal[][] moduleBulkRefreshSignals;
// Per-cycle cached objects (to avoid repeated allocations)
private final SwerveModulePosition[] odomPositions = new SwerveModulePosition[4];
@@ -45,6 +51,16 @@ public DriveOdometry(Drive drive, Imu imu, Module[] modules) {
this.drive = drive;
this.imu = imu;
this.modules = modules;
+ moduleBulkRefreshSignals = new BaseStatusSignal[modules.length][];
+
+ ArrayList signals = new ArrayList<>();
+ for (int i = 0; i < modules.length; i++) {
+ moduleBulkRefreshSignals[i] = modules[i].getBulkRefreshSignals();
+ for (BaseStatusSignal signal : moduleBulkRefreshSignals[i]) {
+ signals.add(signal);
+ }
+ }
+ bulkRefreshSignals = signals.toArray(new BaseStatusSignal[0]);
}
/**
@@ -61,8 +77,39 @@ protected int getPeriodPriority() {
/** Periodic function to read inputs */
@Override
public void rbsiPeriodic() {
+ final long loopStartNanos = System.nanoTime();
+ final long bulkRefreshStartNanos = System.nanoTime();
+ StatusCode bulkRefreshStatus = StatusCode.OK;
+ if (SwerveConstants.kDrivebaseHasMultipleCANBuses) {
+ // Phoenix rejects refreshAll across networks; preserve each module's own connection status.
+ for (int i = 0; i < modules.length; i++) {
+ StatusCode moduleStatus = StatusCode.OK;
+ for (BaseStatusSignal signal : moduleBulkRefreshSignals[i]) {
+ StatusCode status = BaseStatusSignal.refreshAll(signal);
+ if (!status.isOK()) {
+ moduleStatus = status;
+ bulkRefreshStatus = status;
+ }
+ }
+ modules[i].setBulkRefreshStatus(moduleStatus);
+ }
+ } else {
+ bulkRefreshStatus =
+ bulkRefreshSignals.length == 0
+ ? StatusCode.OK
+ : BaseStatusSignal.refreshAll(bulkRefreshSignals);
+ for (Module module : modules) {
+ module.setBulkRefreshStatus(bulkRefreshStatus);
+ }
+ }
+ final long bulkRefreshEndNanos = System.nanoTime();
+ final long lockWaitStartNanos = System.nanoTime();
Drive.odometryLock.lock();
+ final long lockAcquiredNanos = System.nanoTime();
+ long moduleInputsEndNanos = lockAcquiredNanos;
+ long estimatorStartNanos = lockAcquiredNanos;
+ int sampleCount = 0;
try {
final var imuInputs = imu.getInputs();
@@ -70,6 +117,8 @@ public void rbsiPeriodic() {
for (var module : modules) {
module.periodic();
}
+ moduleInputsEndNanos = System.nanoTime();
+ estimatorStartNanos = moduleInputsEndNanos;
// ----------------------------------------------------------------------
// Pure SIM (not replaying a log): use sim pose/yaw
@@ -83,8 +132,8 @@ public void rbsiPeriodic() {
// Coast state uses "now" + current module positions
drive.updateDisabledCoastState(
- DriverStation.isEnabled(),
- DriverStation.isDisabled(),
+ RobotState.isEnabled(),
+ RobotState.isDisabled(),
now,
drive.getSimYawRateRadPerSec(),
drive.getModulePositions());
@@ -93,10 +142,10 @@ public void rbsiPeriodic() {
}
// ----------------------------------------------------------------------
- // 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.)
+ // DISABLED (REAL only): keep estimator history and external buffers alive so delayed vision
+ // measurements can still be applied while the robot is carried or repositioned.
// ----------------------------------------------------------------------
- if (DriverStation.isDisabled() && !isReplayActive) {
+ if (RobotState.isDisabled() && !isReplayActive) {
final double now = TimeUtil.now();
// keep yaw buffers alive
@@ -104,15 +153,20 @@ public void rbsiPeriodic() {
drive.yawBuffersAddSample(now, imuInputs.yawPositionRad, imuInputs.yawRateRadPerSec);
}
+ SwerveModulePosition[] currentPositions = drive.getModulePositions();
+
// Coast state from "now" + current module positions
drive.updateDisabledCoastState(
- DriverStation.isEnabled(),
- DriverStation.isDisabled(),
+ RobotState.isEnabled(),
+ RobotState.isDisabled(),
now,
imuInputs.yawRateRadPerSec,
- drive.getModulePositions());
+ currentPositions);
- // keep pose buffer alive with the *current estimator pose*
+ drive.poseEstimatorUpdateWithTime(
+ now, Rotation2d.fromRadians(imuInputs.yawPositionRad), currentPositions);
+
+ // Keep the external vision-alignment buffer on the same timestamp and pose.
drive.poseBufferAddSample(now, drive.getPose());
drive.setGyroDisconnectedAlert(!imuInputs.connected);
return;
@@ -123,6 +177,7 @@ public void rbsiPeriodic() {
// ----------------------------------------------------------------------
final double[] ts = modules[0].getOdometryTimestamps();
final int n = (ts == null) ? 0 : ts.length;
+ sampleCount = n;
// Always keep yaw buffers “alive” even if no samples
if (n == 0) {
@@ -132,8 +187,8 @@ public void rbsiPeriodic() {
// Coast state update (no per-sample positions available; use current)
drive.updateDisabledCoastState(
- DriverStation.isEnabled(),
- DriverStation.isDisabled(),
+ RobotState.isEnabled(),
+ RobotState.isDisabled(),
now,
imuInputs.yawRateRadPerSec,
drive.getModulePositions());
@@ -219,8 +274,8 @@ public void rbsiPeriodic() {
// Yaw rate: if you have a buffered rate, use it; otherwise imuInputs.yawRateRadPerSec is
// ok.
drive.updateDisabledCoastState(
- DriverStation.isEnabled(),
- DriverStation.isDisabled(),
+ RobotState.isEnabled(),
+ RobotState.isDisabled(),
t,
imuInputs.yawRateRadPerSec,
odomPositions);
@@ -238,7 +293,7 @@ public void rbsiPeriodic() {
// Module distance deltas (valid within batch)
for (int m = 0; m < 4; m++) {
final SwerveModulePosition pos = odomPositions[m];
- final double dist = pos.distanceMeters;
+ final double dist = pos.distance;
if (logDebug && i == n - 1) {
Logger.recordOutput("Odometry/Debug/mod" + m + "_distanceMeters", dist);
@@ -266,10 +321,34 @@ public void rbsiPeriodic() {
drive.setGyroDisconnectedAlert(!imuInputs.connected);
} finally {
+ final long visionApplyStartNanos = System.nanoTime();
+ int visionMeasurementsProcessed = 0;
+ long visionApplyEndNanos;
+ try {
+ visionMeasurementsProcessed =
+ drive.applyQueuedVisionMeasurements(MAX_VISION_MEASUREMENTS_PER_LOOP);
+ Logger.recordOutput("Odometry/Robot", drive.getPose());
+ } finally {
+ visionApplyEndNanos = System.nanoTime();
+ Drive.odometryLock.unlock();
+ }
- Logger.recordOutput("Odometry/Robot", drive.getPose());
-
- Drive.odometryLock.unlock();
+ final long loopEndNanos = System.nanoTime();
+ Logger.recordOutput(
+ "Odometry/Timing/BulkRefreshMS", (bulkRefreshEndNanos - bulkRefreshStartNanos) / 1e6);
+ Logger.recordOutput(
+ "Odometry/Timing/LockWaitMS", (lockAcquiredNanos - lockWaitStartNanos) / 1e6);
+ Logger.recordOutput(
+ "Odometry/Timing/ModuleInputsAndQueueDrainMS",
+ (moduleInputsEndNanos - lockAcquiredNanos) / 1e6);
+ Logger.recordOutput(
+ "Odometry/Timing/EstimatorMS", (visionApplyStartNanos - estimatorStartNanos) / 1e6);
+ Logger.recordOutput(
+ "Odometry/Timing/VisionApplyMS", (visionApplyEndNanos - visionApplyStartNanos) / 1e6);
+ Logger.recordOutput("Odometry/Timing/TotalMS", (loopEndNanos - loopStartNanos) / 1e6);
+ Logger.recordOutput("Odometry/SamplesProcessed", sampleCount);
+ Logger.recordOutput("Odometry/VisionMeasurementsProcessed", visionMeasurementsProcessed);
+ Logger.recordOutput("Odometry/BulkRefreshStatus", bulkRefreshStatus.toString());
}
}
}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java
index ff4402ce..c2f05afe 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java
@@ -17,12 +17,12 @@
package frc.robot.subsystems.drive;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
+import frc.robot.util.MathUtil;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
/**
* Authoritative drivetrain physics model for SIM.
@@ -59,14 +59,13 @@ public DriveSimPhysics(
* @param moduleStates Authoritative wheel states
* @param dtSeconds Loop period
*/
- public void update(SwerveModuleState[] moduleStates, double dtSeconds) {
+ public void update(SwerveModuleVelocity[] moduleStates, double dtSeconds) {
// ------------------ CHASSIS VELOCITY ------------------
- var chassis = kinematics.toChassisSpeeds(moduleStates);
+ var chassis = kinematics.toChassisVelocities(moduleStates);
// Robot-relative velocity
- Translation2d robotVelocity =
- new Translation2d(chassis.vxMetersPerSecond, chassis.vyMetersPerSecond);
+ Translation2d robotVelocity = new Translation2d(chassis.vx, chassis.vy);
// Rotate into field frame
Translation2d fieldVelocity = robotVelocity.rotateBy(pose.getRotation());
@@ -79,7 +78,7 @@ public void update(SwerveModuleState[] moduleStates, double dtSeconds) {
Translation2d newTranslation = pose.getTranslation().plus(fieldVelocity.times(dtSeconds));
// ------------------ ANGULAR ------------------
- double commandedOmega = chassis.omegaRadiansPerSecond;
+ double commandedOmega = chassis.omega;
// Simple torque model with damping
double torque =
diff --git a/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java b/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java
new file mode 100644
index 00000000..2018c258
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/LatestSampleQueue.java
@@ -0,0 +1,94 @@
+// 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.
+
+package frc.robot.subsystems.drive;
+
+import java.util.AbstractQueue;
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.Iterator;
+import java.util.concurrent.locks.ReentrantLock;
+
+/** Fixed-capacity thread-safe queue that evicts its oldest entry when full. */
+final class LatestSampleQueue extends AbstractQueue {
+ private final int capacity;
+ private final ArrayDeque values;
+ private final ReentrantLock lock = new ReentrantLock();
+
+ LatestSampleQueue(int capacity) {
+ if (capacity <= 0) {
+ throw new IllegalArgumentException("capacity must be positive");
+ }
+ this.capacity = capacity;
+ values = new ArrayDeque<>(capacity);
+ }
+
+ /** Adds a value and returns whether the oldest value had to be evicted. */
+ boolean offerLatest(E value) {
+ if (value == null) {
+ throw new NullPointerException("LatestSampleQueue does not accept null values");
+ }
+ lock.lock();
+ try {
+ boolean evicted = values.size() == capacity;
+ if (evicted) {
+ values.removeFirst();
+ }
+ values.addLast(value);
+ return evicted;
+ } finally {
+ lock.unlock();
+ }
+ }
+
+ @Override
+ public boolean offer(E value) {
+ offerLatest(value);
+ return true;
+ }
+
+ @Override
+ public E poll() {
+ lock.lock();
+ try {
+ return values.pollFirst();
+ } finally {
+ lock.unlock();
+ }
+ }
+
+ @Override
+ public E peek() {
+ lock.lock();
+ try {
+ return values.peekFirst();
+ } finally {
+ lock.unlock();
+ }
+ }
+
+ @Override
+ public int size() {
+ lock.lock();
+ try {
+ return values.size();
+ } finally {
+ lock.unlock();
+ }
+ }
+
+ @Override
+ public Iterator iterator() {
+ lock.lock();
+ try {
+ return new ArrayList<>(values).iterator();
+ } finally {
+ lock.unlock();
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
index 87db48e6..a516e0d9 100644
--- a/src/main/java/frc/robot/subsystems/drive/Module.java
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -9,14 +9,16 @@
package frc.robot.subsystems.drive;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Alert;
-import edu.wpi.first.wpilibj.Alert.AlertType;
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
import frc.robot.Constants.DrivebaseConstants;
+import frc.robot.util.Alert;
+import frc.robot.util.Alert.AlertType;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
+import org.wpilib.math.util.Units;
/**
* Module API Class
@@ -45,15 +47,14 @@ public Module(ModuleIO io, int index) {
this.index = index;
driveDisconnectedAlert =
new Alert(
- "Disconnected drive motor on module " + Integer.toString(index) + ".",
- AlertType.kError);
+ "Disconnected drive motor on module " + Integer.toString(index) + ".", AlertType.ERROR);
turnDisconnectedAlert =
new Alert(
- "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError);
+ "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.ERROR);
turnEncoderDisconnectedAlert =
new Alert(
"Disconnected turn encoder on module " + Integer.toString(index) + ".",
- AlertType.kError);
+ AlertType.ERROR);
}
/************************************************************************* */
@@ -88,6 +89,16 @@ public void periodic() {
turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
}
+ /** Returns this module's CTRE telemetry signals for drivetrain-wide refresh. */
+ BaseStatusSignal[] getBulkRefreshSignals() {
+ return io.getBulkRefreshSignals();
+ }
+
+ /** Supplies the result of the drivetrain-wide telemetry refresh. */
+ void setBulkRefreshStatus(StatusCode status) {
+ io.setBulkRefreshStatus(status);
+ }
+
/** Forwards the simulation periodic call to the IO layer */
public void simulationPeriodic() {
io.simulationPeriodic();
@@ -101,13 +112,13 @@ public void simulationPeriodic() {
*
* @param state The requested Swerve Modeule State
*/
- public void runSetpoint(SwerveModuleState state) {
+ public void runSetpoint(SwerveModuleVelocity state) {
// Optimize velocity setpoint
- state.optimize(getAngle());
- state.cosineScale(inputs.turnPosition);
+ state = state.optimize(getAngle());
+ state = state.cosineScale(inputs.turnPosition);
// Apply setpoints
- io.setDriveVelocity(state.speedMetersPerSecond / DrivebaseConstants.kWheelRadiusMeters);
+ io.setDriveVelocity(state.velocity / DrivebaseConstants.kWheelRadiusMeters);
io.setTurnPosition(state.angle);
}
@@ -155,8 +166,8 @@ public SwerveModulePosition getPosition() {
}
/** Returns the module state (turn angle and drive velocity). */
- public SwerveModuleState getState() {
- return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
+ public SwerveModuleVelocity getState() {
+ return new SwerveModuleVelocity(getVelocityMetersPerSec(), getAngle());
}
/** Returns the module positions received this cycle. */
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
index 7affb4f5..79433170 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
@@ -9,10 +9,13 @@
package frc.robot.subsystems.drive;
-import edu.wpi.first.math.geometry.Rotation2d;
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
import org.littletonrobotics.junction.AutoLog;
+import org.wpilib.math.geometry.Rotation2d;
public interface ModuleIO {
+ BaseStatusSignal[] NO_BULK_REFRESH_SIGNALS = new BaseStatusSignal[0];
double[] EMPTY_DOUBLE_ARRAY = new double[0];
Rotation2d[] EMPTY_ROTATION_ARRAY = new Rotation2d[0];
@@ -40,6 +43,14 @@ public static class ModuleIOInputs {
/** Updates the set of loggable inputs. */
public default void updateInputs(ModuleIOInputs inputs) {}
+ /** Returns telemetry signals that should be refreshed in one drivetrain-wide transaction. */
+ public default BaseStatusSignal[] getBulkRefreshSignals() {
+ return NO_BULK_REFRESH_SIGNALS;
+ }
+
+ /** Supplies the result of the drivetrain-wide telemetry refresh. */
+ public default void setBulkRefreshStatus(StatusCode status) {}
+
/** Run the drive motor at the specified open loop value. */
public default void setDriveOpenLoop(double output) {}
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
index b0e291f9..bbfe5a54 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
@@ -10,7 +10,7 @@
package frc.robot.subsystems.drive;
import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
@@ -32,23 +32,15 @@
import com.revrobotics.ResetMode;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.geometry.Rotation2d;
-import 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.MathUtil;
import frc.robot.util.PhoenixUtil;
import frc.robot.util.RBSICANBusRegistry;
import frc.robot.util.RBSIEnum.CTREPro;
@@ -56,6 +48,14 @@
import java.util.Arrays;
import java.util.Queue;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.util.Units;
+import org.wpilib.system.RobotController;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.units.measure.AngularVelocity;
+import org.wpilib.units.measure.Current;
+import org.wpilib.units.measure.Voltage;
/**
* Module IO implementation for blended TalonFX drive motor controller, SparkMax turn motor
@@ -104,6 +104,8 @@ public class ModuleIOBlended implements ModuleIO {
private final StatusSignal turnPosition;
private final Queue turnPositionQueue;
private final StatusSignal turnVelocity;
+ private final BaseStatusSignal[] bulkRefreshSignals;
+ private StatusCode bulkRefreshStatus = StatusCode.OK;
// Connection debouncers
private final Debouncer driveConnectedDebounce =
@@ -179,9 +181,12 @@ public ModuleIOBlended(int module) {
default -> throw new IllegalArgumentException("Invalid module index");
};
- CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName);
+ String moduleBus = SwerveConstants.moduleCANBus(module);
+ var canBus = RBSICANBusRegistry.getBus(moduleBus);
driveTalon = new TalonFX(constants.DriveMotorId, canBus);
- turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless);
+ turnSpark =
+ new SparkMax(
+ Constants.CANBuses.revPort(moduleBus), constants.SteerMotorId, MotorType.kBrushless);
cancoder = new CANcoder(constants.EncoderId, canBus);
turnController = turnSpark.getClosedLoopController();
@@ -199,7 +204,7 @@ public ModuleIOBlended(int module) {
driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio;
driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrentAmps;
driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrentAmps;
- driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit;
+ driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrentAmps;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
// Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing
OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs();
@@ -280,7 +285,8 @@ public ModuleIOBlended(int module) {
// Create drive status signals
drivePosition = driveTalon.getPosition();
drivePositionOdom = drivePosition.clone(); // NEW
- drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom);
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(moduleBus, drivePositionOdom);
driveVelocity = driveTalon.getVelocity();
driveAppliedVolts = driveTalon.getMotorVoltage();
driveCurrent = driveTalon.getStatorCurrent();
@@ -289,7 +295,19 @@ public ModuleIOBlended(int module) {
turnVelocity = cancoder.getVelocity();
turnAbsolutePosition = cancoder.getAbsolutePosition();
turnPosition = cancoder.getPosition();
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(cancoder.getPosition());
+ turnPositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(moduleBus, cancoder.getPosition());
+
+ bulkRefreshSignals =
+ new BaseStatusSignal[] {
+ drivePosition,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnAbsolutePosition,
+ turnPosition,
+ turnVelocity
+ };
// Configure periodic frames
BaseStatusSignal.setUpdateFrequencyForAll(
@@ -301,21 +319,33 @@ public ModuleIOBlended(int module) {
}
@Override
- public void updateInputs(ModuleIOInputs inputs) {
- // Refresh signals
- var driveStatus =
- BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
- var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+ public BaseStatusSignal[] getBulkRefreshSignals() {
+ return bulkRefreshSignals;
+ }
+
+ @Override
+ public void setBulkRefreshStatus(StatusCode status) {
+ bulkRefreshStatus = status;
+ }
- if (!driveStatus.isOK()) {
- Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString());
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ boolean driveStatus =
+ BaseStatusSignal.isAllGood(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ boolean encStatus =
+ BaseStatusSignal.isAllGood(turnAbsolutePosition, turnPosition, turnVelocity);
+
+ if (!driveStatus) {
+ Logger.recordOutput(
+ "CAN/Module" + module + "/DriveRefreshStatus", bulkRefreshStatus.toString());
}
- if (!encStatus.isOK()) {
- Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString());
+ if (!encStatus) {
+ Logger.recordOutput(
+ "CAN/Module" + module + "/EncRefreshStatus", bulkRefreshStatus.toString());
}
// Drive inputs
- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus);
inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble());
inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble());
inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
@@ -323,7 +353,7 @@ public void updateInputs(ModuleIOInputs inputs) {
// Turn inputs (Spark for turn motor, CANcoder for absolute)
SparkUtil.sparkStickyFault = false;
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus);
inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble());
inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble());
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
index 740d42e2..ea13a509 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -9,15 +9,15 @@
package frc.robot.subsystems.drive;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import frc.robot.Constants;
+import frc.robot.util.MathUtil;
import frc.robot.util.TimeUtil;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.system.DCMotor;
+import org.wpilib.math.system.Models;
+import org.wpilib.math.util.Units;
+import org.wpilib.simulation.DCMotorSim;
/**
* Physics sim implementation of module IO. The sim models are configured using a set of module
@@ -51,12 +51,12 @@ public ModuleIOSim() {
// Create drive and turn sim models
driveSim =
new DCMotorSim(
- LinearSystemId.createDCMotorSystem(
+ Models.singleJointedArmFromPhysicalConstants(
DRIVE_GEARBOX, SwerveConstants.kDriveInertia, SwerveConstants.kDriveGearRatio),
DRIVE_GEARBOX);
turnSim =
new DCMotorSim(
- LinearSystemId.createDCMotorSystem(
+ Models.singleJointedArmFromPhysicalConstants(
TURN_GEARBOX, SwerveConstants.kSteerInertia, SwerveConstants.kSteerGearRatio),
TURN_GEARBOX);
@@ -68,15 +68,14 @@ public ModuleIOSim() {
public void simulationPeriodic() {
// Closed-loop control
if (driveClosedLoop) {
- driveAppliedVolts =
- driveFFVolts + driveController.calculate(driveSim.getAngularVelocityRadPerSec());
+ driveAppliedVolts = driveFFVolts + driveController.calculate(driveSim.getAngularVelocity());
} else {
driveController.reset();
}
if (turnClosedLoop) {
// Simple angle loop for module simulation; real feedforward is handled by hardware IO.
- turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad());
+ turnAppliedVolts = turnController.calculate(turnSim.getAngularPosition());
} else {
turnController.reset();
}
@@ -93,27 +92,26 @@ public void simulationPeriodic() {
@Override
public void updateInputs(ModuleIOInputs inputs) {
// Convert rotor -> mechanism
- double mechanismPositionRad = driveSim.getAngularPositionRad();
- double mechanismVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec();
+ double mechanismPositionRad = driveSim.getAngularPosition();
+ double mechanismVelocityRadPerSec = driveSim.getAngularVelocity();
// Drive inputs
inputs.driveConnected = true;
inputs.drivePositionRad = mechanismPositionRad;
inputs.driveVelocityRadPerSec = mechanismVelocityRadPerSec;
inputs.driveAppliedVolts = driveAppliedVolts;
- inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps());
+ inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDraw());
// Turn inputs
- double steerPositionRad = turnSim.getAngularPositionRad();
+ double steerPositionRad = turnSim.getAngularPosition();
inputs.turnConnected = true;
inputs.turnEncoderConnected = true;
inputs.turnAbsolutePosition = new Rotation2d(steerPositionRad);
inputs.turnPosition = new Rotation2d(steerPositionRad);
- inputs.turnVelocityRadPerSec =
- turnSim.getAngularVelocityRadPerSec() / SwerveConstants.kSteerGearRatio;
+ inputs.turnVelocityRadPerSec = turnSim.getAngularVelocity() / SwerveConstants.kSteerGearRatio;
inputs.turnAppliedVolts = turnAppliedVolts;
- inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps());
+ inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDraw());
// Odometry (single sample per loop is fine)
inputs.odometryTimestamps = new double[] {TimeUtil.now()};
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java
index 0fc8c2cc..e449848a 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java
@@ -16,26 +16,26 @@
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits;
import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.RobotController;
import frc.robot.Constants;
import frc.robot.Constants.DrivebaseConstants;
+import frc.robot.util.MathUtil;
import frc.robot.util.SparkUtil;
import java.util.Arrays;
import java.util.Queue;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.system.RobotController;
/**
* Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller,
@@ -85,6 +85,7 @@ public ModuleIOSpark(int module) {
};
driveSpark =
new SparkFlex(
+ Constants.CANBuses.revPort(SwerveConstants.moduleCANBus(module)),
switch (module) {
case 0 -> SwerveConstants.kFLDriveMotorId;
case 1 -> SwerveConstants.kFRDriveMotorId;
@@ -95,6 +96,7 @@ public ModuleIOSpark(int module) {
MotorType.kBrushless);
turnSpark =
new SparkMax(
+ Constants.CANBuses.revPort(SwerveConstants.moduleCANBus(module)),
switch (module) {
case 0 -> SwerveConstants.kFLSteerMotorId;
case 1 -> SwerveConstants.kFRSteerMotorId;
@@ -208,9 +210,11 @@ public ModuleIOSpark(int module) {
// Create odometry queues
timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
drivePositionQueue =
- SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition);
+ SparkOdometryThread.getInstance()
+ .registerSignal(driveSpark, () -> driveEncoder.getPosition().get(0.0));
turnPositionQueue =
- SparkOdometryThread.getInstance().registerSignal(turnSpark, turnEncoder::getPosition);
+ SparkOdometryThread.getInstance()
+ .registerSignal(turnSpark, () -> turnEncoder.getPosition().get(0.0));
}
@Override
@@ -223,7 +227,9 @@ public void updateInputs(ModuleIOInputs inputs) {
driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value);
SparkUtil.ifOk(
driveSpark,
- new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage},
+ new DoubleSupplier[] {
+ () -> driveSpark.getAppliedOutput().get(0.0), () -> driveSpark.getBusVoltage().get(0.0)
+ },
(values) -> inputs.driveAppliedVolts = values[0] * values[1]);
SparkUtil.ifOk(
driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value);
@@ -239,7 +245,9 @@ public void updateInputs(ModuleIOInputs inputs) {
turnSpark, turnEncoder::getVelocity, (value) -> inputs.turnVelocityRadPerSec = value);
SparkUtil.ifOk(
turnSpark,
- new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},
+ new DoubleSupplier[] {
+ () -> turnSpark.getAppliedOutput().get(0.0), () -> turnSpark.getBusVoltage().get(0.0)
+ },
(values) -> inputs.turnAppliedVolts = values[0] * values[1]);
SparkUtil.ifOk(
turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value);
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
index 743bb495..fb416ffb 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
@@ -10,6 +10,7 @@
package frc.robot.subsystems.drive;
import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.hardware.CANcoder;
import com.revrobotics.PersistMode;
@@ -18,30 +19,30 @@
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits;
import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj.RobotController;
import frc.robot.Constants;
import frc.robot.Constants.DrivebaseConstants;
+import frc.robot.util.MathUtil;
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;
+import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.util.Units;
+import org.wpilib.system.RobotController;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.units.measure.AngularVelocity;
/**
* Module IO implementation for Spark Flex drive motor controller, Spark Max turn motor controller,
@@ -70,6 +71,8 @@ public class ModuleIOSparkCANcoder implements ModuleIO {
private final StatusSignal turnAbsolutePosition;
private final StatusSignal turnPosition;
private final StatusSignal turnVelocity;
+ private final BaseStatusSignal[] bulkRefreshSignals;
+ private StatusCode bulkRefreshStatus = StatusCode.OK;
// Queue inputs from odometry thread
private final Queue timestampQueue;
@@ -105,6 +108,7 @@ public ModuleIOSparkCANcoder(int module) {
};
driveSpark =
new SparkFlex(
+ Constants.CANBuses.revPort(SwerveConstants.moduleCANBus(module)),
switch (module) {
case 0 -> SwerveConstants.kFLDriveMotorId;
case 1 -> SwerveConstants.kFRDriveMotorId;
@@ -115,6 +119,7 @@ public ModuleIOSparkCANcoder(int module) {
MotorType.kBrushless);
turnSpark =
new SparkMax(
+ Constants.CANBuses.revPort(SwerveConstants.moduleCANBus(module)),
switch (module) {
case 0 -> SwerveConstants.kFLSteerMotorId;
case 1 -> SwerveConstants.kFRSteerMotorId;
@@ -149,7 +154,7 @@ public ModuleIOSparkCANcoder(int module) {
case 3 -> SwerveConstants.kBREncoderId;
default -> 0;
},
- RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName));
+ RBSICANBusRegistry.getBus(SwerveConstants.moduleCANBus(module)));
driveController = driveSpark.getClosedLoopController();
turnController = turnSpark.getClosedLoopController();
@@ -235,12 +240,15 @@ public ModuleIOSparkCANcoder(int module) {
turnVelocity = cancoder.getVelocity();
turnAbsolutePosition = cancoder.getAbsolutePosition();
turnPosition = cancoder.getPosition();
+ // turnPosition is refreshed by SparkOdometryThread for the high-rate queue.
+ bulkRefreshSignals = new BaseStatusSignal[] {turnAbsolutePosition, turnVelocity};
BaseStatusSignal.setUpdateFrequencyForAll(SwerveConstants.kOdometryFrequency, turnPosition);
// Create odometry queues
timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
drivePositionQueue =
- SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition);
+ SparkOdometryThread.getInstance()
+ .registerSignal(driveSpark, () -> driveEncoder.getPosition().get(0.0));
turnPositionQueue =
SparkOdometryThread.getInstance()
.registerSignal(
@@ -250,12 +258,23 @@ public ModuleIOSparkCANcoder(int module) {
});
}
+ @Override
+ public BaseStatusSignal[] getBulkRefreshSignals() {
+ return bulkRefreshSignals;
+ }
+
+ @Override
+ public void setBulkRefreshStatus(StatusCode status) {
+ bulkRefreshStatus = status;
+ }
+
@Override
public void updateInputs(ModuleIOInputs inputs) {
- // Refresh CANcoder absolute
- var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition, turnPosition, turnVelocity);
- if (!encStatus.isOK()) {
- Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString());
+ boolean encStatus =
+ BaseStatusSignal.isAllGood(turnAbsolutePosition, turnPosition, turnVelocity);
+ if (!encStatus) {
+ Logger.recordOutput(
+ "CAN/Module" + module + "/EncRefreshStatus", bulkRefreshStatus.toString());
}
// Drive inputs (Spark)
@@ -266,7 +285,9 @@ public void updateInputs(ModuleIOInputs inputs) {
driveSpark, driveEncoder::getVelocity, (value) -> inputs.driveVelocityRadPerSec = value);
SparkUtil.ifOk(
driveSpark,
- new DoubleSupplier[] {driveSpark::getAppliedOutput, driveSpark::getBusVoltage},
+ new DoubleSupplier[] {
+ () -> driveSpark.getAppliedOutput().get(0.0), () -> driveSpark.getBusVoltage().get(0.0)
+ },
(values) -> inputs.driveAppliedVolts = values[0] * values[1]);
SparkUtil.ifOk(
driveSpark, driveSpark::getOutputCurrent, (value) -> inputs.driveCurrentAmps = value);
@@ -274,13 +295,15 @@ public void updateInputs(ModuleIOInputs inputs) {
// Turn inputs (CANcoder abs + Spark velocity/applied/current)
SparkUtil.sparkStickyFault = false;
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK());
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus);
inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble());
inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble());
SparkUtil.ifOk(
turnSpark,
- new DoubleSupplier[] {turnSpark::getAppliedOutput, turnSpark::getBusVoltage},
+ new DoubleSupplier[] {
+ () -> turnSpark.getAppliedOutput().get(0.0), () -> turnSpark.getBusVoltage().get(0.0)
+ },
(values) -> inputs.turnAppliedVolts = values[0] * values[1]);
SparkUtil.ifOk(
turnSpark, turnSpark::getOutputCurrent, (value) -> inputs.turnCurrentAmps = value);
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
index 13f35389..ca6e787c 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -10,7 +10,7 @@
package frc.robot.subsystems.drive;
import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
@@ -21,7 +21,6 @@
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.ParentDevice;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
@@ -30,14 +29,6 @@
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.TunerFactory;
@@ -47,6 +38,14 @@
import java.util.Arrays;
import java.util.Queue;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.filter.Debouncer;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.util.Units;
+import org.wpilib.system.RobotController;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.units.measure.AngularVelocity;
+import org.wpilib.units.measure.Current;
+import org.wpilib.units.measure.Voltage;
/**
* Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
@@ -94,6 +93,8 @@ public class ModuleIOTalonFX implements ModuleIO {
private final StatusSignal turnVelocity;
private final StatusSignal turnAppliedVolts;
private final StatusSignal turnCurrent;
+ private final BaseStatusSignal[] bulkRefreshSignals;
+ private StatusCode bulkRefreshStatus = StatusCode.OK;
// Connection debouncers
private final Debouncer driveConnectedDebounce =
@@ -127,7 +128,8 @@ public ModuleIOTalonFX(int module) {
default -> throw new IllegalArgumentException("Invalid module index");
};
- CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName);
+ String moduleBus = SwerveConstants.moduleCANBus(module);
+ var canBus = RBSICANBusRegistry.getBus(moduleBus);
driveTalon = new TalonFX(constants.DriveMotorId, canBus);
turnTalon = new TalonFX(constants.SteerMotorId, canBus);
cancoder = new CANcoder(constants.EncoderId, canBus);
@@ -147,7 +149,7 @@ public ModuleIOTalonFX(int module) {
driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio;
driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrentAmps;
driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrentAmps;
- driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit;
+ driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrentAmps;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
// Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing
OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs();
@@ -211,7 +213,8 @@ public ModuleIOTalonFX(int module) {
// Create drive status signals
drivePosition = driveTalon.getPosition();
drivePositionOdom = drivePosition.clone(); // NEW
- drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom);
+ drivePositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(moduleBus, drivePositionOdom);
driveVelocity = driveTalon.getVelocity();
driveAppliedVolts = driveTalon.getMotorVoltage();
@@ -220,57 +223,76 @@ public ModuleIOTalonFX(int module) {
// Create turn status signals
turnPosition = turnTalon.getPosition();
turnPositionOdom = turnPosition.clone(); // NEW
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPositionOdom);
+ turnPositionQueue =
+ PhoenixOdometryThread.getInstance().registerSignal(moduleBus, turnPositionOdom);
turnAbsolutePosition = cancoder.getAbsolutePosition();
turnVelocity = turnTalon.getVelocity();
turnAppliedVolts = turnTalon.getMotorVoltage();
turnCurrent = turnTalon.getStatorCurrent();
+ bulkRefreshSignals =
+ new BaseStatusSignal[] {
+ drivePosition,
+ driveVelocity,
+ driveAppliedVolts,
+ driveCurrent,
+ turnPosition,
+ turnVelocity,
+ turnAppliedVolts,
+ turnCurrent,
+ turnAbsolutePosition
+ };
+
// Configure periodic frames (IMPORTANT: apply odometry rate to the *odom clones*)
+ drivePositionOdom.setUpdateFrequency(SwerveConstants.kOdometryFrequency);
+ turnPositionOdom.setUpdateFrequency(SwerveConstants.kOdometryFrequency);
BaseStatusSignal.setUpdateFrequencyForAll(
- SwerveConstants.kOdometryFrequency, drivePositionOdom, turnPositionOdom);
+ 50.0, drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
BaseStatusSignal.setUpdateFrequencyForAll(
- 50.0,
- drivePosition,
- turnPosition,
- driveVelocity,
- driveAppliedVolts,
- driveCurrent,
- turnAbsolutePosition,
- turnVelocity,
- turnAppliedVolts,
- turnCurrent);
-
- ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon);
+ 50.0, turnPosition, turnVelocity, turnAppliedVolts, turnCurrent, turnAbsolutePosition);
+
+ driveTalon.optimizeBusUtilization();
+ turnTalon.optimizeBusUtilization();
}
/** Input Updating Loop ************************************************** */
@Override
- public void updateInputs(ModuleIOInputs inputs) {
+ public BaseStatusSignal[] getBulkRefreshSignals() {
+ return bulkRefreshSignals;
+ }
- // Refresh Phoenix signals
- var driveStatus =
- BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
- var turnStatus =
- BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
- var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+ @Override
+ public void setBulkRefreshStatus(StatusCode status) {
+ bulkRefreshStatus = status;
+ }
+
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ boolean driveStatus =
+ BaseStatusSignal.isAllGood(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent);
+ boolean turnStatus =
+ BaseStatusSignal.isAllGood(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+ boolean encStatus = BaseStatusSignal.isAllGood(turnAbsolutePosition);
// Log *which* groups are failing and what the code is
- if (!driveStatus.isOK()) {
- Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString());
+ if (!driveStatus) {
+ Logger.recordOutput(
+ "CAN/Module" + module + "/DriveRefreshStatus", bulkRefreshStatus.toString());
}
- if (!turnStatus.isOK()) {
- Logger.recordOutput("CAN/Module" + module + "/TurnRefreshStatus", turnStatus.toString());
+ if (!turnStatus) {
+ Logger.recordOutput(
+ "CAN/Module" + module + "/TurnRefreshStatus", bulkRefreshStatus.toString());
}
- if (!encStatus.isOK()) {
- Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString());
+ if (!encStatus) {
+ Logger.recordOutput(
+ "CAN/Module" + module + "/EncRefreshStatus", bulkRefreshStatus.toString());
}
// Connectivity flags
- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
- inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK());
- inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK());
+ inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus);
+ inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus);
+ inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus);
// Update drive inputs
inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble());
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
index bd618b43..8b6a162d 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
@@ -12,7 +12,7 @@
// package frc.robot.subsystems.drive;
-// import static edu.wpi.first.units.Units.RotationsPerSecond;
+// import static org.wpilib.units.Units.RotationsPerSecond;
// import com.ctre.phoenix6.BaseStatusSignal;
// import com.ctre.phoenix6.CANBus;
@@ -40,14 +40,14 @@
// 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 org.wpilib.math.filter.Debouncer;
+// import org.wpilib.math.geometry.Rotation2d;
+// import org.wpilib.math.util.Units;
+// import org.wpilib.units.measure.Angle;
+// import org.wpilib.units.measure.AngularVelocity;
+// import org.wpilib.units.measure.Current;
+// import org.wpilib.units.measure.Voltage;
+// import org.wpilib.system.RobotController;
// import frc.robot.Constants;
// import frc.robot.Constants.DrivebaseConstants;
// import frc.robot.generated.TunerConstants;
diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
index e19f5eec..4dfb496e 100644
--- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
+++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
@@ -10,38 +10,42 @@
package frc.robot.subsystems.drive;
import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusSignal;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.RobotController;
-import frc.robot.generated.TunerFactory;
import java.util.ArrayList;
+import java.util.LinkedHashMap;
import java.util.List;
+import java.util.Map;
import java.util.Queue;
-import java.util.concurrent.ArrayBlockingQueue;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.driverstation.DriverStationErrors;
+import org.wpilib.system.RobotController;
+import org.wpilib.units.measure.Angle;
/**
* Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
*
- * This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using
- * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling.
- * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore
- * time synchronization.
+ *
Signals on one CAN FD bus use the "waitForAll" blocking method. When signals span multiple
+ * buses, they are refreshed in separate per-bus groups because Phoenix bulk calls reject mixed
+ * networks. The resulting shared timestamp is an approximation, not cross-bus synchronization.
*/
public class PhoenixOdometryThread extends Thread {
+ private static final int QUEUE_CAPACITY = 128;
+
private final Lock signalsLock =
new ReentrantLock(); // Prevents conflicts when registering signals
private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
+ private final List phoenixSignalBuses = new ArrayList<>();
+ private Map phoenixSignalsByBus = Map.of();
+ private boolean canWaitForAll = false;
private final List genericSignals = new ArrayList<>();
- private final List> phoenixQueues = new ArrayList<>();
- private final List> genericQueues = new ArrayList<>();
- private final List> timestampQueues = new ArrayList<>();
+ private final List> phoenixQueues = new ArrayList<>();
+ private final List> genericQueues = new ArrayList<>();
+ private final List> timestampQueues = new ArrayList<>();
- private static final boolean isCANFD = TunerFactory.INSTANCE.canBus().isNetworkFD();
private static PhoenixOdometryThread instance = null;
private long droppedSamples = 0;
@@ -61,14 +65,14 @@ private PhoenixOdometryThread() {
@Override
public void start() {
- if (timestampQueues.size() > 0) {
+ if (!timestampQueues.isEmpty()) {
super.start();
}
}
/** Registers a Phoenix signal to be read from the thread. */
- public Queue registerSignal(StatusSignal signal) {
- Queue queue = new ArrayBlockingQueue<>(128); // was 20
+ public Queue registerSignal(String busName, StatusSignal signal) {
+ LatestSampleQueue queue = createQueue();
signalsLock.lock();
Drive.odometryLock.lock();
try {
@@ -76,32 +80,44 @@ public Queue registerSignal(StatusSignal signal) {
System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length);
newSignals[phoenixSignals.length] = signal;
phoenixSignals = newSignals;
+ phoenixSignalBuses.add(busName);
+ Map> groups = new LinkedHashMap<>();
+ for (int i = 0; i < phoenixSignals.length; i++) {
+ groups
+ .computeIfAbsent(phoenixSignalBuses.get(i), ignored -> new ArrayList<>())
+ .add(phoenixSignals[i]);
+ }
+ Map groupedArrays = new LinkedHashMap<>();
+ groups.forEach(
+ (bus, signals) -> groupedArrays.put(bus, signals.toArray(new BaseStatusSignal[0])));
+ phoenixSignalsByBus = groupedArrays;
+ canWaitForAll = groups.size() == 1 && new CANBus(busName).isNetworkFD();
phoenixQueues.add(queue);
} finally {
- signalsLock.unlock();
Drive.odometryLock.unlock();
+ signalsLock.unlock();
}
return queue;
}
/** Registers a generic signal to be read from the thread. */
public Queue registerSignal(DoubleSupplier signal) {
- Queue queue = new ArrayBlockingQueue<>(128); // was 20
+ LatestSampleQueue queue = createQueue();
signalsLock.lock();
Drive.odometryLock.lock();
try {
genericSignals.add(signal);
genericQueues.add(queue);
} finally {
- signalsLock.unlock();
Drive.odometryLock.unlock();
+ signalsLock.unlock();
}
return queue;
}
/** Returns a new queue that returns timestamp values for each sample. */
public Queue makeTimestampQueue() {
- Queue queue = new ArrayBlockingQueue<>(128); // was 20
+ LatestSampleQueue queue = createQueue();
Drive.odometryLock.lock();
try {
timestampQueues.add(queue);
@@ -117,17 +133,18 @@ public void run() {
// Wait for updates from all signals
signalsLock.lock();
try {
- if (isCANFD && phoenixSignals.length > 0) {
+ if (canWaitForAll && phoenixSignals.length > 0) {
BaseStatusSignal.waitForAll(2.0 / SwerveConstants.kOdometryFrequency, phoenixSignals);
} else {
- // "waitForAll" does not support blocking on multiple signals with a bus
- // that is not CAN FD, regardless of Pro licensing. No reasoning for this
- // behavior is provided by the documentation.
+ // Phoenix bulk operations cannot span CAN networks. Poll each bus separately when
+ // the drivetrain is split, or when the only bus does not support FD blocking.
Thread.sleep((long) (1000.0 / SwerveConstants.kOdometryFrequency));
- if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals);
+ for (BaseStatusSignal[] busSignals : phoenixSignalsByBus.values()) {
+ BaseStatusSignal.refreshAll(busSignals);
+ }
}
} catch (InterruptedException e) {
- DriverStation.reportWarning("Phoenix odometry thread interrupted", e.getStackTrace());
+ DriverStationErrors.reportWarning("Phoenix odometry thread interrupted", e.getStackTrace());
Thread.currentThread().interrupt();
return;
} finally {
@@ -140,7 +157,7 @@ public void run() {
// Sample timestamp is current FPGA time minus average CAN latency
// Default timestamps from Phoenix are NOT compatible with
// FPGA timestamps, this solution is imperfect but close
- double timestamp = RobotController.getFPGATime() / 1e6;
+ double timestamp = RobotController.getTime() / 1e6;
double totalLatency = 0.0;
for (BaseStatusSignal signal : phoenixSignals) {
totalLatency += signal.getTimestamp().getLatency();
@@ -151,13 +168,13 @@ public void run() {
// Add new samples to queues
for (int i = 0; i < phoenixSignals.length; i++) {
- if (!phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble())) droppedSamples++;
+ offerSample(phoenixQueues.get(i), phoenixSignals[i].getValueAsDouble());
}
for (int i = 0; i < genericSignals.size(); i++) {
- if (!genericQueues.get(i).offer(genericSignals.get(i).getAsDouble())) droppedSamples++;
+ offerSample(genericQueues.get(i), genericSignals.get(i).getAsDouble());
}
- for (int i = 0; i < timestampQueues.size(); i++) {
- if (!timestampQueues.get(i).offer(timestamp)) droppedSamples++;
+ for (LatestSampleQueue timestampQueue : timestampQueues) {
+ offerSample(timestampQueue, timestamp);
}
} finally {
Drive.odometryLock.unlock();
@@ -169,4 +186,14 @@ public void run() {
}
}
}
+
+ private static LatestSampleQueue createQueue() {
+ return new LatestSampleQueue<>(QUEUE_CAPACITY);
+ }
+
+ private void offerSample(LatestSampleQueue queue, double sample) {
+ if (queue.offerLatest(sample)) {
+ droppedSamples++;
+ }
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java
index f4214b10..00e9b4a4 100644
--- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java
+++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java
@@ -11,14 +11,13 @@
import com.revrobotics.REVLibError;
import com.revrobotics.spark.SparkBase;
-import edu.wpi.first.wpilibj.Notifier;
-import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Queue;
-import java.util.concurrent.ArrayBlockingQueue;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.system.Notifier;
+import org.wpilib.system.RobotController;
/**
* Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
@@ -27,15 +26,17 @@
* all measurements in the sample are valid.
*/
public class SparkOdometryThread {
+ private static final int QUEUE_CAPACITY = 128;
+
private final List sparks = new ArrayList<>();
private final List sparkSignals = new ArrayList<>();
private final List genericSignals = new ArrayList<>();
- private final List> sparkQueues = new ArrayList<>();
- private final List> genericQueues = new ArrayList<>();
- private final List> timestampQueues = new ArrayList<>();
+ private final List> sparkQueues = new ArrayList<>();
+ private final List> genericQueues = new ArrayList<>();
+ private final List> timestampQueues = new ArrayList<>();
private static SparkOdometryThread instance = null;
- private Notifier notifier = new Notifier(this::run);
+ private final Notifier notifier = new Notifier(this::run);
private long droppedSamples = 0;
private long loopCount = 0;
@@ -51,14 +52,14 @@ private SparkOdometryThread() {
}
public void start() {
- if (timestampQueues.size() > 0) {
+ if (!timestampQueues.isEmpty()) {
notifier.startPeriodic(1.0 / SwerveConstants.kOdometryFrequency);
}
}
/** Registers a Spark signal to be read from the thread. */
public Queue registerSignal(SparkBase spark, DoubleSupplier signal) {
- Queue queue = new ArrayBlockingQueue<>(128); // was 20
+ LatestSampleQueue queue = createQueue();
Drive.odometryLock.lock();
try {
sparks.add(spark);
@@ -72,7 +73,7 @@ public Queue registerSignal(SparkBase spark, DoubleSupplier signal) {
/** Registers a generic signal to be read from the thread. */
public Queue registerSignal(DoubleSupplier signal) {
- Queue queue = new ArrayBlockingQueue<>(128); // was 20
+ LatestSampleQueue queue = createQueue();
Drive.odometryLock.lock();
try {
genericSignals.add(signal);
@@ -85,7 +86,7 @@ public Queue registerSignal(DoubleSupplier signal) {
/** Returns a new queue that returns timestamp values for each sample. */
public Queue makeTimestampQueue() {
- Queue queue = new ArrayBlockingQueue<>(128); // was 20
+ LatestSampleQueue queue = createQueue();
Drive.odometryLock.lock();
try {
timestampQueues.add(queue);
@@ -100,7 +101,7 @@ private void run() {
Drive.odometryLock.lock();
try {
// Get sample timestamp
- double timestamp = RobotController.getFPGATime() / 1e6;
+ double timestamp = RobotController.getTime() / 1e6;
// Read Spark values, mark invalid in case of error
double[] sparkValues = new double[sparkSignals.size()];
@@ -115,13 +116,13 @@ private void run() {
// If valid, add values to queues
if (isValid) {
for (int i = 0; i < sparkSignals.size(); i++) {
- if (!sparkQueues.get(i).offer(sparkValues[i])) droppedSamples++;
+ offerSample(sparkQueues.get(i), sparkValues[i]);
}
for (int i = 0; i < genericSignals.size(); i++) {
- if (!genericQueues.get(i).offer(genericSignals.get(i).getAsDouble())) droppedSamples++;
+ offerSample(genericQueues.get(i), genericSignals.get(i).getAsDouble());
}
- for (int i = 0; i < timestampQueues.size(); i++) {
- if (!timestampQueues.get(i).offer(timestamp)) droppedSamples++;
+ for (LatestSampleQueue timestampQueue : timestampQueues) {
+ offerSample(timestampQueue, timestamp);
}
}
} finally {
@@ -132,4 +133,14 @@ private void run() {
Logger.recordOutput("Drive/SparkOdomThread/DroppedSamples", droppedSamples);
}
}
+
+ private static LatestSampleQueue createQueue() {
+ return new LatestSampleQueue<>(QUEUE_CAPACITY);
+ }
+
+ private void offerSample(LatestSampleQueue queue, double sample) {
+ if (queue.offerLatest(sample)) {
+ droppedSamples++;
+ }
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
index 50da778b..9e301017 100644
--- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
@@ -14,15 +14,14 @@
package frc.robot.subsystems.drive;
import com.ctre.phoenix6.CANBus;
-import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
-import frc.robot.Constants.CANBuses;
+import frc.robot.Constants.DrivebaseConstants;
import frc.robot.generated.TunerFactory;
import frc.robot.subsystems.imu.ImuIO;
-import frc.robot.subsystems.imu.ImuIONavX;
import frc.robot.subsystems.imu.ImuIOPigeon2;
-import frc.robot.util.YagslConstants;
import java.util.function.Supplier;
+import java.util.stream.Stream;
+import org.wpilib.math.util.Units;
/**
* Holds the proper set of drive constants given the type of drive
@@ -37,7 +36,7 @@ public class SwerveConstants {
public static final double kCoupleRatio;
public static final double kDriveGearRatio;
public static final double kSteerGearRatio;
- public static final String kCANbusName;
+ public static final String kPigeonCanbus;
public static final int kPigeonId;
public static final double kSteerInertia;
public static final double kDriveInertia;
@@ -116,7 +115,7 @@ public class SwerveConstants {
kCoupleRatio = tuner.frontLeft().CouplingGearRatio;
kDriveGearRatio = tuner.frontLeft().DriveMotorGearRatio;
kSteerGearRatio = tuner.frontLeft().SteerMotorGearRatio;
- kCANbusName = CANBuses.DRIVE;
+ kPigeonCanbus = DrivebaseConstants.kPigeonCANBus;
kPigeonId = tuner.drivetrain().Pigeon2Id;
kSteerInertia = tuner.frontLeft().SteerInertia;
kDriveInertia = tuner.frontLeft().DriveInertia;
@@ -128,9 +127,9 @@ public class SwerveConstants {
kFLDriveMotorId = tuner.frontLeft().DriveMotorId;
kFLSteerMotorId = tuner.frontLeft().SteerMotorId;
kFLEncoderId = tuner.frontLeft().EncoderId;
- kFLDriveCanbus = kCANbusName;
- kFLSteerCanbus = kCANbusName;
- kFLEncoderCanbus = kCANbusName;
+ kFLDriveCanbus = DrivebaseConstants.kFLCANBus;
+ kFLSteerCanbus = DrivebaseConstants.kFLCANBus;
+ kFLEncoderCanbus = DrivebaseConstants.kFLCANBus;
kFLDriveType = "kraken";
kFLSteerType = "kraken";
kFLEncoderType = "cancoder";
@@ -144,9 +143,9 @@ public class SwerveConstants {
kFRDriveMotorId = tuner.frontRight().DriveMotorId;
kFRSteerMotorId = tuner.frontRight().SteerMotorId;
kFREncoderId = tuner.frontRight().EncoderId;
- kFRDriveCanbus = kCANbusName;
- kFRSteerCanbus = kCANbusName;
- kFREncoderCanbus = kCANbusName;
+ kFRDriveCanbus = DrivebaseConstants.kFRCANBus;
+ kFRSteerCanbus = DrivebaseConstants.kFRCANBus;
+ kFREncoderCanbus = DrivebaseConstants.kFRCANBus;
kFRDriveType = "kraken";
kFRSteerType = "kraken";
kFREncoderType = "cancoder";
@@ -160,9 +159,9 @@ public class SwerveConstants {
kBLDriveMotorId = tuner.backLeft().DriveMotorId;
kBLSteerMotorId = tuner.backLeft().SteerMotorId;
kBLEncoderId = tuner.backLeft().EncoderId;
- kBLDriveCanbus = kCANbusName;
- kBLSteerCanbus = kCANbusName;
- kBLEncoderCanbus = kCANbusName;
+ kBLDriveCanbus = DrivebaseConstants.kBLCANBus;
+ kBLSteerCanbus = DrivebaseConstants.kBLCANBus;
+ kBLEncoderCanbus = DrivebaseConstants.kBLCANBus;
kBLDriveType = "kraken";
kBLSteerType = "kraken";
kBLEncoderType = "cancoder";
@@ -176,9 +175,9 @@ public class SwerveConstants {
kBRDriveMotorId = tuner.backRight().DriveMotorId;
kBRSteerMotorId = tuner.backRight().SteerMotorId;
kBREncoderId = tuner.backRight().EncoderId;
- kBRDriveCanbus = kCANbusName;
- kBRSteerCanbus = kCANbusName;
- kBREncoderCanbus = kCANbusName;
+ kBRDriveCanbus = DrivebaseConstants.kBRCANBus;
+ kBRSteerCanbus = DrivebaseConstants.kBRCANBus;
+ kBREncoderCanbus = DrivebaseConstants.kBRCANBus;
kBRDriveType = "kraken";
kBRSteerType = "kraken";
kBREncoderType = "cancoder";
@@ -191,83 +190,11 @@ public class SwerveConstants {
break;
case YAGSL:
- kImuType = YagslConstants.swerveDriveJson.imu.type;
- kCoupleRatio = YagslConstants.kCoupleRatio;
- kDriveGearRatio = YagslConstants.kDriveGearRatio;
- kSteerGearRatio = YagslConstants.kSteerGearRatio;
- kCANbusName = YagslConstants.kCANbusName;
- kPigeonId = YagslConstants.kPigeonId;
- kSteerInertia = YagslConstants.kSteerInertia;
- kDriveInertia = YagslConstants.kDriveInertia;
- kSteerFrictionVoltage = YagslConstants.kSteerFrictionVoltage;
- kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage;
- kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit;
- kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit;
- // Front Left
- kFLDriveMotorId = YagslConstants.kFrontLeftDriveMotorId;
- kFLSteerMotorId = YagslConstants.kFrontLeftSteerMotorId;
- kFLEncoderId = YagslConstants.kFrontLeftEncoderId;
- kFLDriveCanbus = YagslConstants.kFrontLeftDriveCanbus;
- kFLSteerCanbus = YagslConstants.kFrontLeftSteerCanbus;
- kFLEncoderCanbus = YagslConstants.kFrontLeftEncoderCanbus;
- kFLDriveType = YagslConstants.kFrontLeftDriveType.toLowerCase();
- kFLSteerType = YagslConstants.kFrontLeftSteerType.toLowerCase();
- kFLEncoderType = YagslConstants.kFrontLeftEncoderType.toLowerCase();
- kFLEncoderOffset = Units.degreesToRadians(YagslConstants.kFrontLeftEncoderOffset);
- kFLDriveInvert = YagslConstants.kFrontLeftDriveInvert;
- kFLSteerInvert = YagslConstants.kFrontLeftSteerInvert;
- kFLEncoderInvert = YagslConstants.kFrontLeftEncoderInvert;
- kFLXPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftXPosInches);
- kFLYPosMeters = Units.inchesToMeters(YagslConstants.kFrontLeftYPosInches);
- // Front Right
- kFRDriveMotorId = YagslConstants.kFrontRightDriveMotorId;
- kFRSteerMotorId = YagslConstants.kFrontRightSteerMotorId;
- kFREncoderId = YagslConstants.kFrontRightEncoderId;
- kFRDriveCanbus = YagslConstants.kFrontRightDriveCanbus;
- kFRSteerCanbus = YagslConstants.kFrontRightSteerCanbus;
- kFREncoderCanbus = YagslConstants.kFrontRightEncoderCanbus;
- kFRDriveType = YagslConstants.kFrontRightDriveType.toLowerCase();
- kFRSteerType = YagslConstants.kFrontRightSteerType.toLowerCase();
- kFREncoderType = YagslConstants.kFrontRightEncoderType.toLowerCase();
- kFREncoderOffset = Units.degreesToRadians(YagslConstants.kFrontRightEncoderOffset);
- kFRDriveInvert = YagslConstants.kFrontRightDriveInvert;
- kFRSteerInvert = YagslConstants.kFrontRightSteerInvert;
- kFREncoderInvert = YagslConstants.kFrontRightEncoderInvert;
- kFRXPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightXPosInches);
- kFRYPosMeters = Units.inchesToMeters(YagslConstants.kFrontRightYPosInches);
- // Back Left
- kBLDriveMotorId = YagslConstants.kBackLeftDriveMotorId;
- kBLSteerMotorId = YagslConstants.kBackLeftSteerMotorId;
- kBLEncoderId = YagslConstants.kBackLeftEncoderId;
- kBLDriveCanbus = YagslConstants.kBackLeftDriveCanbus;
- kBLSteerCanbus = YagslConstants.kBackLeftSteerCanbus;
- kBLEncoderCanbus = YagslConstants.kBackLeftEncoderCanbus;
- kBLDriveType = YagslConstants.kBackLeftDriveType.toLowerCase();
- kBLSteerType = YagslConstants.kBackLeftSteerType.toLowerCase();
- kBLEncoderType = YagslConstants.kBackLeftEncoderType.toLowerCase();
- kBLEncoderOffset = Units.degreesToRadians(YagslConstants.kBackLeftEncoderOffset);
- kBLDriveInvert = YagslConstants.kBackLeftDriveInvert;
- kBLSteerInvert = YagslConstants.kBackLeftSteerInvert;
- kBLEncoderInvert = YagslConstants.kBackLeftEncoderInvert;
- kBLXPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftXPosInches);
- kBLYPosMeters = Units.inchesToMeters(YagslConstants.kBackLeftYPosInches);
- // Back Right
- kBRDriveMotorId = YagslConstants.kBackRightDriveMotorId;
- kBRSteerMotorId = YagslConstants.kBackRightSteerMotorId;
- kBREncoderId = YagslConstants.kBackRightEncoderId;
- kBRDriveCanbus = YagslConstants.kBackRightDriveCanbus;
- kBRSteerCanbus = YagslConstants.kBackRightSteerCanbus;
- kBREncoderCanbus = YagslConstants.kBackRightEncoderCanbus;
- kBRDriveType = YagslConstants.kBackRightDriveType.toLowerCase();
- kBRSteerType = YagslConstants.kBackRightSteerType.toLowerCase();
- kBREncoderType = YagslConstants.kBackRightEncoderType.toLowerCase();
- kBREncoderOffset = Units.degreesToRadians(YagslConstants.kBackRightEncoderOffset);
- kBRDriveInvert = YagslConstants.kBackRightDriveInvert;
- kBRSteerInvert = YagslConstants.kBackRightSteerInvert;
- kBREncoderInvert = YagslConstants.kBackRightEncoderInvert;
- kBRXPosMeters = Units.inchesToMeters(YagslConstants.kBackRightXPosInches);
- kBRYPosMeters = Units.inchesToMeters(YagslConstants.kBackRightYPosInches);
- break;
+ // TODO(2027): Re-enable the YagslConstants-backed mapping when YAGSL and its
+ // required vendordeps publish 2027/SystemCore-compatible releases. Restore
+ // the previous assignments for IMU type, gear ratios, CAN bus, module IDs,
+ // encoder offsets, inversion flags, and module positions.
+ throw new UnsupportedOperationException("YAGSL is disabled pending 2027-compatible deps");
default:
throw new RuntimeException("Invalid Swerve Drive Type");
@@ -286,9 +213,28 @@ public class SwerveConstants {
Math.hypot(kBLXPosMeters, kBLYPosMeters), Math.hypot(kBRXPosMeters, kBRYPosMeters)));
public static final double kDriveBaseRadiusInches = Units.metersToInches(kDriveBaseRadiusMeters);
- // Are we on the CANivore or not?
+ /** True when Phoenix signals must be refreshed separately for multiple networks. */
+ public static final boolean kDrivebaseHasMultipleCANBuses =
+ configuredCANBuses().distinct().count() > 1;
+
+ /** The slowest configured bus determines the common odometry sampling rate. */
public static final double kOdometryFrequency =
- new CANBus(kCANbusName).isNetworkFD() ? 250.0 : 100.0;
+ configuredCANBuses().allMatch(bus -> new CANBus(bus).isNetworkFD()) ? 250.0 : 100.0;
+
+ private static Stream configuredCANBuses() {
+ return Stream.of(kPigeonCanbus, kFLDriveCanbus, kFRDriveCanbus, kBLDriveCanbus, kBRDriveCanbus);
+ }
+
+ /** Returns the single bus shared by every device in a module. */
+ public static String moduleCANBus(int module) {
+ return switch (module) {
+ case 0 -> kFLDriveCanbus;
+ case 1 -> kFRDriveCanbus;
+ case 2 -> kBLDriveCanbus;
+ case 3 -> kBRDriveCanbus;
+ default -> throw new IllegalArgumentException("Invalid module index: " + module);
+ };
+ }
// SPARK Drive encoder configuration
// Rotor Rotations -> Wheel Radians
@@ -308,7 +254,14 @@ public class SwerveConstants {
/** IMU Type enum */
public enum ImuType {
PIGEON(new String[] {"pigeon", "pigeon2"}, ImuIOPigeon2::new),
- NAVX(new String[] {"navx", "navx_spi"}, ImuIONavX::new);
+ // TODO(2027): Re-enable ImuIONavX::new when Studica/NavX supports 2027 WPILib.
+ // NAVX(new String[] {"navx", "navx_spi"}, ImuIONavX::new);
+ NAVX(
+ new String[] {"navx", "navx_spi"},
+ () -> {
+ throw new UnsupportedOperationException(
+ "NavX is disabled pending 2027-compatible Studica/NavX deps");
+ });
private final String[] keys;
public final Supplier factory;
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
index 35d446b2..7c3e99ed 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
@@ -9,20 +9,20 @@
package frc.robot.subsystems.flywheel_example;
-import static edu.wpi.first.units.Units.Second;
-import static edu.wpi.first.units.Units.Seconds;
-import static edu.wpi.first.units.Units.Volts;
import static frc.robot.Constants.FlywheelConstants.*;
+import static org.wpilib.units.Units.Second;
+import static org.wpilib.units.Units.Seconds;
+import static org.wpilib.units.Units.Volts;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
+import frc.robot.util.MathUtil;
import frc.robot.util.RBSISubsystem;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.command2.Command;
+import org.wpilib.command2.sysid.SysIdRoutine;
+import org.wpilib.math.util.Units;
+import org.wpilib.system.RobotController;
public class Flywheel extends RBSISubsystem {
private final FlywheelIO io;
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
index 34ce798d..76ebf360 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
@@ -9,25 +9,25 @@
package frc.robot.subsystems.flywheel_example;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.system.LinearSystem;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import frc.robot.Constants;
import frc.robot.Constants.FlywheelConstants;
+import frc.robot.util.MathUtil;
+import org.wpilib.math.controller.PIDController;
+import org.wpilib.math.controller.SimpleMotorFeedforward;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.system.DCMotor;
+import org.wpilib.math.system.LinearSystem;
+import org.wpilib.math.system.Models;
+import org.wpilib.simulation.FlywheelSim;
+import org.wpilib.system.RobotController;
public class FlywheelIOSim implements FlywheelIO {
private final DCMotor m_gearbox = DCMotor.getNEO(1);
private final LinearSystem m_plant =
- LinearSystemId.createFlywheelSystem(
+ Models.flywheelFromPhysicalConstants(
m_gearbox,
- FlywheelConstants.kSimGearing,
- FlywheelConstants.kSimMomentOfInertiaKgMetersSq);
+ FlywheelConstants.kSimMomentOfInertiaKgMetersSq,
+ FlywheelConstants.kSimGearing);
private final FlywheelSim sim = new FlywheelSim(m_plant, m_gearbox);
private PIDController pid = new PIDController(0.0, 0.0, 0.0);
@@ -42,7 +42,7 @@ public void updateInputs(FlywheelIOInputs inputs) {
if (closedLoop) {
appliedVolts =
MathUtil.clamp(
- pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts,
+ pid.calculate(sim.getAngularVelocity()) + ffVolts,
-FlywheelConstants.kMaxVoltage,
FlywheelConstants.kMaxVoltage);
sim.setInputVoltage(appliedVolts);
@@ -51,9 +51,9 @@ public void updateInputs(FlywheelIOInputs inputs) {
sim.update(Constants.kLoopPeriodSecs);
inputs.positionRad = 0.0;
- inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec();
+ inputs.velocityRadPerSec = sim.getAngularVelocity();
inputs.appliedVolts = appliedVolts;
- inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()};
+ inputs.currentAmps = new double[] {sim.getCurrentDraw()};
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java
index e847f8cf..c5311373 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java
@@ -23,20 +23,20 @@
import com.revrobotics.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.FeedbackSensor;
-import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.Constants.DrivebaseConstants;
import frc.robot.subsystems.drive.SwerveConstants;
import frc.robot.util.SparkUtil;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.controller.SimpleMotorFeedforward;
+import org.wpilib.math.util.Units;
/**
* NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with
@@ -46,9 +46,9 @@ public class FlywheelIOSpark implements FlywheelIO {
// Define the leader / follower motors from the RobotDevices section of RobotContainer
private final SparkMax leader =
- new SparkMax(FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless);
+ new SparkMax(0, FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless);
private final SparkMax follower =
- new SparkMax(FLYWHEEL_FOLLOWER.getDeviceNumber(), MotorType.kBrushless);
+ new SparkMax(0, FLYWHEEL_FOLLOWER.getDeviceNumber(), MotorType.kBrushless);
private final RelativeEncoder encoder = leader.getEncoder();
private final SparkClosedLoopController pid = leader.getClosedLoopController();
// IMPORTANT: Include here all devices listed above that are part of this mechanism!
@@ -116,11 +116,12 @@ public FlywheelIOSpark() {
@Override
public void updateInputs(FlywheelIOInputs inputs) {
- inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / kGearRatio);
+ inputs.positionRad = Units.rotationsToRadians(encoder.getPosition().get(0.0) / kGearRatio);
inputs.velocityRadPerSec =
- Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / kGearRatio);
- inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage();
- inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()};
+ Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity().get(0.0) / kGearRatio);
+ inputs.appliedVolts = leader.getAppliedOutput().get(0.0) * leader.getBusVoltage().get(0.0);
+ inputs.currentAmps =
+ new double[] {leader.getOutputCurrent().get(0.0), follower.getOutputCurrent().get(0.0)};
// AdvantageKit logging
Logger.recordOutput("Flywheel/PositionRad", inputs.positionRad);
@@ -137,7 +138,7 @@ public void setVoltage(double volts) {
@Override
public void setPercent(double percent) {
- leader.set(percent);
+ leader.setThrottle(percent);
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
index a9bb0f91..0b1cf343 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
@@ -25,15 +25,15 @@
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.units.measure.Current;
-import edu.wpi.first.units.measure.Voltage;
import frc.robot.Constants;
import frc.robot.Constants.PowerConstants;
import frc.robot.util.PhoenixUtil;
import frc.robot.util.RBSIEnum.CTREPro;
+import org.wpilib.math.util.Units;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.units.measure.AngularVelocity;
+import org.wpilib.units.measure.Current;
+import org.wpilib.units.measure.Voltage;
public class FlywheelIOTalonFX implements FlywheelIO {
diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java
index c163a2e0..8927ff0a 100644
--- a/src/main/java/frc/robot/subsystems/imu/Imu.java
+++ b/src/main/java/frc/robot/subsystems/imu/Imu.java
@@ -17,9 +17,9 @@
package frc.robot.subsystems.imu;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation3d;
import frc.robot.util.VirtualSubsystem;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation3d;
public class Imu extends VirtualSubsystem {
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java
index 1b2bea5f..8f71b2bc 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java
@@ -17,9 +17,9 @@
package frc.robot.subsystems.imu;
-import edu.wpi.first.math.geometry.Translation3d;
import frc.robot.util.RBSIIO;
import org.littletonrobotics.junction.AutoLog;
+import org.wpilib.math.geometry.Translation3d;
/**
* Single IMU interface exposing all relevant state: orientation, rates, linear accel, jerk, and
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java
index 08a44b1e..267a4910 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java
@@ -17,175 +17,7 @@
package frc.robot.subsystems.imu;
-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;
-import java.util.Queue;
-
-/** 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 {
-
- // Define the NavX Hardware
- private final AHRS navx;
-
- // Queues
- private final Queue yawPositionDegQueue;
- private final Queue yawTimestampQueue;
-
- // Previous accel for jerk calculation (m/s/s)
- private Translation3d prevAcc = Translation3d.kZero;
- private long prevTimestampNs = 0L;
-
- // 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);
-
- // Alliance-based adjustment (your original behavior)
- if (DriverStation.getAlliance().isPresent()
- && DriverStation.getAlliance().get() == Alliance.Red) {
- navx.setAngleAdjustment(180.0);
- } else {
- navx.setAngleAdjustment(0.0);
- }
- navx.reset();
-
- yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
- 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:
- // yawPosition = Rotation2d.fromDegrees(-navx.getAngle());
- // yawRate = -navx.getRawGyroZ()
- //
- // NavX:
- // - getAngle() is degrees (continuous)
- // - getRawGyroZ() is deg/sec
- inputs.yawPositionRad = Units.degreesToRadians(-navx.getAngle());
- inputs.yawRateRadPerSec = Units.degreesToRadians(-navx.getRawGyroZ());
-
- // World linear accel (NavX returns "g" typically); convert to m/s/s
- inputs.linearAccel =
- new Translation3d(
- navx.getWorldLinearAccelX() * Constants.kGravityMetersPerSecSq,
- navx.getWorldLinearAccelY() * Constants.kGravityMetersPerSecSq,
- navx.getWorldLinearAccelZ() * Constants.kGravityMetersPerSecSq);
-
- // Jerk computed as (delta accel) / dt
- if (prevTimestampNs != 0L) {
- final double dt = (start - prevTimestampNs) * 1e-9;
- if (dt > 1e-6) {
- inputs.linearJerk = inputs.linearAccel.minus(prevAcc).div(dt);
- }
- }
-
- // Load "previous values" for the next loop
- prevTimestampNs = start;
- prevAcc = inputs.linearAccel;
-
- // 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);
- System.arraycopy(odomYawRadBuf, 0, yawOut, 0, n);
- inputs.odometryYawTimestamps = tsOut;
- inputs.odometryYawPositionsRad = yawOut;
- } else {
- // ...otherwise return empty arrays
- inputs.odometryYawTimestamps = EMPTY_DOUBLE_ARRAY;
- inputs.odometryYawPositionsRad = EMPTY_DOUBLE_ARRAY;
- }
-
- // Compute how long this took in seconds
- final long end = System.nanoTime();
- inputs.latencySeconds = (end - start) * 1e-9;
- }
-
- /**
- * Zero the YAW to this radian value
- *
- * @param yawRad The radian value to which to zero
- */
- @Override
- public void zeroYawRad(double yawRad) {
- navx.setAngleAdjustment(Units.radiansToDegrees(yawRad));
- navx.zeroYaw();
-
- // Reset jerk history so you don't spike on the next frame
- prevTimestampNs = 0L;
- 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();
- final int n = Math.min(nTs, nYaw);
- if (n <= 0) {
- yawTimestampQueue.clear();
- yawPositionDegQueue.clear();
- return 0;
- }
-
- ensureOdomCapacity(n);
-
- final Iterator itT = yawTimestampQueue.iterator();
- final Iterator itY = yawPositionDegQueue.iterator();
-
- int i = 0;
- while (i < n && itT.hasNext() && itY.hasNext()) {
- odomTsBuf[i] = itT.next();
-
- // queue provides degrees (navx::getYaw). Apply your sign convention (-d) then rad.
- final double yawDeg = -itY.next();
- odomYawRadBuf[i] = Units.degreesToRadians(yawDeg);
-
- i++;
- }
-
- yawTimestampQueue.clear();
- yawPositionDegQueue.clear();
- 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;
- while (newCap < n) newCap *= 2;
- odomTsBuf = new double[newCap];
- odomYawRadBuf = new double[newCap];
- }
-}
+/**
+ * TODO(2027): Restore NavX/Studica IMU IO when Studica/NavX publishes a 2027-compatible vendordep.
+ */
+public class ImuIONavX implements ImuIO {}
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java
index 7b203716..2fde3274 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java
@@ -22,17 +22,17 @@
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;
-import edu.wpi.first.math.geometry.Translation3d;
-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.LinearAcceleration;
import frc.robot.Constants;
import frc.robot.subsystems.drive.PhoenixOdometryThread;
import frc.robot.subsystems.drive.SwerveConstants;
import frc.robot.util.RBSICANBusRegistry;
import java.util.Iterator;
import java.util.Queue;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
+import org.wpilib.units.measure.Angle;
+import org.wpilib.units.measure.AngularVelocity;
+import org.wpilib.units.measure.LinearAcceleration;
/** IMU IO for CTRE Pigeon2 */
public class ImuIOPigeon2 implements ImuIO {
@@ -40,7 +40,7 @@ public class ImuIOPigeon2 implements ImuIO {
// Define the Pigeon2 Hardware
private final Pigeon2 pigeon =
new Pigeon2(
- SwerveConstants.kPigeonId, RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName));
+ SwerveConstants.kPigeonId, RBSICANBusRegistry.getBus(SwerveConstants.kPigeonCanbus));
// Cached signals
private final StatusSignal yawSignal = pigeon.getYaw();
@@ -76,7 +76,9 @@ public ImuIOPigeon2() {
pigeon.optimizeBusUtilization();
odomTimestamps = PhoenixOdometryThread.getInstance().makeTimestampQueue();
- odomYawsDeg = PhoenixOdometryThread.getInstance().registerSignal(yawSignal);
+ odomYawsDeg =
+ PhoenixOdometryThread.getInstance()
+ .registerSignal(SwerveConstants.kPigeonCanbus, yawSignal);
}
/** Update the Inputs */
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java
index 87321a75..aaa39596 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java
@@ -17,10 +17,10 @@
package frc.robot.subsystems.imu;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
import frc.robot.util.TimeUtil;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
/** Simulated IMU for full robot simulation & replay logging */
public class ImuIOSim implements ImuIO {
diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java
index 18f56795..689417f0 100644
--- a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java
+++ b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java
@@ -17,12 +17,6 @@
package frc.robot.subsystems.vision;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Quaternion;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import java.io.FileWriter;
import java.io.IOException;
@@ -34,6 +28,12 @@
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.PhotonPipelineResult;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Quaternion;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
/** Sweeps two simulated cameras in a grid of poses and writes a CSV of performance. */
public class CameraSweepEvaluator {
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index 4d2b4ba7..bb0aa706 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -19,15 +19,6 @@
import static frc.robot.Constants.VisionConstants.*;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.VecBuilder;
-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.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;
@@ -44,6 +35,15 @@
import java.util.Set;
import java.util.concurrent.atomic.AtomicReference;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
public class Vision extends VirtualSubsystem {
@@ -71,8 +71,8 @@ public interface PoseMeasurementConsumer {
// Smoothing buffer (recent fused estimates)
private final ArrayDeque fusedBuffer = new ArrayDeque<>();
- private final double smoothWindowSec = 0.25;
- private final int smoothMaxSize = 12;
+ private static final double SMOOTH_WINDOW_SECONDS = 0.25;
+ private static final int SMOOTH_MAX_SIZE = 12;
// Trusted tags configuration (swappable per event/field)
private final AtomicReference> trustedTags = new AtomicReference<>(Set.of());
@@ -207,9 +207,9 @@ public void rbsiPeriodic() {
GateResult gate = passesScrutiny(cam, obs);
if (tuningMode) {
- Logger.recordOutput("Vision/Camera" + cam + "/GateFail", gate.reason);
+ Logger.recordOutput("Vision/Camera" + cam + "/GateFail", gate.reason());
}
- if (!gate.accepted) {
+ if (!gate.accepted()) {
rejected++;
continue;
}
@@ -221,10 +221,10 @@ public void rbsiPeriodic() {
}
// 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;
+ if (best == null || isBetter(built.estimate(), best)) {
+ best = built.estimate();
+ bestTrustScale = built.trustScale();
+ bestTrustedCount = built.trustedCount();
bestTagCount = obs.tagCount();
}
}
@@ -264,12 +264,9 @@ public void rbsiPeriodic() {
// =====
// Fuse all accepted cams at the newest timestamp among them
- double tFusion = Double.NaN;
- for (int i = 0; i < perCamAccepted.size(); i++) {
- final double timestamp = perCamAccepted.get(i).timestampSeconds();
- if (!Double.isFinite(tFusion) || timestamp > tFusion) {
- tFusion = timestamp;
- }
+ double tFusion = Double.NEGATIVE_INFINITY;
+ for (TimedPose estimate : perCamAccepted) {
+ tFusion = Math.max(tFusion, estimate.timestampSeconds());
}
if (!Double.isFinite(tFusion)) return;
@@ -393,15 +390,7 @@ public void setSingleTagYawGate(boolean enabled, double lookbackSec, double limi
/** Gating + Scoring ***************************************************** */
/** GateResult Class */
- private static final class GateResult {
- final boolean accepted;
- final String reason;
-
- GateResult(boolean accepted, String reason) {
- this.accepted = accepted;
- this.reason = reason;
- }
- }
+ private record GateResult(boolean accepted, String reason) {}
/**
* Gating Function -- checks all sorts of things!
@@ -447,20 +436,10 @@ private GateResult passesScrutiny(int cam, VisionIO.PoseObservation obs) {
}
/** Built Estimate Class */
- private static final class BuiltEstimate {
- final TimedPose estimate;
- final double trustScale;
- final int trustedCount;
-
- BuiltEstimate(TimedPose estimate, double trustScale, int trustedCount) {
- this.estimate = estimate;
- this.trustScale = trustScale;
- this.trustedCount = trustedCount;
- }
- }
+ private record BuiltEstimate(TimedPose estimate, double trustScale, int trustedCount) {}
/**
- * Build a pose esitmate
+ * Build a pose estimate.
*
* @param cam Camera Index
* @param obs PoseObservation
@@ -484,12 +463,12 @@ 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];
+ final Set trusted = trustedTags.get();
+ final int[] usedIds = (obs.usedTagIds() != null) ? obs.usedTagIds() : kEmptyTagIdArray;
int trustedCount = 0;
- for (int i = 0; i < usedIds.length; i++) {
- if (kTrusted.contains(usedIds[i])) trustedCount++;
+ for (int usedId : usedIds) {
+ if (trusted.contains(usedId)) trustedCount++;
}
// If no trusted tags, return null
@@ -533,7 +512,7 @@ private BuiltEstimate buildEstimate(int cam, VisionIO.PoseObservation obs) {
* @param a Base pose
* @param b Competitor pose
*/
- private boolean isBetter(TimedPose a, TimedPose b) {
+ private static 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);
@@ -574,7 +553,7 @@ void set(double dt, Transform2d tf) {
/**
* Fuse poses at a specified timestamp
*
- * @param estimates Array of TimedPose esitmates
+ * @param estimates Array of timed pose estimates
* @param tFusion The timestamp at which to fuse the measurements
*/
private TimedPose fuseAtTime(ArrayList estimates, double tFusion) {
@@ -637,7 +616,7 @@ private Pose2d timeAlignPose(Pose2d visionPoseAtTs, double ts, double tFusion) {
*
* @param alignedAtTF List of aligned poses
* @param tFusion Fusion timestamp
- * @return Fuesed TimedPose object
+ * @return Fused timed pose object
*/
private TimedPose inverseVarianceFuse(ArrayList alignedAtTF, double tFusion) {
// If size of alignedAtTF is 0 or 1, return null or the only value
@@ -650,8 +629,7 @@ private TimedPose inverseVarianceFuse(ArrayList alignedAtTF, double t
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);
+ for (TimedPose e : alignedAtTF) {
final Pose2d p = e.pose();
final Matrix s = e.stdDevs();
@@ -723,14 +701,14 @@ private TimedPose inverseVarianceFuse(ArrayList alignedAtTF, double t
private void pushFused(TimedPose fused) {
fusedBuffer.addLast(fused);
- while (fusedBuffer.size() > smoothMaxSize) {
+ while (fusedBuffer.size() > SMOOTH_MAX_SIZE) {
fusedBuffer.removeFirst();
}
// Trim by time window relative to newest
while (!fusedBuffer.isEmpty()
&& fused.timestampSeconds() - fusedBuffer.peekFirst().timestampSeconds()
- > smoothWindowSec) {
+ > SMOOTH_WINDOW_SECONDS) {
fusedBuffer.removeFirst();
}
}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
index 4f11bf9f..0e0ec9fb 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
@@ -9,9 +9,9 @@
package frc.robot.subsystems.vision;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
import org.littletonrobotics.junction.AutoLog;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
public interface VisionIO {
@AutoLog
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
index dbbbe6f9..d6cbb23f 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
@@ -9,22 +9,22 @@
package frc.robot.subsystems.vision;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.DoubleArrayPublisher;
-import edu.wpi.first.networktables.DoubleArraySubscriber;
-import edu.wpi.first.networktables.DoubleSubscriber;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.TimestampedDoubleArray;
-import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.function.Supplier;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.util.Units;
+import org.wpilib.networktables.DoubleArrayPublisher;
+import org.wpilib.networktables.DoubleArraySubscriber;
+import org.wpilib.networktables.DoubleSubscriber;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.TimestampedDoubleArray;
+import org.wpilib.system.RobotController;
/** IO implementation for real Limelight hardware. */
public class VisionIOLimelight implements VisionIO {
@@ -69,7 +69,7 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) {
public void updateInputs(VisionIOInputs inputs) {
// Update connection status based on whether an update has been seen in the last 250ms
inputs.connected =
- ((RobotController.getFPGATime() - heartbeatSubscriber.getLastChange()) / 1000) < 250;
+ ((RobotController.getTime() - heartbeatSubscriber.getLastChange()) / 1000) < 250;
// Update target observation
inputs.latestTargetObservation =
@@ -85,7 +85,11 @@ public void updateInputs(VisionIOInputs inputs) {
Set unionTagIds = new HashSet<>();
List poseObservations = new ArrayList<>();
- for (var rawSample : megatag1Subscriber.readQueue()) {
+ var megatag1Samples = megatag1Subscriber.readQueue();
+ for (int sampleIndex = Math.max(0, megatag1Samples.length - 1);
+ sampleIndex < megatag1Samples.length;
+ sampleIndex++) {
+ var rawSample = megatag1Samples[sampleIndex];
if (!isValidBotPoseSample(rawSample.value)) continue;
int tagCount = getTagCount(rawSample.value);
@@ -118,7 +122,11 @@ public void updateInputs(VisionIOInputs inputs) {
used));
}
- for (var rawSample : megatag2Subscriber.readQueue()) {
+ var megatag2Samples = megatag2Subscriber.readQueue();
+ for (int sampleIndex = Math.max(0, megatag2Samples.length - 1);
+ sampleIndex < megatag2Samples.length;
+ sampleIndex++) {
+ var rawSample = megatag2Samples[sampleIndex];
if (!isValidBotPoseSample(rawSample.value)) continue;
int tagCount = getTagCount(rawSample.value);
@@ -210,7 +218,7 @@ private static int getTagCount(double[] rawLLArray) {
}
private static void flushOrientationIfDue() {
- long nowUs = RobotController.getFPGATime();
+ long nowUs = RobotController.getTime();
if (nowUs - lastOrientationFlushUs < ORIENTATION_FLUSH_PERIOD_US) return;
NetworkTableInstance.getDefault().flush();
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
index 0763baf7..59238139 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
@@ -11,15 +11,15 @@
import static frc.robot.FieldConstants.*;
-import edu.wpi.first.math.geometry.Pose3d;
-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.List;
import java.util.Set;
import org.photonvision.PhotonCamera;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform3d;
/** IO implementation for real PhotonVision hardware. */
public class VisionIOPhotonVision implements VisionIO {
@@ -41,11 +41,8 @@ public VisionIOPhotonVision(String name, Transform3d robotToCamera) {
public void updateInputs(VisionIOInputs inputs) {
inputs.connected = camera.isConnected();
- // Cap the number of unread results processed per loop
- final int kMaxUnread = 5;
-
final Set unionTagIds = new HashSet<>();
- final ArrayList poseObservations = new ArrayList<>(kMaxUnread);
+ final ArrayList poseObservations = new ArrayList<>(1);
double newestTargetTs = Double.NEGATIVE_INFINITY;
Rotation2d bestYaw = Rotation2d.kZero;
@@ -53,9 +50,11 @@ public void updateInputs(VisionIOInputs inputs) {
List unreadResults =
camera.getAllUnreadResults();
- int startIndex = Math.max(0, unreadResults.size() - kMaxUnread);
-
- for (int resultIndex = startIndex; resultIndex < unreadResults.size(); resultIndex++) {
+ // PhotonVision returns unread changes in FIFO order. Drain the FIFO, but retain only the newest
+ // result so a slow robot loop cannot create a camera-processing backlog.
+ for (int resultIndex = Math.max(0, unreadResults.size() - 1);
+ resultIndex < unreadResults.size();
+ resultIndex++) {
var result = unreadResults.get(resultIndex);
final double ts = result.getTimestampSeconds();
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java
index f26e7024..575b33df 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java
@@ -9,14 +9,14 @@
package frc.robot.subsystems.vision;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.wpilibj.RobotController;
import frc.robot.FieldConstants;
import java.util.function.Supplier;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.system.RobotController;
/** IO implementation for physics sim using PhotonVision simulator. */
public class VisionIOPhotonVisionSim extends VisionIOPhotonVision {
@@ -73,7 +73,7 @@ public void updateInputs(VisionIOInputs inputs) {
}
private void updateVisionSimOncePerLoop() {
- long nowUs = RobotController.getFPGATime();
+ long nowUs = RobotController.getTime();
if (nowUs - lastUpdateUs < MIN_UPDATE_PERIOD_US) return;
visionSim.update(poseSupplier.get());
diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java
index 3f64e100..6af6199b 100644
--- a/src/main/java/frc/robot/util/Alert.java
+++ b/src/main/java/frc/robot/util/Alert.java
@@ -9,16 +9,15 @@
package frc.robot.util;
-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.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
-import java.util.function.Predicate;
+import org.wpilib.driverstation.DriverStationErrors;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.util.sendable.Sendable;
+import org.wpilib.util.sendable.SendableBuilder;
/** Class for managing persistent alerts to be sent over NetworkTables. */
public class Alert {
@@ -49,14 +48,18 @@ public Alert(String text, AlertType type) {
* @param type Alert level specifying urgency.
*/
public Alert(String group, String text, AlertType type) {
- if (!groups.containsKey(group)) {
- groups.put(group, new SendableAlerts());
- SmartDashboard.putData(group, groups.get(group));
- }
+ SendableAlerts alertGroup =
+ groups.computeIfAbsent(
+ group,
+ name -> {
+ SendableAlerts createdGroup = new SendableAlerts();
+ SmartDashboard.putData(name, createdGroup);
+ return createdGroup;
+ });
this.text = text;
this.type = type;
- groups.get(group).alerts.add(this);
+ alertGroup.alerts.add(this);
}
/**
@@ -66,17 +69,7 @@ public Alert(String group, String text, AlertType type) {
public void set(boolean active) {
if (active && !this.active) {
activeStartTime = TimeUtil.now();
- switch (type) {
- case ERROR:
- DriverStation.reportError(text, false);
- break;
- case WARNING:
- DriverStation.reportWarning(text, false);
- break;
- case INFO:
- System.out.println(text);
- break;
- }
+ reportToConsole();
}
this.active = active;
}
@@ -84,32 +77,29 @@ public void set(boolean active) {
/** Updates current alert text. */
public void setText(String text) {
if (active && !text.equals(this.text)) {
- switch (type) {
- case ERROR:
- DriverStation.reportError(text, false);
- break;
- case WARNING:
- DriverStation.reportWarning(text, false);
- break;
- case INFO:
- System.out.println(text);
- break;
- }
+ this.text = text;
+ reportToConsole();
+ return;
}
this.text = text;
}
+ private void reportToConsole() {
+ switch (type) {
+ case ERROR -> DriverStationErrors.reportError(text, false);
+ case WARNING -> DriverStationErrors.reportWarning(text, false);
+ case INFO -> System.out.println(text);
+ }
+ }
+
private static class SendableAlerts implements Sendable {
public final List alerts = new ArrayList<>();
public String[] getStrings(AlertType type) {
- Predicate activeFilter = (Alert x) -> x.type == type && x.active;
- Comparator timeSorter =
- (Alert a1, Alert a2) -> Double.compare(a2.activeStartTime, a1.activeStartTime);
return alerts.stream()
- .filter(activeFilter)
- .sorted(timeSorter)
- .map((Alert a) -> a.text)
+ .filter(alert -> alert.type == type && alert.active)
+ .sorted(Comparator.comparingDouble((Alert alert) -> alert.activeStartTime).reversed())
+ .map(alert -> alert.text)
.toArray(String[]::new);
}
diff --git a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java
index aa9a091c..e50e3e54 100644
--- a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java
+++ b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java
@@ -19,9 +19,6 @@
package frc.robot.util;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.math.interpolation.Interpolator;
import java.util.Collections;
import java.util.Map.Entry;
import java.util.NavigableMap;
@@ -29,6 +26,8 @@
import java.util.OptionalDouble;
import java.util.concurrent.ConcurrentNavigableMap;
import java.util.concurrent.ConcurrentSkipListMap;
+import org.wpilib.math.interpolation.Interpolatable;
+import org.wpilib.math.interpolation.Interpolator;
/**
* A concurrent version of WPIlib's TimeInterpolatableBuffer class to avoid the need for explicit
@@ -116,22 +115,21 @@ public void clear() {
* @return The interpolated value at that timestamp or an empty Optional.
*/
public Optional getSample(double timeSeconds) {
- if (m_pastSnapshots.isEmpty()) {
- return Optional.empty();
- }
-
// Special case for when the requested time is the same as a sample
- var nowEntry = m_pastSnapshots.get(timeSeconds);
- if (nowEntry != null) {
- return Optional.of(nowEntry);
+ T exactSample = m_pastSnapshots.get(timeSeconds);
+ if (exactSample != null) {
+ return Optional.of(exactSample);
}
var bottomBound = m_pastSnapshots.floorEntry(timeSeconds);
var topBound = m_pastSnapshots.ceilingEntry(timeSeconds);
- 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 (bottomBound == null) {
+ return topBound == null ? Optional.empty() : Optional.of(topBound.getValue());
+ }
+ if (topBound == null) {
+ return Optional.of(bottomBound.getValue());
+ }
// If they are the same sample, no interpolation possible/needed
if (topBound.getKey().doubleValue() == bottomBound.getKey().doubleValue()) {
@@ -140,12 +138,12 @@ public Optional getSample(double timeSeconds) {
double t0 = bottomBound.getKey();
double t1 = topBound.getKey();
- double denom = t1 - t0;
+ double timeSpan = 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());
+ if (Math.abs(timeSpan) < 1e-9) return Optional.of(bottomBound.getValue());
- double ratio = (timeSeconds - t0) / denom;
+ double ratio = (timeSeconds - t0) / timeSpan;
ratio = MathUtil.clamp(ratio, 0.0, 1.0);
return Optional.of(
diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java
index f77c8b2f..42c2e3c0 100644
--- a/src/main/java/frc/robot/util/GeomUtil.java
+++ b/src/main/java/frc/robot/util/GeomUtil.java
@@ -9,14 +9,14 @@
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;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform2d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.geometry.Twist2d;
+import org.wpilib.math.kinematics.ChassisVelocities;
/** Geometry utilities for working with translations, rotations, transforms, and poses. */
public class GeomUtil {
@@ -131,14 +131,13 @@ public static Pose3d toPose3d(Transform3d transform) {
}
/**
- * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain
+ * Converts a ChassisVelocities 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);
+ public static Twist2d toTwist2d(ChassisVelocities speeds) {
+ return new Twist2d(speeds.vx, speeds.vy, speeds.omega);
}
/**
diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java
index e4353ade..0aff129b 100644
--- a/src/main/java/frc/robot/util/LocalADStarAK.java
+++ b/src/main/java/frc/robot/util/LocalADStarAK.java
@@ -15,14 +15,14 @@
import com.pathplanner.lib.path.PathPoint;
import com.pathplanner.lib.pathfinding.LocalADStar;
import com.pathplanner.lib.pathfinding.Pathfinder;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.geometry.Translation2d;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.littletonrobotics.junction.LogTable;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.util.Pair;
// PathPlannerLib pathfinder compatible with AdvantageKit log replay
// NOTE: This file is available at
diff --git a/src/main/java/frc/robot/util/MathUtil.java b/src/main/java/frc/robot/util/MathUtil.java
new file mode 100644
index 00000000..6554e952
--- /dev/null
+++ b/src/main/java/frc/robot/util/MathUtil.java
@@ -0,0 +1,58 @@
+// 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.
+
+package frc.robot.util;
+
+/** Small compatibility subset of the WPILib 2026 MathUtil helpers. */
+public final class MathUtil {
+ private MathUtil() {}
+
+ public static double clamp(double value, double low, double high) {
+ return Math.max(low, Math.min(value, high));
+ }
+
+ public static double interpolate(double startValue, double endValue, double t) {
+ return startValue + (endValue - startValue) * clamp(t, 0.0, 1.0);
+ }
+
+ public static double applyDeadband(double value, double deadband) {
+ return applyDeadband(value, deadband, 1.0);
+ }
+
+ public static double applyDeadband(double value, double deadband, double maxMagnitude) {
+ if (Math.abs(value) <= deadband) {
+ return 0.0;
+ }
+
+ if (maxMagnitude / deadband > 1.0e12) {
+ return value > 0.0 ? value - deadband : value + deadband;
+ }
+
+ if (value > 0.0) {
+ return maxMagnitude * (value - deadband) / (maxMagnitude - deadband);
+ } else {
+ return maxMagnitude * (value + deadband) / (maxMagnitude - deadband);
+ }
+ }
+
+ public static double inputModulus(double input, double minimumInput, double maximumInput) {
+ double modulus = maximumInput - minimumInput;
+
+ int numMax = (int) ((input - minimumInput) / modulus);
+ input -= numMax * modulus;
+
+ int numMin = (int) ((input - maximumInput) / modulus);
+ input -= numMin * modulus;
+
+ return input;
+ }
+
+ public static double angleModulus(double angleRadians) {
+ return inputModulus(angleRadians, -Math.PI, Math.PI);
+ }
+}
diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java
index cc12fa91..9fa25537 100644
--- a/src/main/java/frc/robot/util/OverrideSwitches.java
+++ b/src/main/java/frc/robot/util/OverrideSwitches.java
@@ -18,9 +18,9 @@
package frc.robot.util;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
+import org.wpilib.command2.button.Trigger;
+import org.wpilib.driverstation.GenericHID;
+import org.wpilib.driverstation.internal.DriverStationBackend;
/** Interface for physical override switches on operator console. */
public class OverrideSwitches {
@@ -33,7 +33,7 @@ public OverrideSwitches(int port) {
/** Returns whether the controller is connected. */
public boolean isConnected() {
return consoleSwitches.isConnected()
- && !DriverStation.getJoystickIsXbox(consoleSwitches.getPort())
+ && !DriverStationBackend.getJoystickIsGamepad(consoleSwitches.getPort())
&& consoleSwitches.getName().equals("Generic USB Joystick");
}
diff --git a/src/main/java/frc/robot/util/RBSICANBusRegistry.java b/src/main/java/frc/robot/util/RBSICANBusRegistry.java
index 4619ec7f..d574cd9e 100644
--- a/src/main/java/frc/robot/util/RBSICANBusRegistry.java
+++ b/src/main/java/frc/robot/util/RBSICANBusRegistry.java
@@ -15,7 +15,6 @@
import com.ctre.phoenix6.CANBus;
import java.util.Map;
-import java.util.Set;
import java.util.concurrent.ConcurrentHashMap;
/** Centralized CAN bus singleton registry */
@@ -63,9 +62,7 @@ public static CANBus getBus(String name) {
if (sim) {
throw new IllegalStateException("No real CANBus in SIM. Use getLike() or skip CTRE devices.");
}
- CANBus bus = realBuses.get(name);
- if (bus == null) throwUnknown(name, realBuses.keySet());
- return bus;
+ return getRegisteredBus(name, realBuses);
}
/**
@@ -76,9 +73,7 @@ public static CANBus getBus(String name) {
*/
public static CANBusLike getLike(String name) {
checkInit();
- CANBusLike bus = likeBuses.get(name);
- if (bus == null) throwUnknown(name, likeBuses.keySet());
- return bus;
+ return getRegisteredBus(name, likeBuses);
}
/** Check that the Registry is initialized */
@@ -86,9 +81,13 @@ private static void checkInit() {
if (!initialized) throw new IllegalStateException("RBSICANBusRegistry not initialized.");
}
- /** 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);
+ private static T getRegisteredBus(String name, Map buses) {
+ T bus = buses.get(name);
+ if (bus == null) {
+ throw new IllegalArgumentException(
+ "Unknown CAN bus '" + name + "'. Known: " + buses.keySet());
+ }
+ return bus;
}
/** Nested types for Registry Function *********************************** */
diff --git a/src/main/java/frc/robot/util/RBSIController.java b/src/main/java/frc/robot/util/RBSIController.java
index 4090da63..505067d2 100644
--- a/src/main/java/frc/robot/util/RBSIController.java
+++ b/src/main/java/frc/robot/util/RBSIController.java
@@ -8,23 +8,24 @@
package frc.robot.util;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
-import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants.ControllerButtonConstants;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.command2.button.CommandNiDsPS4Controller;
+import org.wpilib.command2.button.CommandNiDsPS5Controller;
+import org.wpilib.command2.button.CommandNiDsXboxController;
+import org.wpilib.command2.button.Trigger;
+import org.wpilib.driverstation.internal.DriverStationBackend;
/**
* Controller-agnostic wrapper for the driver and operator controllers.
*
* The selected physical controller is detected once at robot startup. RobotContainer should bind
* to typed physical-position inputs from this class instead of binding directly to Xbox-, PS4-, or
- * PS5-specific button names. Teams can remap robot actions in {@link
- * frc.robot.Constants.ControllerButtonConstants} without editing this class.
+ * PS5-specific button names. Button trigger names describe where the control is located rather than
+ * the robot action bound to it.
*/
public abstract class RBSIController {
+ private static final double DEFAULT_TRIGGER_PRESSED_THRESHOLD = 0.5;
+
public enum Button {
SOUTH_FACE,
EAST_FACE,
@@ -63,7 +64,7 @@ private RBSIController(int port, String controllerType) {
/** Creates a controller wrapper for the HID currently connected at startup. */
public static RBSIController createDriverController(int port) {
- String name = DriverStation.getJoystickName(port);
+ String name = DriverStationBackend.getJoystickName(port);
RBSIController controller = createController(port, name);
Logger.recordOutput("Controller/Port" + port + "/Name", name);
@@ -72,7 +73,7 @@ public static RBSIController createDriverController(int port) {
}
private static RBSIController createController(int port, String name) {
- if (DriverStation.getJoystickIsXbox(port)) {
+ if (DriverStationBackend.getJoystickIsGamepad(port)) {
return new XboxControllerAdapter(port);
}
@@ -102,10 +103,80 @@ public String getControllerType() {
public abstract Trigger button(Button button);
+ /** Returns the trigger for the south face button (Xbox A / PlayStation Cross). */
+ public Trigger southFaceButton() {
+ return button(Button.SOUTH_FACE);
+ }
+
+ /** Returns the trigger for the east face button (Xbox B / PlayStation Circle). */
+ public Trigger eastFaceButton() {
+ return button(Button.EAST_FACE);
+ }
+
+ /** Returns the trigger for the west face button (Xbox X / PlayStation Square). */
+ public Trigger westFaceButton() {
+ return button(Button.WEST_FACE);
+ }
+
+ /** Returns the trigger for the north face button (Xbox Y / PlayStation Triangle). */
+ public Trigger northFaceButton() {
+ return button(Button.NORTH_FACE);
+ }
+
+ /** Returns the trigger for the left bumper (Xbox LB / PlayStation L1). */
+ public Trigger leftBumper() {
+ return button(Button.LEFT_BUMPER);
+ }
+
+ /** Returns the trigger for the right bumper (Xbox RB / PlayStation R1). */
+ public Trigger rightBumper() {
+ return button(Button.RIGHT_BUMPER);
+ }
+
+ /** Returns the trigger for the left stick being pressed. */
+ public Trigger leftStickButton() {
+ return button(Button.LEFT_STICK);
+ }
+
+ /** Returns the trigger for the right stick being pressed. */
+ public Trigger rightStickButton() {
+ return button(Button.RIGHT_STICK);
+ }
+
+ /** Returns the trigger for the POV/D-pad being pressed left. */
+ public Trigger povLeft() {
+ return button(Button.POV_LEFT);
+ }
+
+ /** Returns the trigger for the POV/D-pad being pressed right. */
+ public Trigger povRight() {
+ return button(Button.POV_RIGHT);
+ }
+
+ /** Returns the trigger for the POV/D-pad being pressed up. */
+ public Trigger povUp() {
+ return button(Button.POV_UP);
+ }
+
+ /** Returns the trigger for the POV/D-pad being pressed down. */
+ public Trigger povDown() {
+ return button(Button.POV_DOWN);
+ }
+
+ /** Returns the trigger for the left analog trigger being pressed. */
+ public Trigger leftTrigger() {
+ return axisTrigger(Axis.LEFT_TRIGGER);
+ }
+
+ /** Returns the trigger for the right analog trigger being pressed. */
+ public Trigger rightTrigger() {
+ return axisTrigger(Axis.RIGHT_TRIGGER);
+ }
+
public abstract double axis(Axis axis);
public Trigger axisTrigger(Axis axis) {
- return axisTrigger(axis, ControllerButtonConstants.kTriggerPressedThreshold);
+ return axisTrigger(axis, DEFAULT_TRIGGER_PRESSED_THRESHOLD);
}
public Trigger axisTrigger(Axis axis, double threshold) {
@@ -121,11 +192,11 @@ public Trigger axisTrigger(Axis axis, double threshold) {
public abstract double getRightY();
private static final class XboxControllerAdapter extends RBSIController {
- private final CommandXboxController controller;
+ private final CommandNiDsXboxController controller;
private XboxControllerAdapter(int port) {
super(port, "Xbox");
- controller = new CommandXboxController(port);
+ controller = new CommandNiDsXboxController(port);
}
@Override
@@ -184,11 +255,11 @@ public double getRightY() {
}
private static final class PS4ControllerAdapter extends RBSIController {
- private final CommandPS4Controller controller;
+ private final CommandNiDsPS4Controller controller;
private PS4ControllerAdapter(int port) {
super(port, "PS4");
- controller = new CommandPS4Controller(port);
+ controller = new CommandNiDsPS4Controller(port);
}
@Override
@@ -239,11 +310,11 @@ public double getRightY() {
}
private static final class PS5ControllerAdapter extends RBSIController {
- private final CommandPS5Controller controller;
+ private final CommandNiDsPS5Controller controller;
private PS5ControllerAdapter(int port) {
super(port, "PS5");
- controller = new CommandPS5Controller(port);
+ controller = new CommandNiDsPS5Controller(port);
}
@Override
diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java
index 6c94945a..45c8aeac 100644
--- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java
+++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java
@@ -13,12 +13,12 @@
package frc.robot.util;
-import edu.wpi.first.wpilibj.RobotController;
import frc.robot.Constants.PowerConstants;
import frc.robot.Constants.RobotDevices;
import frc.robot.util.Alert.AlertType;
import org.littletonrobotics.conduit.ConduitApi;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.system.RobotController;
/**
* Power monitoring virtual subsystem that periodically polls the Power Distribution Module. Each
@@ -35,7 +35,7 @@ public class RBSIPowerMonitor extends VirtualSubsystem {
private final LoggedTunableNumber batteryCapacityAh;
private double totalAmpHours = 0.0;
private double totalEnergyJoules = 0.0;
- private long lastTimestampUs = RobotController.getFPGATime(); // In microseconds
+ private long lastTimestampUs = RobotController.getTime(); // In microseconds
private double lastVoltage = 0.0;
private double lastTotalCurrent = 0.0;
private boolean totalCurrentOverLimit = false;
@@ -61,6 +61,7 @@ public class RBSIPowerMonitor extends VirtualSubsystem {
private final Alert lowVoltageAlert = new Alert("Low battery voltage!", AlertType.WARNING);
private final Alert criticalVoltageAlert =
new Alert("Critical battery voltage!", AlertType.ERROR);
+ private final double[] channelCurrents = new double[24];
private long loops = 0;
@@ -100,13 +101,14 @@ public void rbsiPeriodic() {
Logger.recordOutput("Power/TotalCurrent", totalCurrent);
Logger.recordOutput("Power/TotalCurrentOverLimit", totalCurrentOverLimit);
- for (int ch = 0; ch < Math.min(conduit.getPDPChannelCount(), portAlerts.length); ch++) {
- portAlerts[ch].set(
- conduit.getPDPChannelCurrent(ch) > PowerConstants.kMotorPortMaxCurrentAmps);
+ int channelCount = Math.min(channelCurrents.length, conduit.getPDPChannelCount());
+ for (int ch = 0; ch < channelCount; ch++) {
+ channelCurrents[ch] = conduit.getPDPChannelCurrent(ch);
+ portAlerts[ch].set(channelCurrents[ch] > PowerConstants.kMotorPortMaxCurrentAmps);
}
// --- Battery estimation ---
- long nowUs = RobotController.getFPGATime();
+ long nowUs = RobotController.getTime();
double dtSec = (nowUs - lastTimestampUs) / 1e6;
lastTimestampUs = nowUs;
@@ -142,8 +144,8 @@ public void rbsiPeriodic() {
private void logGroupCurrent(String name, int[] ports) {
double sum = 0.0;
for (int port : ports) {
- if (port >= 0 && port < conduit.getPDPChannelCount()) {
- sum += conduit.getPDPChannelCurrent(port);
+ if (port >= 0 && port < channelCurrents.length) {
+ sum += channelCurrents[port];
}
}
Logger.recordOutput("Power/Subsystems/" + name + "_Current", sum);
diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java
index 52e79140..d4bda6e6 100644
--- a/src/main/java/frc/robot/util/RBSISubsystem.java
+++ b/src/main/java/frc/robot/util/RBSISubsystem.java
@@ -12,8 +12,8 @@
// GNU General Public License for more details.
package frc.robot.util;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.littletonrobotics.junction.Logger;
+import org.wpilib.command2.SubsystemBase;
/**
* This class is designed to include Az-RBSI specific methods on top of the standard WPILib
diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java
index c4c8cdf9..5b9f8080 100644
--- a/src/main/java/frc/robot/util/SparkUtil.java
+++ b/src/main/java/frc/robot/util/SparkUtil.java
@@ -11,6 +11,7 @@
import com.revrobotics.REVLibError;
import com.revrobotics.spark.SparkBase;
+import com.revrobotics.util.Signal;
import java.util.function.Consumer;
import java.util.function.DoubleConsumer;
import java.util.function.DoubleSupplier;
@@ -30,6 +31,17 @@ public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer
}
}
+ /** Processes a signal from a Spark only if the signal is valid. */
+ public static void ifOk(
+ SparkBase spark, Supplier> supplier, DoubleConsumer consumer) {
+ Signal signal = supplier.get();
+ if (signal.isValid() && signal.getError() == REVLibError.kOk) {
+ consumer.accept(signal.get(0.0));
+ } else {
+ sparkStickyFault = true;
+ }
+ }
+
/** Processes a value from a Spark only if the value is valid. */
public static void ifOk(
SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) {
diff --git a/src/main/java/frc/robot/util/TimedPose.java b/src/main/java/frc/robot/util/TimedPose.java
index 440cd315..e30e00fb 100644
--- a/src/main/java/frc/robot/util/TimedPose.java
+++ b/src/main/java/frc/robot/util/TimedPose.java
@@ -13,9 +13,9 @@
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;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
public record TimedPose(Pose2d pose, double timestampSeconds, Matrix stdDevs) {}
diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java
index efd43942..f2e9f95d 100644
--- a/src/main/java/frc/robot/util/YagslConstants.java
+++ b/src/main/java/frc/robot/util/YagslConstants.java
@@ -11,194 +11,10 @@
package frc.robot.util;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.wpilibj.Filesystem;
-import frc.robot.Constants.DeployConstants;
-import java.io.File;
-import java.util.Arrays;
-import java.util.List;
-import swervelib.parser.json.ModuleJson;
-import swervelib.parser.json.PIDFPropertiesJson;
-import swervelib.parser.json.PhysicalPropertiesJson;
-import swervelib.parser.json.SwerveDriveJson;
-
/**
- * The YAGSL swerve drive constants class
- *
- * The purpose of this class is to convert the YAGSL-based JSON configuration file data into the
- * same constants format as produced by the Phoenix Tuner X's ``TunerConstants.java`` file. The goal
- * here is to make implenentation in the drive class easier.
+ * TODO(2027): Restore the YAGSL JSON parser constants when YAGSL and its required vendordeps
+ * publish 2027/SystemCore-compatible releases.
*/
-public class YagslConstants {
-
- // Define the internal YAGSL objects we will read into
- private static final File yagslDir =
- new File(Filesystem.getDeployDirectory(), DeployConstants.kYagslDir);
- public static final SwerveDriveJson swerveDriveJson; // Needed by the Accelerometer subsystem
- private static final PIDFPropertiesJson pidfPropertiesJson;
- private static final PhysicalPropertiesJson physicalPropertiesJson;
- private static final ModuleJson[] moduleJsons;
- private static final ObjectMapper mapper = new ObjectMapper();
-
- // Use a static to read in the YAGSL JSON files with error catching
- static {
- SwerveDriveJson tempSwerveJson = null;
- PIDFPropertiesJson tempPdifJson = null;
- PhysicalPropertiesJson tempPhysicalJson = null;
- ModuleJson[] tempModuleJsons = null;
-
- try {
- tempSwerveJson =
- mapper.readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class);
- tempPdifJson =
- mapper.readValue(
- new File(yagslDir, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
- tempPhysicalJson =
- mapper.readValue(
- new File(yagslDir, "modules/physicalproperties.json"), PhysicalPropertiesJson.class);
-
- } catch (Exception e) {
- throw new RuntimeException(e);
- }
- swerveDriveJson = tempSwerveJson;
- pidfPropertiesJson = tempPdifJson;
- physicalPropertiesJson = tempPhysicalJson;
-
- tempModuleJsons = new ModuleJson[swerveDriveJson.modules.length];
- try {
- for (int i = 0; i < tempModuleJsons.length; i++) {
- // moduleConfigs.put(swerveDriveJson.modules[i], i);
- File moduleFile = new File(yagslDir, "modules/" + swerveDriveJson.modules[i]);
- if (!moduleFile.exists()) {
- throw new IllegalStateException("Missing YAGSL module config: " + moduleFile);
- }
- tempModuleJsons[i] = mapper.readValue(moduleFile, ModuleJson.class);
- }
- } catch (Exception e) {
- throw new RuntimeException(e);
- }
- moduleJsons = tempModuleJsons;
- }
-
- // Define the Tuner X - like constants from the YAGSL objects
-
- // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
- // This may need to be tuned to your individual robot
- public static final double kCoupleRatio = 3.5;
-
- public static final double kDriveGearRatio =
- physicalPropertiesJson.conversionFactors.drive.gearRatio;
- public static final double kSteerGearRatio =
- physicalPropertiesJson.conversionFactors.angle.gearRatio;
-
- public static final String kCANbusName = swerveDriveJson.imu.canbus;
- public static final int kPigeonId = swerveDriveJson.imu.id;
-
- // These are only used for simulation
- public static final double kSteerInertia = 0.004;
- public static final double kDriveInertia = 0.025;
- // Simulated voltage necessary to overcome friction
- public static final double kSteerFrictionVoltage = 0.25;
- public static final double kDriveFrictionVoltage = 0.25;
-
- // Current / Voltage Limits
- public static final double kSteerCurrentLimit = physicalPropertiesJson.currentLimit.angle;
- public static final double kDriveCurrentLimit = physicalPropertiesJson.currentLimit.drive;
- public static final double kOptimalVoltage = physicalPropertiesJson.optimalVoltage;
-
- // The modules themselves
- static List moduleList = Arrays.asList(swerveDriveJson.modules);
-
- // Front Left
- static ModuleJson flModule = moduleJsons[moduleIndex("frontleft.json")];
- public static final int kFrontLeftDriveMotorId = flModule.drive.id;
- public static final int kFrontLeftSteerMotorId = flModule.angle.id;
- public static final int kFrontLeftEncoderId = flModule.encoder.id;
- public static final String kFrontLeftDriveCanbus = flModule.drive.canbus;
- public static final String kFrontLeftSteerCanbus = flModule.angle.canbus;
- public static final String kFrontLeftEncoderCanbus = flModule.encoder.canbus;
- public static final String kFrontLeftDriveType = flModule.drive.type;
- public static final String kFrontLeftSteerType = flModule.angle.type;
- public static final String kFrontLeftEncoderType = flModule.encoder.type;
- public static final double kFrontLeftEncoderOffset = flModule.absoluteEncoderOffset;
- public static final boolean kFrontLeftDriveInvert = flModule.inverted.drive;
- public static final boolean kFrontLeftSteerInvert = flModule.inverted.angle;
- public static final boolean kFrontLeftEncoderInvert = flModule.absoluteEncoderInverted;
- public static final double kFrontLeftXPosInches = flModule.location.left;
- public static final double kFrontLeftYPosInches = flModule.location.front;
-
- // Front Right
- static ModuleJson frModule = moduleJsons[moduleIndex("frontright.json")];
- public static final int kFrontRightDriveMotorId = frModule.drive.id;
- public static final int kFrontRightSteerMotorId = frModule.angle.id;
- public static final int kFrontRightEncoderId = frModule.encoder.id;
- public static final String kFrontRightDriveCanbus = frModule.drive.canbus;
- public static final String kFrontRightSteerCanbus = frModule.angle.canbus;
- public static final String kFrontRightEncoderCanbus = frModule.encoder.canbus;
- public static final String kFrontRightDriveType = frModule.drive.type;
- public static final String kFrontRightSteerType = frModule.angle.type;
- public static final String kFrontRightEncoderType = frModule.encoder.type;
- public static final double kFrontRightEncoderOffset = frModule.absoluteEncoderOffset;
- public static final boolean kFrontRightDriveInvert = frModule.inverted.drive;
- public static final boolean kFrontRightSteerInvert = frModule.inverted.angle;
- public static final boolean kFrontRightEncoderInvert = frModule.absoluteEncoderInverted;
- public static final double kFrontRightXPosInches = frModule.location.left;
- public static final double kFrontRightYPosInches = frModule.location.front;
-
- // Back Left
- static ModuleJson blModule = moduleJsons[moduleIndex("backleft.json")];
- public static final int kBackLeftDriveMotorId = blModule.drive.id;
- public static final int kBackLeftSteerMotorId = blModule.angle.id;
- public static final int kBackLeftEncoderId = blModule.encoder.id;
- public static final String kBackLeftDriveCanbus = blModule.drive.canbus;
- public static final String kBackLeftSteerCanbus = blModule.angle.canbus;
- public static final String kBackLeftEncoderCanbus = blModule.encoder.canbus;
- public static final String kBackLeftDriveType = blModule.drive.type;
- public static final String kBackLeftSteerType = blModule.angle.type;
- public static final String kBackLeftEncoderType = blModule.encoder.type;
- public static final double kBackLeftEncoderOffset = blModule.absoluteEncoderOffset;
- public static final boolean kBackLeftDriveInvert = blModule.inverted.drive;
- public static final boolean kBackLeftSteerInvert = blModule.inverted.angle;
- public static final boolean kBackLeftEncoderInvert = blModule.absoluteEncoderInverted;
- public static final double kBackLeftXPosInches = blModule.location.left;
- public static final double kBackLeftYPosInches = blModule.location.front;
-
- // Back Right
- static ModuleJson brModule = moduleJsons[moduleIndex("backright.json")];
- public static final int kBackRightDriveMotorId = brModule.drive.id;
- public static final int kBackRightSteerMotorId = brModule.angle.id;
- public static final int kBackRightEncoderId = brModule.encoder.id;
- public static final String kBackRightDriveCanbus = brModule.drive.canbus;
- public static final String kBackRightSteerCanbus = brModule.angle.canbus;
- public static final String kBackRightEncoderCanbus = brModule.encoder.canbus;
- public static final String kBackRightDriveType = brModule.drive.type;
- public static final String kBackRightSteerType = brModule.angle.type;
- public static final String kBackRightEncoderType = brModule.encoder.type;
- public static final double kBackRightEncoderOffset = brModule.absoluteEncoderOffset;
- public static final boolean kBackRightDriveInvert = brModule.inverted.drive;
- public static final boolean kBackRightSteerInvert = brModule.inverted.angle;
- public static final boolean kBackRightEncoderInvert = brModule.absoluteEncoderInverted;
- public static final double kBackRightXPosInches = brModule.location.left;
- public static final double kBackRightYPosInches = brModule.location.front;
-
- // YAGSL PIDF Values
- public static final double kDriveP = pidfPropertiesJson.drive.p;
- public static final double kDriveI = pidfPropertiesJson.drive.i;
- public static final double kDriveD = pidfPropertiesJson.drive.d;
- public static final double kDriveF = pidfPropertiesJson.drive.f;
- public static final double kDriveIZ = pidfPropertiesJson.drive.iz;
- public static final double kSteerP = pidfPropertiesJson.angle.p;
- public static final double kSteerI = pidfPropertiesJson.angle.i;
- public static final double kSteerD = pidfPropertiesJson.angle.d;
- public static final double kSteerF = pidfPropertiesJson.angle.f;
- public static final double kSteerIZ = pidfPropertiesJson.angle.iz;
-
- private static int moduleIndex(String fileName) {
- int index = moduleList.indexOf(fileName);
- if (index < 0) {
- throw new IllegalStateException(
- "YAGSL swervedrive.json is missing required module config '" + fileName + "'.");
- }
- return index;
- }
+public final class YagslConstants {
+ private YagslConstants() {}
}
diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java
index 7f686d65..740d829e 100644
--- a/src/test/CurrentLimitTests.java
+++ b/src/test/CurrentLimitTests.java
@@ -13,21 +13,22 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
+import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.hardware.TalonFX;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.units.measure.Current;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import java.util.function.Supplier;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
+import org.wpilib.driverstation.RobotState;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.simulation.DriverStationSim;
+import org.wpilib.system.Timer;
+import org.wpilib.units.measure.Current;
public class CurrentLimitTests implements AutoCloseable {
final int CONFIG_RETRY_COUNT = 5;
@@ -44,7 +45,7 @@ public void close() {
public void constructDevices() {
assert HAL.initialize(500, 0);
- talon = new TalonFX(0);
+ talon = new TalonFX(0, new CANBus(""));
/* enable the robot */
DriverStationSim.setEnabled(true);
@@ -62,7 +63,7 @@ void shutdown() {
@Test
public void robotIsEnabled() {
/* verify that the robot is enabled */
- assertTrue(DriverStation.isEnabled());
+ assertTrue(RobotState.isEnabled());
}
@Test
@@ -85,7 +86,6 @@ public void testStatorLimit() {
/* Get the next update for stator current */
double initialCurrent = waitForCurrentAbove(statorCurrent, 100.0, 1.0);
- System.out.println("Stator current is " + initialCurrent + " A");
assertTrue(initialCurrent > 100); // Stator current should be in excess of 100 amps
/* Now apply the stator current limit */
@@ -98,7 +98,6 @@ public void testStatorLimit() {
/* Get the next update for stator current */
statorCurrent.waitForUpdate(1);
- System.out.println("Stator current is " + statorCurrent);
assertTrue(statorCurrent.getValueAsDouble() < 25); // Give some wiggle room
}
@@ -124,7 +123,6 @@ public void testSupplyLimit() {
/* Get the next update for supply current */
supplyCurrent.waitForUpdate(1);
- System.out.println("Supply current is " + supplyCurrent);
assertTrue(
supplyCurrent.getValueAsDouble() > 25); // Supply current should be high before limiting
@@ -138,7 +136,6 @@ public void testSupplyLimit() {
/* Get the next update for supply current */
supplyCurrent.waitForUpdate(1);
- System.out.println("Supply current is " + supplyCurrent);
assertTrue(
supplyCurrent.getValueAsDouble() <= 15
&& supplyCurrent.getValueAsDouble()
@@ -150,7 +147,6 @@ public void testSupplyLimit() {
/* Get the next update for supply current */
supplyCurrent.waitForUpdate(1);
- System.out.println("Supply current is " + supplyCurrent);
assertTrue(supplyCurrent.getValueAsDouble() < 10); // Give some wiggle room
}
@@ -166,8 +162,8 @@ private void retryConfigApply(Supplier toApply) {
private double waitForCurrentAbove(
StatusSignal currentSignal, double threshold, double timeoutSec) {
double current = currentSignal.getValueAsDouble();
- double startTime = Timer.getFPGATimestamp();
- while (current <= threshold && Timer.getFPGATimestamp() - startTime < timeoutSec) {
+ double startTime = Timer.getTimestamp();
+ while (current <= threshold && Timer.getTimestamp() - startTime < timeoutSec) {
currentSignal.waitForUpdate(0.100);
current = currentSignal.getValueAsDouble();
}
diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java
index 2bc7eb03..995f8e6b 100644
--- a/src/test/FusedCANcoderTests.java
+++ b/src/test/FusedCANcoderTests.java
@@ -15,16 +15,17 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import edu.wpi.first.hal.HAL;
import java.util.function.Supplier;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
+import org.wpilib.hardware.hal.HAL;
public class FusedCANcoderTests {
final double SET_DELTA = 0.1;
@@ -37,8 +38,8 @@ public class FusedCANcoderTests {
public void constructDevices() {
assert HAL.initialize(500, 0);
- talon = new TalonFX(0);
- cancoder = new CANcoder(0);
+ talon = new TalonFX(0, new CANBus(""));
+ cancoder = new CANcoder(0, new CANBus(""));
}
@Test
@@ -73,8 +74,6 @@ public void testIndividualPos() {
BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
BaseStatusSignal.waitForAll(1.0, talonPos, cancoderPos);
- System.out.println("Talon Pos vs expected: " + talonPos + " vs " + TALON_POSITION);
- System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
assertEquals(talonPos.getValueAsDouble(), TALON_POSITION, SET_DELTA);
assertEquals(cancoderPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
}
@@ -118,8 +117,6 @@ public void testFusedCANcoderPos() {
talonPos.waitForUpdate(1.0);
/* Make sure Talon matches CANcoder, since it should be using CANcoder's position */
- System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION);
- System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
assertEquals(talonPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
assertEquals(cancoderPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
}
@@ -163,8 +160,6 @@ public void testRemoteCANcoderPos() {
talonPos.waitForUpdate(1.0);
/* Make sure Talon matches CANcoder, since it should be using CANcoder's position */
- System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION);
- System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
assertEquals(talonPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
assertEquals(cancoderPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
}
diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java
index 177ae60a..90b96654 100644
--- a/src/test/LatencyCompensationTests.java
+++ b/src/test/LatencyCompensationTests.java
@@ -15,13 +15,14 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.system.Timer;
public class LatencyCompensationTests {
final double DOUBLE_DELTA = 0.01;
@@ -33,8 +34,8 @@ public class LatencyCompensationTests {
public void constructDevices() {
assert HAL.initialize(500, 0);
- talonfx = new TalonFX(0);
- cancoder = new CANcoder(0);
+ talonfx = new TalonFX(0, new CANBus(""));
+ cancoder = new CANcoder(0, new CANBus(""));
}
@Test
@@ -61,7 +62,6 @@ public void testLatencyCompensator() {
/* Wait for an update on all of them so they're synchronized */
StatusCode status = StatusCode.OK;
for (int i = 0; i < 5; ++i) {
- System.out.println("Waiting on signals");
status = BaseStatusSignal.waitForAll(1, talonPos, talonVel, cancoderPos, cancoderVel);
if (status.isOK()) break;
}
@@ -83,10 +83,6 @@ public void testLatencyCompensator() {
double functionCompensatedCANcoder =
BaseStatusSignal.getLatencyCompensatedValueAsDouble(cancoderPos, cancoderVel);
- /* Assert the two methods match */
- System.out.println("Talon Pos: " + compensatedTalonPos + " - " + functionCompensatedTalon);
- System.out.println(
- "CANcoder Pos: " + compensatedCANcoderPos + " - " + functionCompensatedCANcoder);
assertEquals(compensatedTalonPos, functionCompensatedTalon, DOUBLE_DELTA);
assertEquals(compensatedCANcoderPos, functionCompensatedCANcoder, DOUBLE_DELTA);
}
diff --git a/src/test/frc/robot/FieldStateTest.java b/src/test/frc/robot/FieldStateTest.java
new file mode 100644
index 00000000..37fd8f54
--- /dev/null
+++ b/src/test/frc/robot/FieldStateTest.java
@@ -0,0 +1,36 @@
+// Copyright (c) 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.
+
+package frc.robot;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+import org.wpilib.driverstation.Alliance;
+
+class FieldStateTest {
+ @Test
+ void autoWinnerIsActiveDuringEvenShiftsIncludingBoundaries() {
+ assertFalse(FieldState.isAllianceActiveDuringShift(105.0, Alliance.BLUE, Alliance.BLUE));
+ assertTrue(FieldState.isAllianceActiveDuringShift(104.999, Alliance.BLUE, Alliance.BLUE));
+ assertTrue(FieldState.isAllianceActiveDuringShift(80.0, Alliance.BLUE, Alliance.BLUE));
+ assertFalse(FieldState.isAllianceActiveDuringShift(79.999, Alliance.BLUE, Alliance.BLUE));
+ assertFalse(FieldState.isAllianceActiveDuringShift(55.0, Alliance.BLUE, Alliance.BLUE));
+ assertTrue(FieldState.isAllianceActiveDuringShift(54.999, Alliance.BLUE, Alliance.BLUE));
+ assertTrue(FieldState.isAllianceActiveDuringShift(30.0, Alliance.BLUE, Alliance.BLUE));
+ }
+
+ @Test
+ void nonWinnerHasComplementaryShiftSchedule() {
+ assertTrue(FieldState.isAllianceActiveDuringShift(105.0, Alliance.RED, Alliance.BLUE));
+ assertFalse(FieldState.isAllianceActiveDuringShift(80.0, Alliance.RED, Alliance.BLUE));
+ assertTrue(FieldState.isAllianceActiveDuringShift(55.0, Alliance.RED, Alliance.BLUE));
+ assertFalse(FieldState.isAllianceActiveDuringShift(30.0, Alliance.RED, Alliance.BLUE));
+ }
+}
diff --git a/src/test/frc/robot/RobotConstantsTest.java b/src/test/frc/robot/RobotConstantsTest.java
index 352eae37..4950a3d1 100644
--- a/src/test/frc/robot/RobotConstantsTest.java
+++ b/src/test/frc/robot/RobotConstantsTest.java
@@ -3,13 +3,16 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
import frc.robot.subsystems.drive.SwerveConstants;
+import java.util.Arrays;
import org.junit.jupiter.api.Test;
class RobotConstantsTest {
@Test
void robotDeviceCanBusesMatchSwerveConstants() {
+ assertEquals(SwerveConstants.kPigeonCanbus, Constants.RobotDevices.PIGEON.getBus());
assertEquals(SwerveConstants.kFLDriveCanbus, Constants.RobotDevices.FL_DRIVE.getBus());
assertEquals(SwerveConstants.kFLSteerCanbus, Constants.RobotDevices.FL_ROTATION.getBus());
assertEquals(SwerveConstants.kFLEncoderCanbus, Constants.RobotDevices.FL_CANCODER.getBus());
@@ -27,6 +30,65 @@ void robotDeviceCanBusesMatchSwerveConstants() {
assertEquals(SwerveConstants.kBREncoderCanbus, Constants.RobotDevices.BR_CANCODER.getBus());
}
+ @Test
+ void eachModuleHasOneRegisteredCANBusForAllThreeDevices() {
+ for (int module = 0; module < 4; module++) {
+ String moduleBus = SwerveConstants.moduleCANBus(module);
+ assertTrue(Arrays.asList(Constants.CANBuses.ALL).contains(moduleBus));
+ switch (module) {
+ case 0 -> {
+ assertEquals(Constants.DrivebaseConstants.kFLCANBus, moduleBus);
+ assertEquals(moduleBus, SwerveConstants.kFLDriveCanbus);
+ assertEquals(moduleBus, SwerveConstants.kFLSteerCanbus);
+ assertEquals(moduleBus, SwerveConstants.kFLEncoderCanbus);
+ }
+ case 1 -> {
+ assertEquals(Constants.DrivebaseConstants.kFRCANBus, moduleBus);
+ assertEquals(moduleBus, SwerveConstants.kFRDriveCanbus);
+ assertEquals(moduleBus, SwerveConstants.kFRSteerCanbus);
+ assertEquals(moduleBus, SwerveConstants.kFREncoderCanbus);
+ }
+ case 2 -> {
+ assertEquals(Constants.DrivebaseConstants.kBLCANBus, moduleBus);
+ assertEquals(moduleBus, SwerveConstants.kBLDriveCanbus);
+ assertEquals(moduleBus, SwerveConstants.kBLSteerCanbus);
+ assertEquals(moduleBus, SwerveConstants.kBLEncoderCanbus);
+ }
+ case 3 -> {
+ assertEquals(Constants.DrivebaseConstants.kBRCANBus, moduleBus);
+ assertEquals(moduleBus, SwerveConstants.kBRDriveCanbus);
+ assertEquals(moduleBus, SwerveConstants.kBRSteerCanbus);
+ assertEquals(moduleBus, SwerveConstants.kBREncoderCanbus);
+ }
+ default -> throw new AssertionError();
+ }
+ }
+ assertTrue(Arrays.asList(Constants.CANBuses.ALL).contains(SwerveConstants.kPigeonCanbus));
+ assertThrows(IllegalArgumentException.class, () -> SwerveConstants.moduleCANBus(4));
+ }
+
+ @Test
+ void systemcorePortsAreExplicit() {
+ assertEquals("can_s1", Constants.CANBuses.RIO);
+ assertEquals(0, Constants.CANBuses.revPort(Constants.CANBuses.SYSTEMCORE_0));
+ assertEquals(4, Constants.CANBuses.revPort(Constants.CANBuses.SYSTEMCORE_4));
+ assertEquals(1, Constants.CANBuses.revPort(Constants.CANBuses.RIO));
+ assertThrows(IllegalArgumentException.class, () -> Constants.CANBuses.revPort("DriveTrain"));
+ }
+
+ @Test
+ void defaultDriveUsesThreeSystemcoreBuses() {
+ assertEquals(Constants.CANBuses.SYSTEMCORE_2, Constants.DrivebaseConstants.kFLCANBus);
+ assertEquals(Constants.CANBuses.SYSTEMCORE_2, Constants.DrivebaseConstants.kFRCANBus);
+ assertEquals(Constants.CANBuses.SYSTEMCORE_3, Constants.DrivebaseConstants.kBLCANBus);
+ assertEquals(Constants.CANBuses.SYSTEMCORE_3, Constants.DrivebaseConstants.kBRCANBus);
+ assertEquals(Constants.CANBuses.SYSTEMCORE_4, Constants.DrivebaseConstants.kPigeonCANBus);
+ assertEquals(
+ 4,
+ Arrays.stream(Constants.CANBuses.ALL).distinct().count()); // mechanisms + three drive buses
+ assertTrue(SwerveConstants.kDrivebaseHasMultipleCANBuses);
+ }
+
@Test
void fieldLayoutConstantsAreInternallyConsistent() {
assertEquals(FieldConstants.defaultAprilTagType.getLayout(), FieldConstants.aprilTagLayout);
diff --git a/src/test/frc/robot/commands/DriveCommandsTests.java b/src/test/frc/robot/commands/DriveCommandsTests.java
index 1a82ac45..c0db9fe5 100644
--- a/src/test/frc/robot/commands/DriveCommandsTests.java
+++ b/src/test/frc/robot/commands/DriveCommandsTests.java
@@ -3,17 +3,17 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotNull;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.OperatorConstants;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.imu.Imu;
import frc.robot.subsystems.imu.ImuIOSim;
+import frc.robot.util.MathUtil;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
+import org.wpilib.command2.Command;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.simulation.DriverStationSim;
class DriveCommandsTests {
private static final double EPSILON = 1e-9;
diff --git a/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java b/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java
index cda13bb4..9cace145 100644
--- a/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java
+++ b/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java
@@ -5,8 +5,8 @@
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
-import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
+import org.wpilib.math.util.Units;
class GeneratedTunerConstantsTest {
private static final double EPSILON = 1e-9;
diff --git a/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java b/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java
index fc454264..501039da 100644
--- a/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java
+++ b/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java
@@ -5,15 +5,15 @@
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import frc.robot.subsystems.imu.Imu;
import frc.robot.subsystems.imu.ImuIOSim;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.simulation.DriverStationSim;
class DriveSubsystemTests {
private static final double EPSILON = 1e-9;
@@ -98,4 +98,31 @@ void disabledCoastDoesNotCountBaselineSampleAsStationary() {
drive.updateDisabledCoastState(false, true, 1.50, 0.0, stationaryPositions);
assertFalse(drive.isDisabledCoast(1.50));
}
+
+ @Test
+ void pathPlannerStartResetsWhenVisionIsMissingOrEstimatorIsUninitialized() {
+ Pose2d pathStart = new Pose2d(4.0, 6.0, Rotation2d.kZero);
+
+ assertEquals(
+ Drive.PathPlannerStartAction.RESET_TO_PATH_START,
+ Drive.determinePathPlannerStartAction(
+ new Pose2d(6.0, 6.0, Rotation2d.kZero), pathStart, false, 0.5));
+ assertEquals(
+ Drive.PathPlannerStartAction.RESET_TO_PATH_START,
+ Drive.determinePathPlannerStartAction(Pose2d.kZero, pathStart, true, 0.5));
+ }
+
+ @Test
+ void pathPlannerStartUsesNearbyVisionAndBlocksDistantVision() {
+ Pose2d pathStart = new Pose2d(4.0, 6.0, Rotation2d.kZero);
+
+ assertEquals(
+ Drive.PathPlannerStartAction.USE_VISION_POSE,
+ Drive.determinePathPlannerStartAction(
+ new Pose2d(4.4, 6.0, Rotation2d.kZero), pathStart, true, 0.5));
+ assertEquals(
+ Drive.PathPlannerStartAction.BLOCK_AUTO,
+ Drive.determinePathPlannerStartAction(
+ new Pose2d(4.6, 6.0, Rotation2d.kZero), pathStart, true, 0.5));
+ }
}
diff --git a/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java b/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java
new file mode 100644
index 00000000..83f1b992
--- /dev/null
+++ b/src/test/frc/robot/subsystems/drive/LatestSampleQueueTest.java
@@ -0,0 +1,34 @@
+// 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.
+
+package frc.robot.subsystems.drive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class LatestSampleQueueTest {
+ @Test
+ void overflowEvictsOldestAndRetainsNewestInOrder() {
+ LatestSampleQueue queue = new LatestSampleQueue<>(3);
+
+ assertFalse(queue.offerLatest(1));
+ assertFalse(queue.offerLatest(2));
+ assertFalse(queue.offerLatest(3));
+ assertTrue(queue.offerLatest(4));
+
+ assertEquals(3, queue.size());
+ assertEquals(2, queue.poll());
+ assertEquals(3, queue.poll());
+ assertEquals(4, queue.poll());
+ assertNull(queue.poll());
+ }
+}
diff --git a/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java b/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java
index 7d5aac3f..0d4e83c1 100644
--- a/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java
+++ b/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java
@@ -2,9 +2,9 @@
import static org.junit.jupiter.api.Assertions.assertEquals;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import org.junit.jupiter.api.Test;
+import org.wpilib.command2.sysid.SysIdRoutine;
+import org.wpilib.math.util.Units;
class FlywheelSysIdTests {
diff --git a/src/test/frc/robot/subsystems/vision/VisionIOTests.java b/src/test/frc/robot/subsystems/vision/VisionIOTests.java
index 9238f0b3..ceec191c 100644
--- a/src/test/frc/robot/subsystems/vision/VisionIOTests.java
+++ b/src/test/frc/robot/subsystems/vision/VisionIOTests.java
@@ -6,10 +6,10 @@
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.util.Units;
class VisionIOTests {
private static final double EPSILON = 1e-9;
diff --git a/src/test/frc/robot/util/UtilTests.java b/src/test/frc/robot/util/UtilTests.java
index 94c759f2..3a9e01e7 100644
--- a/src/test/frc/robot/util/UtilTests.java
+++ b/src/test/frc/robot/util/UtilTests.java
@@ -5,10 +5,11 @@
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.RobotController;
+import frc.robot.Constants;
import java.util.NavigableMap;
import org.junit.jupiter.api.Test;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.system.RobotController;
class UtilTests {
private static final double EPSILON = 1e-9;
@@ -17,7 +18,7 @@ class UtilTests {
void timeUtilReturnsSecondsFromAdvantageKitMicrosecondTimestamp() {
assertTrue(HAL.initialize(500, 0));
- double expectedSeconds = RobotController.getFPGATime() * 1.0e-6;
+ double expectedSeconds = RobotController.getTime() * 1.0e-6;
double actualSeconds = TimeUtil.now();
assertEquals(expectedSeconds, actualSeconds, 0.050);
@@ -25,9 +26,9 @@ void timeUtilReturnsSecondsFromAdvantageKitMicrosecondTimestamp() {
@Test
void robotDeviceIdUsesValueEqualityAndHandlesMissingPowerPort() {
- RobotDeviceId first = new RobotDeviceId(7, "rio", null);
- RobotDeviceId second = new RobotDeviceId(7, new String("rio"), null);
- RobotDeviceId differentBus = new RobotDeviceId(7, "DriveTrain", null);
+ RobotDeviceId first = new RobotDeviceId(7, Constants.CANBuses.RIO, null);
+ RobotDeviceId second = new RobotDeviceId(7, new String(Constants.CANBuses.RIO), null);
+ RobotDeviceId differentBus = new RobotDeviceId(7, Constants.CANBuses.SYSTEMCORE_2, null);
assertEquals(first, second);
assertEquals(first.hashCode(), second.hashCode());
@@ -38,8 +39,8 @@ void robotDeviceIdUsesValueEqualityAndHandlesMissingPowerPort() {
@Test
void robotDeviceIdLooksUpCanBusThroughRegistry() {
- RBSICANBusRegistry.initSim("rio");
- RobotDeviceId device = new RobotDeviceId(1, "rio", 0);
+ RBSICANBusRegistry.initSim(Constants.CANBuses.RIO);
+ RobotDeviceId device = new RobotDeviceId(1, Constants.CANBuses.RIO, 0);
assertThrows(IllegalStateException.class, device::getCANBus);
}
diff --git a/themes/frc-biocore-high-contrast-theme-1.0.1.vsix b/themes/frc-biocore-high-contrast-theme-1.0.1.vsix
new file mode 100644
index 00000000..35457cc3
Binary files /dev/null and b/themes/frc-biocore-high-contrast-theme-1.0.1.vsix differ
diff --git a/themes/frc-rebuilt-high-contrast-theme-1.0.0.vsix b/themes/frc-rebuilt-high-contrast-theme-1.0.0.vsix
new file mode 100644
index 00000000..d3463de0
Binary files /dev/null and b/themes/frc-rebuilt-high-contrast-theme-1.0.0.vsix differ
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 469ae542..1a8abd54 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,9 +1,9 @@
{
"fileName": "AdvantageKit.json",
"name": "AdvantageKit",
- "version": "26.0.2",
+ "version": "27.0.0-alpha-4",
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
- "frcYear": "2026",
+ "wpilibYear": "2027_alpha5",
"mavenUrls": [
"https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
],
@@ -12,18 +12,18 @@
{
"groupId": "org.littletonrobotics.akit",
"artifactId": "akit-java",
- "version": "26.0.2"
+ "version": "27.0.0-alpha-4"
}
],
"jniDependencies": [
{
"groupId": "org.littletonrobotics.akit",
"artifactId": "akit-wpilibio",
- "version": "26.0.2",
+ "version": "27.0.0-alpha-4",
"skipInvalidPlatforms": false,
"isJar": false,
"validPlatforms": [
- "linuxathena",
+ "linuxsystemcore",
"linuxx86-64",
"linuxarm64",
"osxuniversal",
diff --git a/vendordeps/Autopilot.json b/vendordeps/Autopilot.json
index a8e979c1..2e4137bd 100644
--- a/vendordeps/Autopilot.json
+++ b/vendordeps/Autopilot.json
@@ -2,17 +2,18 @@
"name": "Autopilot",
"uuid": "4e922d47-9954-4db5-929f-c708cc62e152",
"fileName": "Autopilot.json",
- "version": "1.6.1",
- "frcYear": "2026",
- "jsonUrl": "https://therekrab.github.io/autopilot/vendordep.json",
+ "version": "1.7.0-alpha-6",
+ "frcYear": "2027",
+ "wpilibYear": "2027_alpha5",
+ "jsonUrl": "https://raw.githubusercontent.com/AZ-First/autopilot/refs/heads/sc2027/vendordep.json",
"mavenUrls": [
- "https://jitpack.io"
+ "https://raw.githubusercontent.com/AZ-First/autopilot/refs/heads/sc2027/maven"
],
"javaDependencies": [
{
"groupId": "com.github.therekrab",
"artifactId": "autopilot",
- "version": "1.6.1"
+ "version": "1.7.0-alpha-6"
}
],
"cppDependencies": [],
diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json
deleted file mode 100644
index 7665da4e..00000000
--- a/vendordeps/ChoreoLib2026.json
+++ /dev/null
@@ -1,44 +0,0 @@
-{
- "fileName": "ChoreoLib2026.json",
- "name": "ChoreoLib",
- "version": "2026.0.3",
- "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
- "frcYear": "2026",
- "mavenUrls": [
- "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/",
- "https://repo1.maven.org/maven2"
- ],
- "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json",
- "javaDependencies": [
- {
- "groupId": "choreo",
- "artifactId": "ChoreoLib-java",
- "version": "2026.0.3"
- },
- {
- "groupId": "com.google.code.gson",
- "artifactId": "gson",
- "version": "2.11.0"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": [
- {
- "groupId": "choreo",
- "artifactId": "ChoreoLib-cpp",
- "version": "2026.0.3",
- "libName": "ChoreoLib",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal",
- "linuxathena",
- "linuxarm32",
- "linuxarm64"
- ]
- }
- ]
-}
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/CommandsV2.json
similarity index 54%
rename from vendordeps/WPILibNewCommands.json
rename to vendordeps/CommandsV2.json
index d90630e9..8358b098 100644
--- a/vendordeps/WPILibNewCommands.json
+++ b/vendordeps/CommandsV2.json
@@ -1,25 +1,32 @@
{
- "fileName": "WPILibNewCommands.json",
- "name": "WPILib-New-Commands",
+ "fileName": "CommandsV2.json",
+ "name": "Commands V2",
"version": "1.0.0",
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
- "frcYear": "2026",
+ "wpilibYear": "2027_alpha5",
"mavenUrls": [],
"jsonUrl": "",
+ "conflictsWith": [
+ {
+ "uuid": "4decdc05-a056-46cf-9561-39449bbb01306",
+ "errorMessage": "Users can not have both Commands v2 and Commands v3 vendordeps in their robot program.",
+ "offlineFileName": "CommandsV3.json"
+ }
+ ],
"javaDependencies": [
{
- "groupId": "edu.wpi.first.wpilibNewCommands",
- "artifactId": "wpilibNewCommands-java",
+ "groupId": "org.wpilib.commandsv2",
+ "artifactId": "commandsv2-java",
"version": "wpilib"
}
],
"jniDependencies": [],
"cppDependencies": [
{
- "groupId": "edu.wpi.first.wpilibNewCommands",
- "artifactId": "wpilibNewCommands-cpp",
+ "groupId": "org.wpilib.commandsv2",
+ "artifactId": "commandsv2-cpp",
"version": "wpilib",
- "libName": "wpilibNewCommands",
+ "libName": "commandsv2",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
"sharedLibrary": true,
diff --git a/vendordeps/PathplannerLib-2026.1.2.json b/vendordeps/PathplannerLibSystemCoreAlpha.json
similarity index 75%
rename from vendordeps/PathplannerLib-2026.1.2.json
rename to vendordeps/PathplannerLibSystemCoreAlpha.json
index 5f04ffa7..9b36d2bf 100644
--- a/vendordeps/PathplannerLib-2026.1.2.json
+++ b/vendordeps/PathplannerLibSystemCoreAlpha.json
@@ -1,18 +1,18 @@
{
- "fileName": "PathplannerLib-2026.1.2.json",
+ "fileName": "PathplannerLibSystemCoreAlpha.json",
"name": "PathplannerLib",
- "version": "2026.1.2",
+ "version": "2027.0.0-alpha-3",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
- "frcYear": "2026",
+ "wpilibYear": "2027_alpha5",
"mavenUrls": [
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
],
- "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+ "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLibSystemCoreAlpha.json",
"javaDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
- "version": "2026.1.2"
+ "version": "2027.0.0-alpha-3"
}
],
"jniDependencies": [],
@@ -20,7 +20,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
- "version": "2026.1.2",
+ "version": "2027.0.0-alpha-3",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -29,8 +29,7 @@
"windowsx86-64",
"linuxx86-64",
"osxuniversal",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"linuxarm64"
]
}
diff --git a/vendordeps/Phoenix5-5.36.0.json b/vendordeps/Phoenix5-5.36.0.json
deleted file mode 100644
index c60dd4cc..00000000
--- a/vendordeps/Phoenix5-5.36.0.json
+++ /dev/null
@@ -1,171 +0,0 @@
-{
- "fileName": "Phoenix5-5.36.0.json",
- "name": "CTRE-Phoenix (v5)",
- "version": "5.36.0",
- "frcYear": "2026",
- "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
- "mavenUrls": [
- "https://maven.ctr-electronics.com/release/"
- ],
- "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2026-latest.json",
- "requires": [
- {
- "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
- "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.",
- "offlineFileName": "Phoenix6-frc2026-latest.json",
- "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json"
- }
- ],
- "conflictsWith": [
- {
- "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": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
- "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.",
- "offlineFileName": "Phoenix5-replay-frc2026-latest.json"
- }
- ],
- "javaDependencies": [
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "api-java",
- "version": "5.36.0"
- },
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "wpiapi-java",
- "version": "5.36.0"
- }
- ],
- "jniDependencies": [
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "cci",
- "version": "5.36.0",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "cci-sim",
- "version": "5.36.0",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- }
- ],
- "cppDependencies": [
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "wpiapi-cpp",
- "version": "5.36.0",
- "libName": "CTRE_Phoenix_WPI",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "api-cpp",
- "version": "5.36.0",
- "libName": "CTRE_Phoenix",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "cci",
- "version": "5.36.0",
- "libName": "CTRE_PhoenixCCI",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "wpiapi-cpp-sim",
- "version": "5.36.0",
- "libName": "CTRE_Phoenix_WPISim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "api-cpp-sim",
- "version": "5.36.0",
- "libName": "CTRE_PhoenixSim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "cci-sim",
- "version": "5.36.0",
- "libName": "CTRE_PhoenixCCISim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- }
- ]
-}
diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-26.50.0-alpha-1.json
similarity index 87%
rename from vendordeps/Phoenix6-frc2026-latest.json
rename to vendordeps/Phoenix6-26.50.0-alpha-1.json
index e14f95f9..40dd410c 100644
--- a/vendordeps/Phoenix6-frc2026-latest.json
+++ b/vendordeps/Phoenix6-26.50.0-alpha-1.json
@@ -1,60 +1,60 @@
{
- "fileName": "Phoenix6-frc2026-latest.json",
+ "fileName": "Phoenix6-26.50.0-alpha-1.json",
"name": "CTRE-Phoenix (v6)",
- "version": "26.3.0",
- "frcYear": "2026",
+ "version": "26.50.0-alpha-1",
+ "wpilibYear": "2027_alpha5",
"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-frc2026-latest.json",
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2027-latest.json",
"conflictsWith": [
{
"uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
- "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
- "offlineFileName": "Phoenix6-replay-frc2026-latest.json"
+ "errorMessage": "Users cannot have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+ "offlineFileName": "Phoenix6-replay-frc2027-latest.json"
}
],
"javaDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java",
- "version": "26.3.0"
+ "version": "26.50.0-alpha-1"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "api-cpp",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
- "linuxathena"
+ "linuxsystemcore"
],
"simMode": "hwsim"
},
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
- "linuxathena"
+ "linuxsystemcore"
],
"simMode": "hwsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "api-cpp-sim",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -68,7 +68,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -82,7 +82,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -96,7 +96,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -110,7 +110,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -124,7 +124,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -138,7 +138,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -152,7 +152,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -166,7 +166,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -180,7 +180,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -194,7 +194,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -208,7 +208,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -224,7 +224,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -233,14 +233,14 @@
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
- "linuxathena"
+ "linuxsystemcore"
],
"simMode": "hwsim"
},
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -249,14 +249,14 @@
"windowsx86-64",
"linuxx86-64",
"linuxarm64",
- "linuxathena"
+ "linuxsystemcore"
],
"simMode": "hwsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -272,7 +272,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -288,7 +288,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -304,7 +304,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -320,7 +320,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -336,7 +336,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -352,7 +352,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFXS",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProTalonFXS",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -368,7 +368,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -384,7 +384,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -400,7 +400,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANrange",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProCANrange",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -416,7 +416,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdi",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProCANdi",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -432,7 +432,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANdle",
- "version": "26.3.0",
+ "version": "26.50.0-alpha-1",
"libName": "CTRE_SimProCANdle",
"headerClassifier": "headers",
"sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index e7a0655f..2b6bd6b2 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,63 +1,60 @@
{
"fileName": "REVLib.json",
"name": "REVLib",
- "version": "2026.0.5",
- "frcYear": "2026",
+ "version": "2027.0.0-alpha-4",
+ "wpilibYear": "2027_alpha5",
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
"mavenUrls": [
"https://maven.revrobotics.com/"
],
- "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json",
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2027.json",
"javaDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-java",
- "version": "2026.0.5"
+ "version": "2027.0.0-alpha-4"
}
],
"jniDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
},
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibBackendDriver",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
},
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibWpiBackendDriver",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
}
@@ -66,7 +63,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-cpp",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"libName": "REVLib",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -75,15 +72,14 @@
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
},
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"libName": "REVLibDriver",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -92,15 +88,14 @@
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
},
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibBackendDriver",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"libName": "BackendDriver",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
@@ -108,15 +103,14 @@
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
},
{
"groupId": "com.revrobotics.frc",
"artifactId": "RevLibWpiBackendDriver",
- "version": "2026.0.5",
+ "version": "2027.0.0-alpha-4",
"libName": "REVLibWpi",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
@@ -124,8 +118,7 @@
"windowsx86-64",
"linuxarm64",
"linuxx86-64",
- "linuxathena",
- "linuxarm32",
+ "linuxsystemcore",
"osxuniversal"
]
}
diff --git a/vendordeps/ReduxLib-2026.1.2.json b/vendordeps/ReduxLib-2026.1.2.json
deleted file mode 100644
index 431fe17e..00000000
--- a/vendordeps/ReduxLib-2026.1.2.json
+++ /dev/null
@@ -1,69 +0,0 @@
-{
- "fileName": "ReduxLib-2026.1.2.json",
- "name": "ReduxLib",
- "version": "2026.1.2",
- "frcYear": "2026",
- "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
- "mavenUrls": [
- "https://maven.reduxrobotics.com/"
- ],
- "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2026.json",
- "javaDependencies": [
- {
- "groupId": "com.reduxrobotics.frc",
- "artifactId": "ReduxLib-java",
- "version": "2026.1.2"
- }
- ],
- "jniDependencies": [
- {
- "groupId": "com.reduxrobotics.frc",
- "artifactId": "ReduxLib-fifo",
- "version": "2026.1.2",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "linuxathena",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal",
- "windowsx86-64"
- ]
- }
- ],
- "cppDependencies": [
- {
- "groupId": "com.reduxrobotics.frc",
- "artifactId": "ReduxLib-cpp",
- "version": "2026.1.2",
- "libName": "ReduxLib",
- "headerClassifier": "headers",
- "sourcesClassifier": "sources",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "linuxathena",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal",
- "windowsx86-64"
- ]
- },
- {
- "groupId": "com.reduxrobotics.frc",
- "artifactId": "ReduxLib-fifo",
- "version": "2026.1.2",
- "libName": "reduxfifo",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "linuxathena",
- "linuxx86-64",
- "linuxarm64",
- "osxuniversal",
- "windowsx86-64"
- ]
- }
- ]
-}
diff --git a/vendordeps/Studica.json b/vendordeps/Studica.json
deleted file mode 100644
index daf14345..00000000
--- a/vendordeps/Studica.json
+++ /dev/null
@@ -1,71 +0,0 @@
-{
- "fileName": "Studica.json",
- "name": "Studica",
- "version": "2026.0.0",
- "frcYear": "2026",
- "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
- "mavenUrls": [
- "https://dev.studica.com/maven/release/2026/"
- ],
- "jsonUrl": "https://dev.studica.com/maven/release/2026/json/Studica-2026.0.0.json",
- "javaDependencies": [
- {
- "groupId": "com.studica.frc",
- "artifactId": "Studica-java",
- "version": "2026.0.0"
- }
- ],
- "jniDependencies": [
- {
- "groupId": "com.studica.frc",
- "artifactId": "Studica-driver",
- "version": "2026.0.0",
- "skipInvalidPlatforms": true,
- "isJar": false,
- "validPlatforms": [
- "windowsx86-64",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- }
- ],
- "cppDependencies": [
- {
- "groupId": "com.studica.frc",
- "artifactId": "Studica-cpp",
- "version": "2026.0.0",
- "libName": "Studica",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- },
- {
- "groupId": "com.studica.frc",
- "artifactId": "Studica-driver",
- "version": "2026.0.0",
- "libName": "StudicaDriver",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- }
- ]
-}
diff --git a/vendordeps/ThriftyLib-2026.json b/vendordeps/ThriftyLib-2026.json
deleted file mode 100644
index 69917f06..00000000
--- a/vendordeps/ThriftyLib-2026.json
+++ /dev/null
@@ -1,20 +0,0 @@
-{
- "fileName": "ThriftyLib-2026.json",
- "name": "ThriftyLib",
- "version": "2026.1.2",
- "frcYear": "2026",
- "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
- "mavenUrls": [
- "https://docs.home.thethriftybot.com"
- ],
- "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib-2026.json",
- "javaDependencies": [
- {
- "groupId": "com.thethriftybot.frc",
- "artifactId": "ThriftyLib-java",
- "version": "2026.1.2"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": []
-}
diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json
index f5b535e5..f623d349 100644
--- a/vendordeps/URCL.json
+++ b/vendordeps/URCL.json
@@ -2,7 +2,7 @@
"fileName": "URCL.json",
"name": "URCL",
"version": "2026.0.0",
- "frcYear": "2026",
+ "wpilibYear": "2027_alpha5",
"uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c",
"mavenUrls": [
"https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
deleted file mode 100644
index aa65f46c..00000000
--- a/vendordeps/maple-sim.json
+++ /dev/null
@@ -1,26 +0,0 @@
-{
- "fileName": "maple-sim.json",
- "name": "maplesim",
- "version": "0.4.0-beta",
- "frcYear": "2026",
- "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
- "mavenUrls": [
- "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases",
- "https://repo1.maven.org/maven2"
- ],
- "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json",
- "javaDependencies": [
- {
- "groupId": "org.ironmaple",
- "artifactId": "maplesim-java",
- "version": "0.4.0-beta"
- },
- {
- "groupId": "org.dyn4j",
- "artifactId": "dyn4j",
- "version": "5.0.2"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": []
-}
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 6279e58e..3f6b31fc 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,9 +1,9 @@
{
"fileName": "photonlib.json",
"name": "photonlib",
- "version": "v2026.2.2",
+ "version": "v2027.0.0-alpha-2",
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
- "frcYear": "2026",
+ "wpilibYear": "2027_alpha5",
"mavenUrls": [
"https://maven.photonvision.org/repository/internal",
"https://maven.photonvision.org/repository/snapshots"
@@ -13,12 +13,12 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
- "version": "v2026.2.2",
+ "version": "v2027.0.0-alpha-2",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
"windowsx86-64",
- "linuxathena",
+ "linuxsystemcore",
"linuxx86-64",
"osxuniversal"
]
@@ -28,14 +28,14 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-cpp",
- "version": "v2026.2.2",
+ "version": "v2027.0.0-alpha-2",
"libName": "photonlib",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
- "linuxathena",
+ "linuxsystemcore",
"linuxx86-64",
"osxuniversal"
]
@@ -43,14 +43,14 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
- "version": "v2026.2.2",
+ "version": "v2027.0.0-alpha-2",
"libName": "photontargeting",
"headerClassifier": "headers",
"sharedLibrary": true,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
- "linuxathena",
+ "linuxsystemcore",
"linuxx86-64",
"osxuniversal"
]
@@ -60,12 +60,12 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-java",
- "version": "v2026.2.2"
+ "version": "v2027.0.0-alpha-2"
},
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-java",
- "version": "v2026.2.2"
+ "version": "v2027.0.0-alpha-2"
}
]
}
diff --git a/vendordeps/yagsl-2026.4.1.json b/vendordeps/yagsl-2026.4.1.json
deleted file mode 100644
index 60ce44a2..00000000
--- a/vendordeps/yagsl-2026.4.1.json
+++ /dev/null
@@ -1,46 +0,0 @@
-{
- "fileName": "yagsl-2026.4.1.json",
- "name": "YAGSL",
- "version": "2026.4.1",
- "frcYear": "2026",
- "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400",
- "mavenUrls": [
- "https://yet-another-software-suite.github.io/YAGSL/releases/",
- "https://repo1.maven.org/maven2"
- ],
- "jsonUrl": "https://yet-another-software-suite.github.io/YAGSL/yagsl.json",
- "javaDependencies": [
- {
- "groupId": "swervelib",
- "artifactId": "YAGSL-java",
- "version": "2026.4.1"
- },
- {
- "groupId": "org.dyn4j",
- "artifactId": "dyn4j",
- "version": "5.0.2"
- }
- ],
- "requires": [
- {
- "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
- "errorMessage": "ReduxLib is required!",
- "offlineFileName": "ReduxLib.json",
- "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2026.json"
- },
- {
- "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
- "errorMessage": "REVLib is required!",
- "offlineFileName": "REVLib.json",
- "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json"
- },
- {
- "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0",
- "errorMessage": "ThriftyLib is required!",
- "offlineFileName": "ThriftyLib.json",
- "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib-2026.json"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": []
-}
+ 
+