Add basic driver controls by aidnem · Pull Request #55 · team401/2026-Robot-Code

aidnem · 2026-02-11T17:15:00Z

Adds driver controls and a coordinateRobotActions method which:

updates match state
Handles intake & climber actions together (to avoid extending in 2 directions at once)
Determines if vision can be trusted
Runs shot calculator for smart or manual modes and determines if we have a "real" shot or are aiming for the closest possible shot when an actual shot is impossible
Determines when to run the spindexer/balltower to only shoot when we'll make the shot

…er and some for operator

…slide

…ntake and climber extension

…chine

…indings

…hood stowing, and gating shots based on possibility

Copilot

Pull request overview

This pull request adds basic driver control functionality for teleop operation, establishing a coordination layer that manages robot actions based on controller inputs. The PR implements manual and smart autonomy modes, shooting controls, intake deployment coordination, and hood stowing logic for navigating under field obstacles.

Changes:

Added CoordinationLayer class to coordinate subsystem actions based on driver/operator inputs with support for manual and smart autonomy modes
Implemented driver and operator controller bindings for intake, shooting, climbing, and autonomy mode switching
Added utility classes (EnhancedLine2d, OptionalUtil, AllianceUtil) and subsystem state checking methods (isAimedCorrectly, isAtGoalVelocity, isStowed)

Reviewed changes

Copilot reviewed 33 out of 33 changed files in this pull request and generated 12 comments.

Show a summary per file

File	Description
EnhancedLine2d.java	New utility class for 2D line intersection detection to support trench collision detection
OptionalUtil.java	New utility class providing helper methods for working with multiple Optional values
AllianceUtil.java	Added getOppAlliance() method to get the opposing alliance
TurretSubsystem.java	Added isAimedCorrectly() method to check if turret is at goal heading
ShooterSubsystem.java	Added coast state, velocity getters, stopShooter(), and isAtGoalVelocity() methods
ShooterState.java	Added CoastState for coasting the shooter to a stop
IntakeSubsystem.java	Added isStowed() method to check if intake is within frame perimeter
HoodSubsystem.java	Added setShouldStowForTrench(), getCurrentAngle(), and isAimedCorrectly() methods
MatchState.java	Renamed auto override parameters to weWonAuto/weLostAuto for clarity
CoordinationLayer.java	Major refactor: added button loop, state machines for extension coordination, manual/smart mode switching, shot aiming, and hood stowing logic
RobotContainer.java	Updated to use CoordinationLayer for bindings and vision localizer initialization
ControllerSetup.java	Removed intake and match state bindings (moved to CoordinationLayer)
ShotCalculations.java	Modified calculateShotFromMap to always return a shot (clamped if out of range) with isReal flag
VisionConstants.java	Added disconnectedDebounceTime for vision connection debouncing
TurretConstants.java	Added turretSetpointEpsilon for determining when turret is at target
ShooterConstants.java	Added shooterVelocitySetpointEpsilon for velocity tolerance checking
ManualModeConstants.java	New constants class for manual mode shooting parameters
JsonConstants.java	Added manualModeConstants loading and tuning server route
IntakeConstants.java	Added stowThresholdAngle for determining when intake is stowed
IndexerConstants.java	Removed indexerDemoMode flag, added indexingVelocity constant
HopperConstants.java	Removed hopperDemoMode flag, added indexingVelocity constant
HoodConstants.java	Added hoodSetpointEpsilon and timeToStowHood constants
FieldLocations.java	Added getManualHubShootingPosition() method
FieldLocationInstance.java	Added manualHubShootingPosition field
FieldConstants.java	Added oppInnerCenterPoint() method for opponent hub center
AllianceBasedFieldConstants.java	New class providing alliance-based field location accessors
controllers-xbox.json	Added comprehensive driver and operator bindings for all teleop controls
VisionConstants.json	Added disconnectedDebounceTime configuration
ManualModeConstants.json	New JSON configuration for manual mode constants
IntakeConstants.json	Added stowThresholdAngle configuration
HopperConstants.json	Removed hopperDemoMode configuration
FeatureFlags.json	Enabled runHood flag

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot

Pull request overview

Copilot reviewed 36 out of 36 changed files in this pull request and generated 8 comments.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-02-24T19:39:40Z

