From 7eb22656810394b8d60c29fef424431e1b3346dd Mon Sep 17 00:00:00 2001 From: James Mwangi Date: Thu, 7 May 2026 21:26:23 -0400 Subject: [PATCH 1/7] Apply speed modifier to drivetrain, change speed modification conditions, and update Hood to only deploy while shooting (plus a change to the Mech3d) --- .../org/team157/robot/BuildConstants.java | 12 +++--- .../java/org/team157/robot/Constants.java | 2 +- .../org/team157/robot/RobotContainer.java | 39 +++++++++++++------ .../subsystems/SunstoneV2Mechanism3D.java | 2 +- 4 files changed, 35 insertions(+), 20 deletions(-) diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 49f3c6e..5d740fe 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "FRC2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 357; - public static final String GIT_SHA = "3a0121448822ec0deb5cc19e3e95fa164cb6437d"; - public static final String GIT_DATE = "2026-05-07 15:10:36 EDT"; - public static final String GIT_BRANCH = "task/reexport-as-model"; - public static final String BUILD_DATE = "2026-05-07 16:00:43 EDT"; - public static final long BUILD_UNIX_TIME = 1778184043836L; + public static final int GIT_REVISION = 358; + public static final String GIT_SHA = "1347e10a01d5f5638101845d7774f2b45807473d"; + public static final String GIT_DATE = "2026-05-07 16:01:15 EDT"; + public static final String GIT_BRANCH = "feature/sotm-control"; + public static final String BUILD_DATE = "2026-05-07 20:50:33 EDT"; + public static final long BUILD_UNIX_TIME = 1778201433389L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team157/robot/Constants.java b/src/main/java/org/team157/robot/Constants.java index e92e4b9..79db7f9 100644 --- a/src/main/java/org/team157/robot/Constants.java +++ b/src/main/java/org/team157/robot/Constants.java @@ -42,7 +42,7 @@ public static enum Mode { public static class ModifierConstants { // Reduces drive speed by this factor when precision mode is active. - public static final double PRECISION_DRIVE_MODIFIER = 0.5; + public static final double PRECISION_DRIVE_MODIFIER = 0.4; // When true, reduces drive speed by 50%. public static final boolean ROOKIE_MODE = false; public static final double ROOKIE_DRIVE_MODIFIER = 0.5; diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index 6d792d8..33e4195 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -236,9 +236,9 @@ private void configureBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driverController.getLeftY(), - () -> -driverController.getLeftX(), - () -> -driverController.getRightX())); + () -> modifySpeed(-driverController.getLeftY()), + () -> modifySpeed(-driverController.getLeftX()), + () -> modifySpeed(-driverController.getRightX()))); // Update the pose estimation and turret tracking angle while no other vision commands are // running. vision.setDefaultCommand(vision.setDefault(drive, turret)); @@ -281,8 +281,12 @@ private void configureBindings() { /// FlYWHEEL HOOD /// ///////////////////// // Enables dynamic control of the flywheel and hood. - driverController.a().toggleOnTrue(flywheel.setDynamicVelocity()); - driverController.a().toggleOnTrue(hood.setDynamicHoodAngle()); + driverController + .a() + .toggleOnTrue(flywheel.setDynamicVelocity()); + driverController + .a() + .toggleOnTrue(hood.setDynamicHoodAngle()); //////////////////////////// /// INTAKE UPTAKE HOPPER /// @@ -300,9 +304,13 @@ private void configureBindings() { driverController.rightTrigger().whileTrue(hopper.set(1)); } // Runs the hopper, uptake, and intake backwards at a low speed to clear jams. - driverController.y().whileTrue(forceOuttake()); + driverController + .y() + .whileTrue(forceOuttake()); // Wiggles the intake up and down to free up stuck balls - driverController.x().toggleOnTrue(slapdown.wiggleIntake()); + driverController + .x() + .toggleOnTrue(slapdown.wiggleIntake()); ////////////////////////////////////////////////// /// OPERATOR COMMANDS /// ////////////////////////////////////////////////// @@ -315,9 +323,13 @@ private void configureBindings() { // Disables automatic turret tracking when manual override is enabled, // allowing the operator to control the turret without interference from vision tracking. - turretTrackingTrigger().whileTrue(turret.trackTagGlobalRelative()); - turretTrackingTrigger().whileTrue(flywheel.setDynamicVelocity()); - turretTrackingTrigger().whileTrue(hood.setDynamicHoodAngle()); + turretTrackingTrigger() + .whileTrue(turret.trackTagGlobalRelative()); + turretTrackingTrigger() + .whileTrue(flywheel.setDynamicVelocity()); + turretTrackingTrigger() + .and(driverController.rightTrigger()) + .whileTrue(hood.setDynamicHoodAngle()); /////////////////////// /// MANUAL FLYWHEEL /// @@ -365,9 +377,12 @@ private void configureBindings() { .toggleOnTrue(slapdown.retractIntake()); } - // If the right bumper is held, apply the precision modifier of 0.5x speed. + /** Apply a speed modifier when the right bumper (dedicated toggle) + * or shooting trigger are held, or the robot is under the trench. */ public double modifySpeed(final double speed) { - if (driverController.rightBumper().getAsBoolean() || drive.isUnderTrench()) { + if (driverController.rightBumper().getAsBoolean() + || driverController.rightTrigger().getAsBoolean() + || drive.isUnderTrench()) { return speed * ModifierConstants.PRECISION_DRIVE_MODIFIER; } else { return speed; diff --git a/src/main/java/org/team157/robot/subsystems/SunstoneV2Mechanism3D.java b/src/main/java/org/team157/robot/subsystems/SunstoneV2Mechanism3D.java index d5de6e5..bf8f09c 100644 --- a/src/main/java/org/team157/robot/subsystems/SunstoneV2Mechanism3D.java +++ b/src/main/java/org/team157/robot/subsystems/SunstoneV2Mechanism3D.java @@ -76,7 +76,7 @@ public Transform3d getHoodPivotLocation() { return new Transform3d( 0.1 * Math.cos(turret.getTurretRotation().in(Radians)), 0.1 * Math.sin(turret.getTurretRotation().in(Radians)), - 0.070, + 0.064, new Rotation3d(0, 0, turret.getTurretRotation().in(Radians))); } From a596e66f76dbfcc03b4bc1628380a0396631383c Mon Sep 17 00:00:00 2001 From: James Mwangi Date: Thu, 7 May 2026 21:39:09 -0400 Subject: [PATCH 2/7] Shrink trench zones by 2 meters on X axis and add separate trench drive modifier. --- src/main/deploy/positionDetails.json | 16 ++++---- .../org/team157/robot/BuildConstants.java | 10 ++--- .../java/org/team157/robot/Constants.java | 1 + .../org/team157/robot/RobotContainer.java | 37 ++++++++----------- 4 files changed, 29 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/positionDetails.json b/src/main/deploy/positionDetails.json index 9e570cf..2c433d6 100644 --- a/src/main/deploy/positionDetails.json +++ b/src/main/deploy/positionDetails.json @@ -59,27 +59,27 @@ "yMax": 7.98703 }, "blueTrenchLow": { - "xMin": 2.5259, + "xMin": 3.5259, "yMin": -10, - "xMax": 6.7197, + "xMax": 5.7197, "yMax": 1.589881 }, "blueTrenchHigh": { - "xMin": 2.5259, + "xMin": 3.5259, "yMin": 6.492081, - "xMax": 6.7197, + "xMax": 5.7197, "yMax": 18.096644 }, "redTrenchLow": { - "xMin": 9.815700, + "xMin": 10.815700, "yMin": -10, - "xMax": 14.004738, + "xMax": 13.004738, "yMax": 1.589881 }, "redTrenchHigh": { - "xMin": 9.815700, + "xMin": 10.815700, "yMin": 6.492081, - "xMax": 14.004738, + "xMax": 13.004738, "yMax": 18.096644 } } diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 5d740fe..9d0e500 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "FRC2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 358; - public static final String GIT_SHA = "1347e10a01d5f5638101845d7774f2b45807473d"; - public static final String GIT_DATE = "2026-05-07 16:01:15 EDT"; + public static final int GIT_REVISION = 359; + public static final String GIT_SHA = "7eb22656810394b8d60c29fef424431e1b3346dd"; + public static final String GIT_DATE = "2026-05-07 21:26:23 EDT"; public static final String GIT_BRANCH = "feature/sotm-control"; - public static final String BUILD_DATE = "2026-05-07 20:50:33 EDT"; - public static final long BUILD_UNIX_TIME = 1778201433389L; + public static final String BUILD_DATE = "2026-05-07 21:30:24 EDT"; + public static final long BUILD_UNIX_TIME = 1778203824656L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team157/robot/Constants.java b/src/main/java/org/team157/robot/Constants.java index 79db7f9..cd1a7fd 100644 --- a/src/main/java/org/team157/robot/Constants.java +++ b/src/main/java/org/team157/robot/Constants.java @@ -43,6 +43,7 @@ public static enum Mode { public static class ModifierConstants { // Reduces drive speed by this factor when precision mode is active. public static final double PRECISION_DRIVE_MODIFIER = 0.4; + public static final double TRENCH_DRIVE_MODIFIER = 0.6; // When true, reduces drive speed by 50%. public static final boolean ROOKIE_MODE = false; public static final double ROOKIE_DRIVE_MODIFIER = 0.5; diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index 33e4195..de48972 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -281,12 +281,8 @@ private void configureBindings() { /// FlYWHEEL HOOD /// ///////////////////// // Enables dynamic control of the flywheel and hood. - driverController - .a() - .toggleOnTrue(flywheel.setDynamicVelocity()); - driverController - .a() - .toggleOnTrue(hood.setDynamicHoodAngle()); + driverController.a().toggleOnTrue(flywheel.setDynamicVelocity()); + driverController.a().toggleOnTrue(hood.setDynamicHoodAngle()); //////////////////////////// /// INTAKE UPTAKE HOPPER /// @@ -304,13 +300,9 @@ private void configureBindings() { driverController.rightTrigger().whileTrue(hopper.set(1)); } // Runs the hopper, uptake, and intake backwards at a low speed to clear jams. - driverController - .y() - .whileTrue(forceOuttake()); + driverController.y().whileTrue(forceOuttake()); // Wiggles the intake up and down to free up stuck balls - driverController - .x() - .toggleOnTrue(slapdown.wiggleIntake()); + driverController.x().toggleOnTrue(slapdown.wiggleIntake()); ////////////////////////////////////////////////// /// OPERATOR COMMANDS /// ////////////////////////////////////////////////// @@ -323,13 +315,11 @@ private void configureBindings() { // Disables automatic turret tracking when manual override is enabled, // allowing the operator to control the turret without interference from vision tracking. + turretTrackingTrigger().whileTrue(turret.trackTagGlobalRelative()); + turretTrackingTrigger().whileTrue(flywheel.setDynamicVelocity()); turretTrackingTrigger() - .whileTrue(turret.trackTagGlobalRelative()); - turretTrackingTrigger() - .whileTrue(flywheel.setDynamicVelocity()); - turretTrackingTrigger() - .and(driverController.rightTrigger()) - .whileTrue(hood.setDynamicHoodAngle()); + .and(driverController.rightTrigger()) + .whileTrue(hood.setDynamicHoodAngle()); /////////////////////// /// MANUAL FLYWHEEL /// @@ -377,13 +367,16 @@ private void configureBindings() { .toggleOnTrue(slapdown.retractIntake()); } - /** Apply a speed modifier when the right bumper (dedicated toggle) - * or shooting trigger are held, or the robot is under the trench. */ + /** + * Apply a speed modifier when the right bumper (dedicated toggle) or shooting trigger are held, + * or the robot is under the trench. + */ public double modifySpeed(final double speed) { if (driverController.rightBumper().getAsBoolean() - || driverController.rightTrigger().getAsBoolean() - || drive.isUnderTrench()) { + || driverController.rightTrigger().getAsBoolean()) { return speed * ModifierConstants.PRECISION_DRIVE_MODIFIER; + } else if(drive.isUnderTrench()) { + return speed * ModifierConstants.TRENCH_DRIVE_MODIFIER; } else { return speed; } From 4eef33e1932c31a8ad13402e4ba600abdb66e8b8 Mon Sep 17 00:00:00 2001 From: James Mwangi Date: Fri, 8 May 2026 07:14:48 -0400 Subject: [PATCH 3/7] Kill Claude --- CLAUDE.md | 81 ------------------------------------------------------- 1 file changed, 81 deletions(-) delete mode 100644 CLAUDE.md diff --git a/CLAUDE.md b/CLAUDE.md deleted file mode 100644 index af984b7..0000000 --- a/CLAUDE.md +++ /dev/null @@ -1,81 +0,0 @@ -# CLAUDE.md - -This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. - -## Build & Deploy Commands - -```bash -./gradlew build # Compile and check for errors -./gradlew deploy # Build and deploy to RoboRIO over USB or network -./gradlew spotlessApply # Format code (Google Java Format via Spotless) -./gradlew spotlessCheck # Check formatting without modifying -./gradlew test # Run JUnit 5 tests -./gradlew replayWatch # AdvantageKit log replay for debugging -``` - -The CI runs `./gradlew build` on every push/PR via `.github/workflows/build.yml`. - -`BuildConstants.java` is auto-generated with Git info — never edit it manually. -`src/main/java/org/team157/robot/generated/TunerConstants.java` is generated by CTRE Tuner X — edit with caution. - -## Architecture Overview - -This is a **WPILib command-based** robot using **AdvantageKit** for structured logging and replay and **Yet Another Mechanism System** for mechanism/motor control abstraction & simulation. The three runtime modes are selected in `Robot.java`: -- `REAL` — hardware with real sensors -- `SIM` — physics simulation (run on desktop) -- `REPLAY` — replay from an AdvantageKit `.wpilog` file - -### IO Interface Pattern - -Every mechanism follows this pattern (example: Intake): - -| File | Role | -|------|------| -| `Intake.java` | Subsystem — extends `SubsystemBase`, holds an `IntakeIO`, calls `Logger.processInputs()` each periodic | -| `IntakeIO.java` | Interface — defines `IntakeIOInputs` (annotated `@AutoLog`) and hardware command methods | -| `IntakeIOTalonFX.java` | Hardware impl — uses Phoenix 6 TalonFX / CTRE devices | -| `IntakeConstants.java` | CAN IDs, gear ratios, PID gains, physical constants | - -The subsystem never talks to hardware directly — it only calls `io.*` methods. This enables REAL/SIM/REPLAY switching at construction time in `RobotContainer`. - -### Subsystems - -| Subsystem | Key Notes | -|-----------|-----------| -| `drive/` | 4-module swerve, Pigeon 2 gyro, SwerveDrivePoseEstimator, PathPlanner AutoBuilder | -| `vision/` | PhotonVision multi-camera AprilTag pose estimation; feeds into drive odometry | -| `turret/` | Rotates toward targets; uses vision for `trackTagGlobalRelative` | -| `flywheel/` | Velocity-controlled shooter wheel | -| `hood/` | Adjustable angle based on estimated target distance | -| `intake/` | Ball intake roller | -| `hopper/` | Ball storage/feeding | -| `uptake/` | Feeds balls from hopper to flywheel | -| `slapdown/` | Intake pivot deploy/retract | -| `leds/` | 38-pixel addressable strip on PWM port 9 | - -### Key Central Files - -- **`RobotContainer.java`** — instantiates all subsystems, wires default commands, registers PathPlanner named commands (`DeployIntake`, `RunIntake`, `RunHopper`, `ShootBalls`, `Wiggle`), and configures both Xbox controller button bindings. -- **`Constants.java`** — global constants (field dimensions, CAN bus names: `"rio"` and `"canivore"`). -- **`commands/DriveCommands.java`** — factory methods for field-relative teleop drive, SysId characterization routines, and wheel radius measurement. - -### Motor & Sensor Hardware - -- **TalonFX (Kraken X60, Kraken X44)** via Phoenix 6 for all mechanisms -- **CANcoder** for swerve module absolute position -- **Pigeon 2** IMU for gyro -- Two CAN buses: `"rio"` (RoboRIO) and `"canivore"` (CANivore) -- **REV Through Bore Encoder** for sensing mechanism positions (instantiated as a generic WPILib `DutyCycleEncoder`) -- REVLib available but not currently in active use - -### Vision - -Three PhotonVision cameras (`camera0`, `camera1`, `camera2`). `VisionIO` interface has `VisionIOPhotonVision` (real), `VisionIOPhotonVisionSim` (simulation), and `VisionIOLimelight` (alternative). Pose estimates are fed into `Drive`'s `SwerveDrivePoseEstimator`. - -### Autonomous - -PathPlanner v2026 manages paths. Named commands are registered in `RobotContainer` via `NamedCommands.registerCommand(...)`. Path files live in `src/main/deploy/pathplanner/`. - -### Logging - -AdvantageKit `Logger` records all `@AutoLog`-annotated inputs each loop. On the robot, logs write to a USB stick. Metadata (Git SHA, branch, build date) is logged at startup from `BuildConstants`. From 73cffc5252654fd848c4d43929d8b0d4bcea36d7 Mon Sep 17 00:00:00 2001 From: James Mwangi Date: Fri, 8 May 2026 18:47:12 -0400 Subject: [PATCH 4/7] Add operator control to adjust flywheel speed factor --- .../org/team157/robot/BuildConstants.java | 10 +-- src/main/java/org/team157/robot/Robot.java | 1 + .../org/team157/robot/RobotContainer.java | 61 +++++++++++++------ .../robot/subsystems/flywheel/Flywheel.java | 5 +- 4 files changed, 52 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 9d0e500..441fde1 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "FRC2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 359; - public static final String GIT_SHA = "7eb22656810394b8d60c29fef424431e1b3346dd"; - public static final String GIT_DATE = "2026-05-07 21:26:23 EDT"; + public static final int GIT_REVISION = 361; + public static final String GIT_SHA = "4eef33e1932c31a8ad13402e4ba600abdb66e8b8"; + public static final String GIT_DATE = "2026-05-08 07:14:48 EDT"; public static final String GIT_BRANCH = "feature/sotm-control"; - public static final String BUILD_DATE = "2026-05-07 21:30:24 EDT"; - public static final long BUILD_UNIX_TIME = 1778203824656L; + public static final String BUILD_DATE = "2026-05-08 12:02:41 EDT"; + public static final long BUILD_UNIX_TIME = 1778256161616L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team157/robot/Robot.java b/src/main/java/org/team157/robot/Robot.java index 6f79c45..1729472 100644 --- a/src/main/java/org/team157/robot/Robot.java +++ b/src/main/java/org/team157/robot/Robot.java @@ -114,6 +114,7 @@ public void robotPeriodic() { // Gets hub activity status to display on the dashboard. Logger.recordOutput("Misc/Hub Active?", m_robotContainer.isHubActive()); Logger.recordOutput("Misc/Under Trench?", RobotContainer.drive.isUnderTrench()); + Logger.recordOutput("Flywheel Modifier", RobotContainer.ballisticSpeedModifier); m_field.setRobotPose(RobotContainer.drive.getPose()); } diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index de48972..d703faf 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -66,6 +66,11 @@ public class RobotContainer { RotationsPerSecond.of(0.75) .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + /** + * Speed factor used in flywheel ballistic equations, to be manually adjusted by the operator + */ + public static double ballisticSpeedModifier = 1; + // Subsystems private final Vision vision; public static Drive drive; @@ -287,22 +292,24 @@ private void configureBindings() { //////////////////////////// /// INTAKE UPTAKE HOPPER /// //////////////////////////// - // Swaps the intake and shooting triggers if Maya mode is enabled, per Maya's preference. - if (ModifierConstants.MAYA_MODE) { - // Shooting on left trigger, intake on right trigger - driverController.leftTrigger().whileTrue(uptake.set(1)); - driverController.rightTrigger().whileTrue(intake.runIntake()); - driverController.leftTrigger().whileTrue(hopper.set(1)); - } else { - // Shooting on right trigger, intake on left trigger - driverController.rightTrigger().whileTrue(uptake.set(1)); - driverController.leftTrigger().whileTrue(intake.runIntake()); - driverController.rightTrigger().whileTrue(hopper.set(1)); - } + driverController.rightTrigger().whileTrue(uptake.set(1)); + driverController.leftTrigger().whileTrue(intake.runIntake()); + driverController.rightTrigger().whileTrue(hopper.set(1)); + // Runs the hopper, uptake, and intake backwards at a low speed to clear jams. - driverController.y().whileTrue(forceOuttake()); + driverController + .y() + .whileTrue(forceOuttake()); // Wiggles the intake up and down to free up stuck balls - driverController.x().toggleOnTrue(slapdown.wiggleIntake()); + driverController + .x() + .toggleOnTrue(slapdown.wiggleIntake()); + + // (in/de)creases the ballistic modifier + operatorController + .x() + .or(operatorController.b()) + .onTrue(setModifier()); ////////////////////////////////////////////////// /// OPERATOR COMMANDS /// ////////////////////////////////////////////////// @@ -315,8 +322,10 @@ private void configureBindings() { // Disables automatic turret tracking when manual override is enabled, // allowing the operator to control the turret without interference from vision tracking. - turretTrackingTrigger().whileTrue(turret.trackTagGlobalRelative()); - turretTrackingTrigger().whileTrue(flywheel.setDynamicVelocity()); + turretTrackingTrigger() + .whileTrue(turret.trackTagGlobalRelative()); + turretTrackingTrigger() + .whileTrue(flywheel.setDynamicVelocity()); turretTrackingTrigger() .and(driverController.rightTrigger()) .whileTrue(hood.setDynamicHoodAngle()); @@ -360,7 +369,10 @@ private void configureBindings() { // Deploy and retract the intake with the A and Y buttons, but only when the // back button is held to prevent accidental activation during teleop. - operatorController.a().and(operatorController.back()).toggleOnTrue(slapdown.deployIntake()); + operatorController + .a() + .and(operatorController.back()) + .toggleOnTrue(slapdown.deployIntake()); operatorController .y() .and(operatorController.back()) @@ -375,13 +387,26 @@ public double modifySpeed(final double speed) { if (driverController.rightBumper().getAsBoolean() || driverController.rightTrigger().getAsBoolean()) { return speed * ModifierConstants.PRECISION_DRIVE_MODIFIER; - } else if(drive.isUnderTrench()) { + } else if (drive.isUnderTrench()) { return speed * ModifierConstants.TRENCH_DRIVE_MODIFIER; } else { return speed; } } + /** Update the ballistic equation modifier based on the operator's button presses */ + public void setBallisticSpeedModifier() { + if (driverController.x().getAsBoolean()) { + ballisticSpeedModifier = ballisticSpeedModifier + 0.1; + } else if (driverController.b().getAsBoolean()) { + ballisticSpeedModifier = ballisticSpeedModifier - 0.1; + } + } + + public InstantCommand setModifier() { + return new InstantCommand(() -> setBallisticSpeedModifier()); + } + public boolean isHubActive() { Optional alliance = DriverStation.getAlliance(); // If we have no alliance, we cannot be enabled, therefore no hub. diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java index 85fc226..6cb95bd 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java @@ -88,7 +88,7 @@ public Command setVelocity(AngularVelocity speed) { * @return {@link Command} continuously updating the flywheel velocity. */ public Command setDynamicVelocity() { - return io.setVelocity(this::getDesiredFlywheelVelocity); + return io.setVelocity(() -> getDesiredFlywheelVelocity()); } //////////////////////// @@ -206,7 +206,8 @@ public AngularVelocity getDesiredFlywheelVelocity() { double desiredRPM = (ballVelocity * 60) / (Math.PI * flywheelDiameterMeters) - * FlywheelConstants.SPEED_FACTOR; + * FlywheelConstants.SPEED_FACTOR + * RobotContainer.ballisticSpeedModifier; return RPM.of(Math.max(2800, desiredRPM)); } From 5ccb4a0a85f991e41fcc9853d6ef8464cbef77db Mon Sep 17 00:00:00 2001 From: Aztechs157-Git Date: Tue, 12 May 2026 16:58:56 -0400 Subject: [PATCH 5/7] wpi practice field tests and changes 5-8-26 - RG, JM --- .../org/team157/robot/BuildConstants.java | 10 +++---- .../org/team157/robot/RobotContainer.java | 30 ++++++------------- .../robot/generated/TunerConstants.java | 17 +++++++++-- .../robot/subsystems/intake/Intake.java | 15 +++++++++- .../subsystems/intake/IntakeConstants.java | 11 +++++++ .../robot/subsystems/intake/IntakeIO.java | 12 ++++++++ .../subsystems/intake/IntakeIOTalonFX.java | 14 ++++++++- 7 files changed, 78 insertions(+), 31 deletions(-) diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 441fde1..9c85ff8 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "FRC2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 361; - public static final String GIT_SHA = "4eef33e1932c31a8ad13402e4ba600abdb66e8b8"; - public static final String GIT_DATE = "2026-05-08 07:14:48 EDT"; + public static final int GIT_REVISION = 362; + public static final String GIT_SHA = "73cffc5252654fd848c4d43929d8b0d4bcea36d7"; + public static final String GIT_DATE = "2026-05-08 18:47:12 EDT"; public static final String GIT_BRANCH = "feature/sotm-control"; - public static final String BUILD_DATE = "2026-05-08 12:02:41 EDT"; - public static final long BUILD_UNIX_TIME = 1778256161616L; + public static final String BUILD_DATE = "2026-05-08 20:09:59 EDT"; + public static final long BUILD_UNIX_TIME = 1778285399296L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index d703faf..eb9834e 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -287,7 +287,7 @@ private void configureBindings() { ///////////////////// // Enables dynamic control of the flywheel and hood. driverController.a().toggleOnTrue(flywheel.setDynamicVelocity()); - driverController.a().toggleOnTrue(hood.setDynamicHoodAngle()); + // driverController.a().toggleOnTrue(hood.setDynamicHoodAngle()); //////////////////////////// /// INTAKE UPTAKE HOPPER /// @@ -297,19 +297,12 @@ private void configureBindings() { driverController.rightTrigger().whileTrue(hopper.set(1)); // Runs the hopper, uptake, and intake backwards at a low speed to clear jams. - driverController - .y() - .whileTrue(forceOuttake()); + driverController.y().whileTrue(forceOuttake()); // Wiggles the intake up and down to free up stuck balls - driverController - .x() - .toggleOnTrue(slapdown.wiggleIntake()); + driverController.x().toggleOnTrue(slapdown.wiggleIntake()); // (in/de)creases the ballistic modifier - operatorController - .x() - .or(operatorController.b()) - .onTrue(setModifier()); + operatorController.x().or(operatorController.b()).onTrue(setModifier()); ////////////////////////////////////////////////// /// OPERATOR COMMANDS /// ////////////////////////////////////////////////// @@ -322,10 +315,8 @@ private void configureBindings() { // Disables automatic turret tracking when manual override is enabled, // allowing the operator to control the turret without interference from vision tracking. - turretTrackingTrigger() - .whileTrue(turret.trackTagGlobalRelative()); - turretTrackingTrigger() - .whileTrue(flywheel.setDynamicVelocity()); + turretTrackingTrigger().whileTrue(turret.trackTagGlobalRelative()); + turretTrackingTrigger().whileTrue(flywheel.setDynamicVelocity()); turretTrackingTrigger() .and(driverController.rightTrigger()) .whileTrue(hood.setDynamicHoodAngle()); @@ -369,10 +360,7 @@ private void configureBindings() { // Deploy and retract the intake with the A and Y buttons, but only when the // back button is held to prevent accidental activation during teleop. - operatorController - .a() - .and(operatorController.back()) - .toggleOnTrue(slapdown.deployIntake()); + operatorController.a().and(operatorController.back()).toggleOnTrue(slapdown.deployIntake()); operatorController .y() .and(operatorController.back()) @@ -396,9 +384,9 @@ public double modifySpeed(final double speed) { /** Update the ballistic equation modifier based on the operator's button presses */ public void setBallisticSpeedModifier() { - if (driverController.x().getAsBoolean()) { + if (operatorController.x().getAsBoolean()) { ballisticSpeedModifier = ballisticSpeedModifier + 0.1; - } else if (driverController.b().getAsBoolean()) { + } else if (operatorController.b().getAsBoolean()) { ballisticSpeedModifier = ballisticSpeedModifier - 0.1; } } diff --git a/src/main/java/org/team157/robot/generated/TunerConstants.java b/src/main/java/org/team157/robot/generated/TunerConstants.java index 9ffcb19..8d2fbfe 100644 --- a/src/main/java/org/team157/robot/generated/TunerConstants.java +++ b/src/main/java/org/team157/robot/generated/TunerConstants.java @@ -56,12 +56,11 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120); + private static final Current kSlipCurrent = Amps.of(60); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -72,6 +71,18 @@ public class TunerConstants { // impacting performance. .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true)); + + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can + // set a relatively + // low + // stator current limit to help avoid brownouts without + // impacting performance. + .withStatorCurrentLimit(Amps.of(40)) + .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; diff --git a/src/main/java/org/team157/robot/subsystems/intake/Intake.java b/src/main/java/org/team157/robot/subsystems/intake/Intake.java index 3dd27e1..cf4f75c 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/Intake.java +++ b/src/main/java/org/team157/robot/subsystems/intake/Intake.java @@ -4,6 +4,9 @@ package org.team157.robot.subsystems.intake; +import static edu.wpi.first.units.Units.RPM; + +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -45,13 +48,23 @@ public Command set(double dutycycle) { return io.set(dutycycle); } + /** + * Set the intake to a fixed target angular velocity. + * + * @param speed The target angular velocity. + * @return {@link Command} setting the intake to the specified velocity. + */ + public Command setVelocity(AngularVelocity speed) { + return io.setVelocity(speed); + } + /** * Run the intake at a set speed. Used for autonomous and button bindings. * * @return a {@link Command} running the intake motors at 100% duty cycle */ public Command runIntake() { - return set(1); + return setVelocity(RPM.of(3000)); } @Override diff --git a/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java b/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java index a383293..c414a58 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java @@ -1,6 +1,17 @@ package org.team157.robot.subsystems.intake; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; + public class IntakeConstants { /** Roller motor IDs */ public static final int ROLLER_MOTOR_ID = 14, FOLLOWER_MOTOR_ID = 15; + + public static final double KP = 1, KI = 0, KD = 0; + public static final AngularVelocity ANGULAR_VELOCITY = RPM.of(5800); + public static final AngularAcceleration ANGULAR_ACCELERATION = + RotationsPerSecondPerSecond.of(11600); } diff --git a/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java b/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java index a1c9670..cabb723 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java @@ -1,8 +1,10 @@ package org.team157.robot.subsystems.intake; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import org.littletonrobotics.junction.AutoLog; +import org.team157.robot.subsystems.hood.HoodIO; /** * Defines the input data to be logged by AdvantageKit, along with methods and {@link Command}s @@ -35,6 +37,16 @@ default void updateInputs(IntakeIOInputs inputs) {} /** Updates the values for the simulated version of the intake roller mechanism. */ default void simIterate() {} + /** + * Sets the intake to a fixed target angular velocity. + * + * @param velocity The target angular velocity. + * @return a {@link Command} setting the intake to the specified velocity. + */ + default Command setVelocity(AngularVelocity velocity) { + return Commands.none(); + } + /** * Stops the intake roller. * diff --git a/src/main/java/org/team157/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/org/team157/robot/subsystems/intake/IntakeIOTalonFX.java index cd858f9..3cd5edd 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/org/team157/robot/subsystems/intake/IntakeIOTalonFX.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.Pair; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team157.robot.Constants; @@ -40,7 +41,13 @@ public IntakeIOTalonFX(SubsystemBase subsystem) { SmartMotorControllerConfig intakeMotorConfig = new SmartMotorControllerConfig(subsystem) - .withControlMode(ControlMode.OPEN_LOOP) + .withControlMode(ControlMode.CLOSED_LOOP) + .withClosedLoopController( + IntakeConstants.KP, + IntakeConstants.KI, + IntakeConstants.KD, + IntakeConstants.ANGULAR_VELOCITY, + IntakeConstants.ANGULAR_ACCELERATION) .withGearing(1) .withMotorInverted(false) .withIdleMode(MotorMode.COAST) @@ -83,6 +90,11 @@ public Command set(double dutycycle) { return intake.set(dutycycle); } + @Override + public Command setVelocity(AngularVelocity velocity) { + return intake.setSpeed(velocity); + } + @Override public void simIterate() { intake.simIterate(); From 16c40aa234d9924af57d0f767fa3b4b0a9b8817d Mon Sep 17 00:00:00 2001 From: James Mwangi Date: Fri, 15 May 2026 18:13:24 -0400 Subject: [PATCH 6/7] Miscellaneous Cleanup changes prior to review. --- src/main/java/org/team157/robot/BuildConstants.java | 12 ++++++------ src/main/java/org/team157/robot/Constants.java | 8 +------- .../team157/robot/subsystems/intake/IntakeIO.java | 1 - 3 files changed, 7 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index c98e8ac..efc20dc 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "FRC2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 363; - public static final String GIT_SHA = "5ccb4a0a85f991e41fcc9853d6ef8464cbef77db"; - public static final String GIT_DATE = "2026-05-12 16:58:56 EDT"; + public static final int GIT_REVISION = 374; + public static final String GIT_SHA = "f7f882e99507da8da9d09302734ee9ba52b93c3f"; + public static final String GIT_DATE = "2026-05-15 16:44:03 EDT"; public static final String GIT_BRANCH = "feature/sotm-control"; - public static final String BUILD_DATE = "2026-05-15 16:43:33 EDT"; - public static final long BUILD_UNIX_TIME = 1778877813712L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-05-15 17:43:07 EDT"; + public static final long BUILD_UNIX_TIME = 1778881387494L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/org/team157/robot/Constants.java b/src/main/java/org/team157/robot/Constants.java index cd1a7fd..5051d57 100644 --- a/src/main/java/org/team157/robot/Constants.java +++ b/src/main/java/org/team157/robot/Constants.java @@ -43,7 +43,7 @@ public static enum Mode { public static class ModifierConstants { // Reduces drive speed by this factor when precision mode is active. public static final double PRECISION_DRIVE_MODIFIER = 0.4; - public static final double TRENCH_DRIVE_MODIFIER = 0.6; + public static final double TRENCH_DRIVE_MODIFIER = 0.8; // When true, reduces drive speed by 50%. public static final boolean ROOKIE_MODE = false; public static final double ROOKIE_DRIVE_MODIFIER = 0.5; @@ -51,12 +51,6 @@ public static class ModifierConstants { // Overrides ROOKIE_MODE. public static final boolean DEMO_MODE = false; public static final double DEMO_DRIVE_MODIFIER = 0.25; - /** - * When true, swaps the intake and shooting triggers, per Maya's preference. Makes no change - * to drivetrain speed. For future reference, avoid making catches for specific user edge - * cases like this, make decisions off general consensus. - */ - public static final boolean MAYA_MODE = false; } public static class ControllerConstants { diff --git a/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java b/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java index cabb723..3332fbe 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/org/team157/robot/subsystems/intake/IntakeIO.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import org.littletonrobotics.junction.AutoLog; -import org.team157.robot.subsystems.hood.HoodIO; /** * Defines the input data to be logged by AdvantageKit, along with methods and {@link Command}s From 27d44f19313fc4147b2318da9ba059f8633b7673 Mon Sep 17 00:00:00 2001 From: James Mwangi Date: Fri, 15 May 2026 20:44:26 -0400 Subject: [PATCH 7/7] Address PR comments. --- src/main/java/org/team157/robot/RobotContainer.java | 5 ++--- .../org/team157/robot/subsystems/flywheel/Flywheel.java | 7 ++++--- .../java/org/team157/robot/subsystems/intake/Intake.java | 1 + .../team157/robot/subsystems/intake/IntakeConstants.java | 1 + 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index a4fc8c2..254b21c 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -311,7 +311,6 @@ private void configureBindings() { ///////////////////// // Enables dynamic control of the flywheel and hood. driverController.a().toggleOnTrue(flywheel.setDynamicVelocity()); - // driverController.a().toggleOnTrue(hood.setDynamicHoodAngle()); //////////////////////////// /// INTAKE UPTAKE HOPPER /// @@ -409,9 +408,9 @@ public double modifySpeed(final double speed) { /** Update the ballistic equation modifier based on the operator's button presses */ public void setBallisticSpeedModifier() { if (operatorController.x().getAsBoolean()) { - ballisticSpeedModifier = ballisticSpeedModifier + 0.1; + ballisticSpeedModifier = ballisticSpeedModifier + 0.05; } else if (operatorController.b().getAsBoolean()) { - ballisticSpeedModifier = ballisticSpeedModifier - 0.1; + ballisticSpeedModifier = ballisticSpeedModifier - 0.05; } } diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java index 6cb95bd..3677e5b 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java @@ -188,9 +188,10 @@ public void setShotParams(double height, double distance) { } /** - * Gets the desired flywheel velocity for the current shot, recalculating shot parameters each - * time it is called. - * + * Gets the desired flywheel velocity for the current shot, + * recalculating shot parameters each time it is called. + * The result of this calculation is multiplied by a modifier controlled by the operator. + * * @return The desired angular velocity of the flywheel. */ public AngularVelocity getDesiredFlywheelVelocity() { diff --git a/src/main/java/org/team157/robot/subsystems/intake/Intake.java b/src/main/java/org/team157/robot/subsystems/intake/Intake.java index cf4f75c..6872a5f 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/Intake.java +++ b/src/main/java/org/team157/robot/subsystems/intake/Intake.java @@ -64,6 +64,7 @@ public Command setVelocity(AngularVelocity speed) { * @return a {@link Command} running the intake motors at 100% duty cycle */ public Command runIntake() { + // Arbitrary untuned value return setVelocity(RPM.of(3000)); } diff --git a/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java b/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java index c414a58..e46ba7f 100644 --- a/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/org/team157/robot/subsystems/intake/IntakeConstants.java @@ -10,6 +10,7 @@ public class IntakeConstants { /** Roller motor IDs */ public static final int ROLLER_MOTOR_ID = 14, FOLLOWER_MOTOR_ID = 15; + //TODO: tune or consider going back to open loop control public static final double KP = 1, KI = 0, KD = 0; public static final AngularVelocity ANGULAR_VELOCITY = RPM.of(5800); public static final AngularAcceleration ANGULAR_ACCELERATION =