+    double D = l.d.cross(dneg);
+
+    // Use Cramer’s rule
+    if (D == 0.0) {


The comparison uses exact equality (==) for floating-point numbers, which is generally unreliable due to floating-point precision issues. Consider using a small epsilon for comparison instead, such as Math.abs(D) < EPSILON.

@godmar is this necessary? I feel like if it's very very small, that just means the lines are super close to parallel right?

Yes. I think that's fine here. If D is very small (close to zero), then dividing by it will be very large, so the test params.[st] < 1.0 should fail; this would be true even if were positive infinity.
The check == 0.0 guards against getting NaN here.

…owing hood, and format

make every subsystem write a graph viz of its state machine when in sim

godmar

I didn't see anything obviously wrong. This will evolve further anyhow.

godmar · 2026-02-26T00:42:35Z

+   * When the turret heading is within turretSetpointEpsilon of its goal heading, it is considered
+   * to be "at the target"
+   */
+  public final Angle turretSetpointEpsilon = Degrees.of(1.0);


What if the turrent never makes it this close? Should there be a timeout?

Hopefully the shooter will always make it this close. We could add a timeout but I think if it's acceptable to shoot outside of this range then it means we need a wider range.

godmar · 2026-02-26T00:43:49Z

-    driveController.setD(gains.kD());
-    DRIVE_KV = gains.kV();
+    System.out.println("setDriveGains called with " + gains);
+    // Don't actually update gains as it causes awful drive performance in sim.


Don't forget to uncomment.

I think we might want to just leave these as they are, unless we're going to retune the physical sim constants of drive to make it behave properly. If you enable setting the gains in sim, it makes the steer wheels unable to target their setpoint properly and they oscillate indefinitely.

godmar · 2026-02-26T02:01:41Z

+                // location for a manual mode shot if we ever have to run "no turret"
+                && turret.map(TurretSubsystem::isAimedCorrectly).orElse(true));
+
+    if (canShoot) {


Am I reading this logic right that that hopper and indexer won't be activated until the robot is aiming at the target? This will add delay, won't it?

Should we turn the indexer at least on before that?

We might want to start running the indexer a bit sooner. The problem is, based on shop testing tonight, the indexer was grabbing fuel by itself without ever running hopper. It managed to shoot 4 before I had time to turn the hopper on.

We might want to start running the indexer a bit sooner. The problem is, based on shop testing tonight, the indexer was grabbing fuel by itself without ever running hopper. It managed to shoot 4 before I had time to turn the hopper on.

Nice.

…dable

(#49) start adding mappings to controllers-xbox.json

294267f

aidnem linked an issue Feb 11, 2026 that may be closed by this pull request

Add basic driver controls to coordination layer #49

Closed

4 tasks

aidnem added 17 commits February 11, 2026 12:15

(#49) Update controllers-xbox with better values

c5a4e39

Merge branch 'main' into 49-add-basic-driver-controls

779bd15

(#49) Begin adding basic state to CoordinationLayer

4067458

(#49) Remove subsystem demo modes, add basic button bindings for driv…

c99ed47

…er and some for operator

(#49) Update controls to latest revision and format

4d77c6f

(#49) Replace operator stow intake with operator toggle intake deploy

4bfaf9e

(#49) Update button bindings to reflect latest changes to button map …

b4f72c3

…slide

(#49) Add extensionStateMachine to coordination layer to coordinate i…

4ae781c

…ntake and climber extension

Merge branch 'main' into 49-add-basic-driver-controls

a0da038

(#49) Fix error in controllers and add printing of extension state ma…

7a0cd67

…chine

(#49) WIP - Start adding actual action coordination based on button b…

d1b827d

…indings

Merge branch 'main' into 49-add-basic-driver-controls

179025b

(#49) Correctly select the shot target, controls are done except for …

674ed6b

…hood stowing, and gating shots based on possibility

(#49) Add predictive hood stowing

3c6155f

Merge branch 'main' into 49-add-basic-driver-controls

4d37b8d

(#49) Integrate MatchState with coordination layer

4741160

(#49) 'finish' adding required logic for shooting to coordinationLayer

3d28651

aidnem marked this pull request as ready for review February 24, 2026 18:33

avidraccoon requested a review from Copilot February 24, 2026 18:33

Copilot started reviewing on behalf of avidraccoon February 24, 2026 18:33 View session

Copilot AI reviewed Feb 24, 2026

View reviewed changes

aidnem added 4 commits February 24, 2026 13:58

(#49) Re-sync constants

4a11d42

(#49) Fix whileTrue usage in operator bindings

7021c87

(#49) Remove remaining RunCommands and improve shooter gating logic

7915805

(#49) Fix various bugs in inputs and clean up javadocs

acace3b

aidnem requested a review from Copilot February 24, 2026 19:34

Copilot started reviewing on behalf of aidnem February 24, 2026 19:35 View session

Copilot AI reviewed Feb 24, 2026

View reviewed changes

aidnem added 3 commits February 24, 2026 14:48

(#49) Add manual mode hub shooting

5a1eddc

(#49) Make shooter enter correct state when exiting test mode

8767a78

(#49) update shooter stopping logic to correctly stop shooter when st…

1a30c61

…owing hood, and format

aidnem requested review from avidraccoon and godmar February 24, 2026 22:23

(#49) dump state machines to file

10b0471

make every subsystem write a graph viz of its state machine when in sim

godmar approved these changes Feb 26, 2026

View reviewed changes

aidnem added 3 commits February 25, 2026 21:50

(#49) Remove saveObject call

fa8b85a

(#49) Rename all pitch stuff to ExitPitch in hood to make it more rea…

5ccf6dd

…dable

(#49) Rename OptionalUtil methods to indicate multiple optionals

d20c6ab

aidnem requested a review from godmar February 26, 2026 03:02

godmar approved these changes Feb 26, 2026

View reviewed changes

aidnem merged commit 2aa5353 into main Feb 26, 2026
3 checks passed

PChild deleted the 49-add-basic-driver-controls branch April 14, 2026 16:24

Conversation

aidnem commented Feb 11, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI Feb 24, 2026

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

godmar left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

aidnem commented Feb 11, 2026 •

edited

Loading