From 32635180130b5f082c99c13f689f7f82e1c7bd52 Mon Sep 17 00:00:00 2001 From: fishnos Date: Tue, 27 Jan 2026 15:01:15 -0500 Subject: [PATCH 01/13] Refactored auto util. --- .../robot/commands/autos/tower/ScoreL1.java | 19 +++- .../java/frc/robot/constants/Constants.java | 46 -------- .../robot/lib/util/IntermediateGenerator.java | 104 ++++++++++++++++++ 3 files changed, 118 insertions(+), 51 deletions(-) create mode 100644 src/main/java/frc/robot/lib/util/IntermediateGenerator.java diff --git a/src/main/java/frc/robot/commands/autos/tower/ScoreL1.java b/src/main/java/frc/robot/commands/autos/tower/ScoreL1.java index febeafe..0d1908d 100644 --- a/src/main/java/frc/robot/commands/autos/tower/ScoreL1.java +++ b/src/main/java/frc/robot/commands/autos/tower/ScoreL1.java @@ -18,6 +18,7 @@ import frc.robot.lib.BLine.Path; import frc.robot.lib.BLine.Path.TranslationTarget; import frc.robot.lib.BLine.Path.Waypoint; +import frc.robot.lib.util.IntermediateGenerator; import frc.robot.lib.util.RebelUtil; import frc.robot.subsystems.swerve.SwerveDrive; import frc.robot.subsystems.swerve.SwerveDrive.CurrentSystemState; @@ -26,11 +27,17 @@ public class ScoreL1 extends Command { private final SwerveDrive swerveDrive; private final RobotState robotState; + private final IntermediateGenerator.GeneratorMode intermediateMode; private boolean isFinished = false; public ScoreL1() { + this(IntermediateGenerator.GeneratorMode.DEFAULT); + } + + public ScoreL1(IntermediateGenerator.GeneratorMode intermediateMode) { this.swerveDrive = SwerveDrive.getInstance(); this.robotState = RobotState.getInstance(); + this.intermediateMode = intermediateMode; } @Override @@ -53,7 +60,6 @@ public void initialize() { Logger.recordOutput("ScoreL1/Status", "Running"); - // note: i cant see this working without a pose flip Pose2d currentPoseBlue = RebelUtil.applyFlip(currentPose); Pose2d towerPoseBlue = Constants.ScoreTowerConstants.TOWER_WAYPOINTS[0]; double minDistanceToTower = towerPoseBlue.getTranslation().getDistance(currentPoseBlue.getTranslation()); @@ -74,11 +80,14 @@ public void initialize() { List pathElements = new ArrayList<>(); pathElements.add(new Waypoint(currentPoseBlue)); - boolean needsIntermediate = Constants.ScoreTowerConstants.shouldUseIntermediate(currentPoseBlue, towerPoseBlue); - Translation2d[] intermediateTranslation = Constants.ScoreTowerConstants.intermediateTranslation( + boolean needsIntermediate = IntermediateGenerator.shouldUseIntermediate( + intermediateMode, + currentPoseBlue, + towerPoseBlue); + Translation2d[] intermediateTranslation = IntermediateGenerator.intermediateTranslations( + intermediateMode, currentPoseBlue, - towerPoseIndex - ); + towerPoseIndex); if (needsIntermediate) { for (int i = intermediateTranslation.length - 1; i >= 0; i--) { diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 819eabc..b90f628 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.configs.SwerveConfig; -import frc.robot.constants.AlignmentConstants.Tower; import frc.robot.lib.util.ConfigLoader; /** @@ -122,51 +121,6 @@ public static final class ScoreTowerConstants { Rotation2d.fromDegrees(180.0)) }; - public static boolean shouldUseIntermediate(Pose2d currentPoseBlue, Pose2d towerPoseBlue) { - return isBehindTower(currentPoseBlue, towerPoseBlue) || isInsideTower(currentPoseBlue); - } - - public static Translation2d[] intermediateTranslation(Pose2d currentPoseBlue, int towerPoseIndex) { - if (shouldUseIntermediate(currentPoseBlue, TOWER_WAYPOINTS[towerPoseIndex])) { - // left - if (towerPoseIndex == 0) { - return new Translation2d[] { - new Translation2d( - TOWER_WAYPOINTS[towerPoseIndex].getX() + INTERMEDIATE_CLEARANCE_METERS.getX(), - Tower.towerPoses[towerPoseIndex].getY() - INTERMEDIATE_CLEARANCE_METERS.getY()), - isInsideTower(currentPoseBlue) ? new Translation2d( - RobotConstants.robotWidth / 2.0 + 0.1, - Tower.towerPoses[towerPoseIndex].getY() - INTERMEDIATE_CLEARANCE_METERS.getY()) : currentPoseBlue.getTranslation() - }; - // middle and right - } else { - return new Translation2d[] { - new Translation2d( - Tower.towerPoses[2].getX() + INTERMEDIATE_CLEARANCE_METERS.getX(), - Tower.towerPoses[towerPoseIndex].getY() + INTERMEDIATE_CLEARANCE_METERS.getY()), - isInsideTower(currentPoseBlue) ? new Translation2d( - RobotConstants.robotWidth / 2.0 + 0.1, - Tower.towerPoses[towerPoseIndex].getY() + INTERMEDIATE_CLEARANCE_METERS.getY()) : currentPoseBlue.getTranslation() - }; - } - } - - return new Translation2d[]{}; - } - - public static boolean isInsideTower(Pose2d currentPoseBlue) { - double robotRadius = RobotConstants.robotWidth / 2.0; - - return currentPoseBlue.getX() < Tower.towerPoses[0].getX() && - currentPoseBlue.getY() > (Tower.towerPoses[1].getY() - Tower.towerWidth / 2.0 - robotRadius) && - currentPoseBlue.getY() < (Tower.towerPoses[1].getY() + Tower.towerWidth / 2.0 + robotRadius); - } - - public static boolean isBehindTower(Pose2d currentPoseBlue, Pose2d towerPoseBlue) { - return (currentPoseBlue.getX() < towerPoseBlue.getX() + - RobotConstants.robotWidth / 2.0); - } - private ScoreTowerConstants() {} } diff --git a/src/main/java/frc/robot/lib/util/IntermediateGenerator.java b/src/main/java/frc/robot/lib/util/IntermediateGenerator.java new file mode 100644 index 0000000..745496a --- /dev/null +++ b/src/main/java/frc/robot/lib/util/IntermediateGenerator.java @@ -0,0 +1,104 @@ +package frc.robot.lib.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.constants.AlignmentConstants.Tower; +import frc.robot.constants.Constants; + +public final class IntermediateGenerator { + public enum GeneratorMode { + DEFAULT + } + + private IntermediateGenerator() {} + + public static boolean shouldUseIntermediate( + GeneratorMode mode, + Pose2d currentPoseBlue, + Pose2d towerPoseBlue) { + return strategyFor(mode).shouldUseIntermediate(currentPoseBlue, towerPoseBlue); + } + + public static Translation2d[] intermediateTranslations( + GeneratorMode mode, + Pose2d currentPoseBlue, + int towerPoseIndex) { + return strategyFor(mode).intermediateTranslations(currentPoseBlue, towerPoseIndex); + } + + private static Strategy strategyFor(GeneratorMode mode) { + switch (mode) { + case DEFAULT: + default: + return DefaultStrategy.INSTANCE; + } + } + + private interface Strategy { + boolean shouldUseIntermediate(Pose2d currentPoseBlue, Pose2d towerPoseBlue); + + Translation2d[] intermediateTranslations(Pose2d currentPoseBlue, int towerPoseIndex); + } + + private static final class DefaultStrategy implements Strategy { + private static final DefaultStrategy INSTANCE = new DefaultStrategy(); + + @Override + public boolean shouldUseIntermediate(Pose2d currentPoseBlue, Pose2d towerPoseBlue) { + return isBehindTower(currentPoseBlue, towerPoseBlue) || isInsideTower(currentPoseBlue); + } + + @Override + public Translation2d[] intermediateTranslations(Pose2d currentPoseBlue, int towerPoseIndex) { + if (shouldUseIntermediate(currentPoseBlue, Constants.ScoreTowerConstants.TOWER_WAYPOINTS[towerPoseIndex])) { + // left + if (towerPoseIndex == 0) { + return new Translation2d[] { + new Translation2d( + Constants.ScoreTowerConstants.TOWER_WAYPOINTS[towerPoseIndex].getX() + + Constants.ScoreTowerConstants.INTERMEDIATE_CLEARANCE_METERS.getX(), + Tower.towerPoses[towerPoseIndex].getY() + - Constants.ScoreTowerConstants.INTERMEDIATE_CLEARANCE_METERS.getY()), + isInsideTower(currentPoseBlue) + ? new Translation2d( + Constants.RobotConstants.robotWidth / 2.0 + 0.1, + Tower.towerPoses[towerPoseIndex].getY() + - Constants.ScoreTowerConstants.INTERMEDIATE_CLEARANCE_METERS.getY()) + : currentPoseBlue.getTranslation() + }; + // middle and right + } else { + return new Translation2d[] { + new Translation2d( + Tower.towerPoses[2].getX() + + Constants.ScoreTowerConstants.INTERMEDIATE_CLEARANCE_METERS.getX(), + Tower.towerPoses[towerPoseIndex].getY() + + Constants.ScoreTowerConstants.INTERMEDIATE_CLEARANCE_METERS.getY()), + isInsideTower(currentPoseBlue) + ? new Translation2d( + Constants.RobotConstants.robotWidth / 2.0 + 0.1, + Tower.towerPoses[towerPoseIndex].getY() + + Constants.ScoreTowerConstants.INTERMEDIATE_CLEARANCE_METERS.getY()) + : currentPoseBlue.getTranslation() + }; + } + } + + return new Translation2d[] {}; + } + + private boolean isInsideTower(Pose2d currentPoseBlue) { + double robotRadius = Constants.RobotConstants.robotWidth / 2.0; + + return currentPoseBlue.getX() < Tower.towerPoses[0].getX() + && currentPoseBlue.getY() + > (Tower.towerPoses[1].getY() - Tower.towerWidth / 2.0 - robotRadius) + && currentPoseBlue.getY() + < (Tower.towerPoses[1].getY() + Tower.towerWidth / 2.0 + robotRadius); + } + + private boolean isBehindTower(Pose2d currentPoseBlue, Pose2d towerPoseBlue) { + return (currentPoseBlue.getX() < towerPoseBlue.getX() + Constants.RobotConstants.robotWidth / 2.0); + } + } +} From ed5cd64643b1f01e9371c4da83d7fb50b46e0c4b Mon Sep 17 00:00:00 2001 From: fishnos Date: Wed, 28 Jan 2026 20:05:18 -0500 Subject: [PATCH 02/13] Initial autos. Need to get critique. --- src/main/deploy/autos/paths/U.json | 55 ------- src/main/deploy/autos/paths/alge_weave.json | 116 --------------- src/main/deploy/autos/paths/blinexample.json | 55 ------- .../autos/paths/bottom_start_to_outpust.json | 48 ++++++ ...json => bottom_start_to_output_shoot.json} | 69 ++++----- src/main/deploy/autos/paths/corr_sweep.json | 95 ------------ src/main/deploy/autos/paths/corrigan.json | 90 ----------- src/main/deploy/autos/paths/event_test.json | 33 ----- src/main/deploy/autos/paths/lknkn.json | 140 ------------------ ...e.json => middle_start_to_tower_left.json} | 19 +-- .../paths/middle_start_to_tower_middle.json | 37 +++++ ....json => middle_start_to_tower_right.json} | 18 +-- src/main/deploy/autos/paths/square.json | 80 ---------- .../deploy/autos/paths/top left sweep.json | 140 ------------------ .../autos/paths/top_start_to_depot_lob.json | 66 +++++++++ .../autos/paths/top_start_to_depot_shoot.json | 66 +++++++++ 16 files changed, 264 insertions(+), 863 deletions(-) delete mode 100644 src/main/deploy/autos/paths/U.json delete mode 100644 src/main/deploy/autos/paths/alge_weave.json delete mode 100644 src/main/deploy/autos/paths/blinexample.json create mode 100644 src/main/deploy/autos/paths/bottom_start_to_outpust.json rename src/main/deploy/autos/paths/{climb auto.json => bottom_start_to_output_shoot.json} (52%) delete mode 100644 src/main/deploy/autos/paths/corr_sweep.json delete mode 100644 src/main/deploy/autos/paths/corrigan.json delete mode 100644 src/main/deploy/autos/paths/event_test.json delete mode 100644 src/main/deploy/autos/paths/lknkn.json rename src/main/deploy/autos/paths/{drivescore.json => middle_start_to_tower_left.json} (65%) create mode 100644 src/main/deploy/autos/paths/middle_start_to_tower_middle.json rename src/main/deploy/autos/paths/{new_path.json => middle_start_to_tower_right.json} (51%) delete mode 100644 src/main/deploy/autos/paths/square.json delete mode 100644 src/main/deploy/autos/paths/top left sweep.json create mode 100644 src/main/deploy/autos/paths/top_start_to_depot_lob.json create mode 100644 src/main/deploy/autos/paths/top_start_to_depot_shoot.json diff --git a/src/main/deploy/autos/paths/U.json b/src/main/deploy/autos/paths/U.json deleted file mode 100644 index 5e42b25..0000000 --- a/src/main/deploy/autos/paths/U.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 8.0, - "y_meters": 2.2, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 5.673797108880322, - "y_meters": 2.2909948998216887, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 5.077803782831055, - "y_meters": 3.1699697515410676, - "intermediate_handoff_radius_meters": 0.35 - }, - { - "type": "translation", - "x_meters": 5.539143728593179, - "y_meters": 3.9497853136869683, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.953428333073189, - "y_meters": 4.064811390926694, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 4.0, - "start_ordinal": 0, - "end_ordinal": 4 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/alge_weave.json b/src/main/deploy/autos/paths/alge_weave.json deleted file mode 100644 index ced4f72..0000000 --- a/src/main/deploy/autos/paths/alge_weave.json +++ /dev/null @@ -1,116 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 1.1927847553561772, - "y_meters": 6.425944525866314, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 1.801228598687076, - "y_meters": 5.974815160395581, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 1.656400056307944, - "y_meters": 5.371795277663214, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 0.803169839171657, - "y_meters": 4.432120796908524, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 1.6038396692659045, - "profiled_rotation": false - } - }, - { - "type": "translation", - "x_meters": 0.6363482494068751, - "y_meters": 3.6826795118055813, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 1.7046103312732073, - "y_meters": 2.3843589560997422, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 1.812483937419045, - "y_meters": 1.7899326063064587, - "intermediate_handoff_radius_meters": 0.35 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 1.0943851018973976, - "y_meters": 1.2456605762473654, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.23490872826353779, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 13.055266236434294, - "y_meters": 5.34714639030153, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -3.141592653589793, - "profiled_rotation": false - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 3.0, - "start_ordinal": 0, - "end_ordinal": 2 - }, - { - "value": 4.5, - "start_ordinal": 3, - "end_ordinal": 3 - }, - { - "value": 3.0, - "start_ordinal": 4, - "end_ordinal": 4 - }, - { - "value": 4.5, - "start_ordinal": 5, - "end_ordinal": 5 - }, - { - "value": 2.5999999999999996, - "start_ordinal": 6, - "end_ordinal": 7 - }, - { - "value": 4.5, - "start_ordinal": 8, - "end_ordinal": 8 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/blinexample.json b/src/main/deploy/autos/paths/blinexample.json deleted file mode 100644 index 6b8ca81..0000000 --- a/src/main/deploy/autos/paths/blinexample.json +++ /dev/null @@ -1,55 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.740817252022808, - "y_meters": 2.187150386507378, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 2.166694550886783, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 3.07774834803718, - "y_meters": 2.552280393534792, - "intermediate_handoff_radius_meters": 0.49999999999999994 - }, - { - "type": "translation", - "x_meters": 2.5827518617406233, - "y_meters": 2.912603654251586, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 2.599181939042099, - "y_meters": 3.4356535488404836, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.1194349256900247, - "y_meters": 3.59357343640197, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.01367537593373334, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 2.0, - "start_ordinal": 2, - "end_ordinal": 4 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/bottom_start_to_outpust.json b/src/main/deploy/autos/paths/bottom_start_to_outpust.json new file mode 100644 index 0000000..7dac895 --- /dev/null +++ b/src/main/deploy/autos/paths/bottom_start_to_outpust.json @@ -0,0 +1,48 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.712899751449876, + "y_meters": 2.050045567522784, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -2.356194490192345, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 2.1669875724937864, + "y_meters": 1.0764084507042266, + "intermediate_handoff_radius_meters": 0.25 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.13488141309989543, + "y_meters": 0.5677920463960238, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": 3.141592653589793, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 1.8, + "start_ordinal": 0, + "end_ordinal": 1 + }, + { + "value": 3.5, + "start_ordinal": 2, + "end_ordinal": 2 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/climb auto.json b/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json similarity index 52% rename from src/main/deploy/autos/paths/climb auto.json rename to src/main/deploy/autos/paths/bottom_start_to_output_shoot.json index 87fa7f8..4404712 100644 --- a/src/main/deploy/autos/paths/climb auto.json +++ b/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json @@ -3,72 +3,78 @@ { "type": "waypoint", "translation_target": { - "x_meters": 4.5170921429667645, - "y_meters": 3.9074246351115107, + "x_meters": 0.14941331036584227, + "y_meters": 0.5968558409279208, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": 0.007100252321236427, + "rotation_radians": 3.141592653589793, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 1.102352612218736, - "y_meters": 4.159371845586849, + "x_meters": 2.9371781275890623, + "y_meters": 2.398811101905552, "intermediate_handoff_radius_meters": 0.25 }, { "type": "waypoint", "translation_target": { - "x_meters": 0.38472798346075665, - "y_meters": 4.167770085936025, + "x_meters": 3.712899751449874, + "y_meters": 2.5441300745650377, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -1.5226933581299462, + "rotation_radians": 1.5707963267948966, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 0.37632974311157874, - "y_meters": 5.217120221872825, + "x_meters": 5.409960120639246, + "y_meters": 2.544130074565034, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -1.5789112256089768, + "rotation_radians": 1.5707963267948966, "profiled_rotation": true } }, { - "type": "translation", - "x_meters": 1.001143820318155, - "y_meters": 5.2843061446662505, - "intermediate_handoff_radius_meters": 0.2 + "type": "waypoint", + "translation_target": { + "x_meters": 7.252339462488104, + "y_meters": 3.299788732394365, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -0.7838745474520066, + "profiled_rotation": true + } }, { "type": "waypoint", "translation_target": { - "x_meters": 1.414329743111578, - "y_meters": 3.680242237973255, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 7.874039433997281, + "y_meters": 2.5877257663628823, + "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": 0.0, + "rotation_radians": -2.7803347073460016, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 0.9016071741012749, - "y_meters": 3.6970387186716103, + "x_meters": 7.158804857039177, + "y_meters": 1.2943869096934515, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -0.030492344018781738, + "rotation_radians": 2.8008070395117843, "profiled_rotation": true } } @@ -76,24 +82,19 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 2.0, - "start_ordinal": 2, - "end_ordinal": 3 - }, - { - "value": 2.0, + "value": 4.5, "start_ordinal": 0, - "end_ordinal": 1 + "end_ordinal": 2 }, { - "value": 0.5, - "start_ordinal": 6, - "end_ordinal": 6 + "value": 2.5, + "start_ordinal": 3, + "end_ordinal": 3 }, { - "value": 2.0, + "value": 3.5, "start_ordinal": 4, - "end_ordinal": 5 + "end_ordinal": 6 } ] } diff --git a/src/main/deploy/autos/paths/corr_sweep.json b/src/main/deploy/autos/paths/corr_sweep.json deleted file mode 100644 index a28d2fb..0000000 --- a/src/main/deploy/autos/paths/corr_sweep.json +++ /dev/null @@ -1,95 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.5437456078706946, - "y_meters": 0.5660049191848167, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -3.1245144458096408, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 6.023952021531309, - "y_meters": 0.5783309908643748, - "intermediate_handoff_radius_meters": 0.44999999999999996 - }, - { - "type": "translation", - "x_meters": 6.899999999999997, - "y_meters": 2.131416022487702, - "intermediate_handoff_radius_meters": 0.49999999999999994 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.7007686876586146, - "y_meters": 2.142141673614087, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 3.1412749105380193, - "profiled_rotation": true - } - }, - { - "type": "rotation", - "rotation_radians": 3.138404615121565, - "t_ratio": 0.5004112379100598, - "profiled_rotation": true - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.946418833450457, - "y_meters": 2.131112438510189, - "intermediate_handoff_radius_meters": 0.3 - }, - "rotation_target": { - "rotation_radians": 2.3491646521132608, - "profiled_rotation": true - } - }, - { - "type": "rotation", - "rotation_radians": 2.2421911212138683, - "t_ratio": 0.6924687836352978, - "profiled_rotation": true - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 1.0580628359075142, - "y_meters": 3.006799162626444, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.9513833837615121, - "profiled_rotation": false - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 3.0, - "start_ordinal": 0, - "end_ordinal": 2 - }, - { - "value": 1.2, - "start_ordinal": 5, - "end_ordinal": 5 - }, - { - "value": 2.0, - "start_ordinal": 3, - "end_ordinal": 3 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/corrigan.json b/src/main/deploy/autos/paths/corrigan.json deleted file mode 100644 index a78d915..0000000 --- a/src/main/deploy/autos/paths/corrigan.json +++ /dev/null @@ -1,90 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.0, - "y_meters": 5.0, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.86, - "y_meters": 3.99, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.282990886423841, - "y_meters": 2.7479858233259753, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 3.4074301794278763, - "y_meters": 2.039554643760937, - "intermediate_handoff_radius_meters": 0.2 - }, - { - "type": "translation", - "x_meters": 4.923092148381162, - "y_meters": 2.0525556563805125, - "intermediate_handoff_radius_meters": 0.2 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.79, - "y_meters": 2.291, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.33161255787892263, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 2.0, - "start_ordinal": 3, - "end_ordinal": 5 - }, - { - "value": 2.0, - "start_ordinal": 0, - "end_ordinal": 1 - }, - { - "value": 2.5, - "start_ordinal": 2, - "end_ordinal": 2 - } - ], - "max_acceleration_meters_per_sec2": [ - { - "value": 10.0, - "start_ordinal": 0, - "end_ordinal": 0 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/event_test.json b/src/main/deploy/autos/paths/event_test.json deleted file mode 100644 index aeef99f..0000000 --- a/src/main/deploy/autos/paths/event_test.json +++ /dev/null @@ -1,33 +0,0 @@ -{ - "path_elements": [ - { - "type": "translation", - "x_meters": 10.127602211778514, - "y_meters": 4.759861696256541, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "event_trigger", - "t_ratio": 0.5000000000000009, - "lib_key": "test1" - }, - { - "type": "translation", - "x_meters": 11.138374221808974, - "y_meters": 4.753224026238958, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 13.271779774206143, - "y_meters": 3.659848910751931, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -1.457700607887588, - "profiled_rotation": true - } - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/lknkn.json b/src/main/deploy/autos/paths/lknkn.json deleted file mode 100644 index 72d6f53..0000000 --- a/src/main/deploy/autos/paths/lknkn.json +++ /dev/null @@ -1,140 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.568090983509657, - "y_meters": 5.30993077342423, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -3.141592653589793, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.3361937032803906, - "y_meters": 6.708578314055129, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -0.0856277532637236, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 3.4217540588607287, - "y_meters": 7.182178798268579, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 3.8890419877446614, - "y_meters": 7.395876577730745, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.364504395444601, - "y_meters": 7.496517563946532, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 1.455034129857985, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 6.900562772593829, - "y_meters": 7.323249094112343, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.054809308003093, - "y_meters": 6.861199841221172, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -2.2698549165663313, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.991826177539658, - "y_meters": 5.5325279527833615, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 6.6846446681625045, - "y_meters": 5.168664166131563, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 6.210507244280676, - "y_meters": 5.0993567781978895, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.60541984994169, - "y_meters": 5.28726308571899, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -3.1331460120377237, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 3.5, - "start_ordinal": 0, - "end_ordinal": 1 - }, - { - "value": 3.0, - "start_ordinal": 2, - "end_ordinal": 3 - }, - { - "value": 3.5, - "start_ordinal": 4, - "end_ordinal": 4 - }, - { - "value": 3.0, - "start_ordinal": 5, - "end_ordinal": 6 - }, - { - "value": 3.5, - "start_ordinal": 7, - "end_ordinal": 7 - }, - { - "value": 3.0, - "start_ordinal": 8, - "end_ordinal": 9 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/drivescore.json b/src/main/deploy/autos/paths/middle_start_to_tower_left.json similarity index 65% rename from src/main/deploy/autos/paths/drivescore.json rename to src/main/deploy/autos/paths/middle_start_to_tower_left.json index f0a814c..0688884 100644 --- a/src/main/deploy/autos/paths/drivescore.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_left.json @@ -3,20 +3,20 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.0, - "y_meters": 2.0, + "x_meters": 3.712899751449874, + "y_meters": 3.3143206296603154, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": 0.0, + "rotation_radians": 0.7853981633974483, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 5.399999999999999, - "y_meters": 2.0, + "x_meters": 1.2538375025779391, + "y_meters": 3.299788732394365, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { @@ -26,16 +26,9 @@ } ], "constraints": { - "max_acceleration_meters_per_sec2": [ - { - "value": 14.0, - "start_ordinal": 0, - "end_ordinal": 1 - } - ], "max_velocity_meters_per_sec": [ { - "value": 4.5, + "value": 4.0, "start_ordinal": 0, "end_ordinal": 1 } diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_middle.json b/src/main/deploy/autos/paths/middle_start_to_tower_middle.json new file mode 100644 index 0000000..7626e22 --- /dev/null +++ b/src/main/deploy/autos/paths/middle_start_to_tower_middle.json @@ -0,0 +1,37 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.7128997514498776, + "y_meters": 3.677618061309034, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": 0.7853981633974483, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.219242639280508, + "y_meters": 3.7066818558409267, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 4.0, + "start_ordinal": 0, + "end_ordinal": 1 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/new_path.json b/src/main/deploy/autos/paths/middle_start_to_tower_right.json similarity index 51% rename from src/main/deploy/autos/paths/new_path.json rename to src/main/deploy/autos/paths/middle_start_to_tower_right.json index 188a773..247494f 100644 --- a/src/main/deploy/autos/paths/new_path.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_right.json @@ -3,30 +3,24 @@ { "type": "waypoint", "translation_target": { - "x_meters": 1.2200363909133334, - "y_meters": 2.193815194465836, + "x_meters": 3.6983678541839247, + "y_meters": 4.171702568351285, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -0.5883563397944677, + "rotation_radians": 0.7853981633974483, "profiled_rotation": true } }, - { - "type": "rotation", - "rotation_radians": 0.0, - "t_ratio": 0.5000000000000003, - "profiled_rotation": true - }, { "type": "waypoint", "translation_target": { - "x_meters": 4.477999999999999, - "y_meters": 3.1134933797940105, + "x_meters": 1.2337745365464536, + "y_meters": 4.157170671085335, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -0.2708130020111995, + "rotation_radians": 0.0, "profiled_rotation": true } } diff --git a/src/main/deploy/autos/paths/square.json b/src/main/deploy/autos/paths/square.json deleted file mode 100644 index 5651448..0000000 --- a/src/main/deploy/autos/paths/square.json +++ /dev/null @@ -1,80 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 0.0, - "y_meters": 1.0, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -1.5707963267948966, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 0.0, - "y_meters": 0.0, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 1.0, - "y_meters": 0.0, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 1.5707963267948966, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 1.0, - "y_meters": 1.0, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 3.141592653589793, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 0.0, - "y_meters": 1.0, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -1.5707963267948966, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 1.0, - "start_ordinal": 0, - "end_ordinal": 4 - } - ], - "max_acceleration_meters_per_sec2": [ - { - "value": 9.0, - "start_ordinal": 0, - "end_ordinal": 0 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/top left sweep.json b/src/main/deploy/autos/paths/top left sweep.json deleted file mode 100644 index d48f9ea..0000000 --- a/src/main/deploy/autos/paths/top left sweep.json +++ /dev/null @@ -1,140 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.4095000218474345, - "y_meters": 6.253298931380595, - "intermediate_handoff_radius_meters": 0.15000000000000002 - }, - "rotation_target": { - "rotation_radians": -0.9793535244808479, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 2.977857535430647, - "y_meters": 5.897935790683703, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 2.8849329552794285, - "y_meters": 6.907231930354891, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 3.1330893783958023, - "y_meters": 7.331220763517274, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.6466173173372796, - "y_meters": 7.5513688115054345, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 6.745145299973625, - "y_meters": 7.530452606482431, - "intermediate_handoff_radius_meters": 0.2 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.48983265222326, - "y_meters": 7.465223555226681, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 1.5847418961152384, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 7.493229293433003, - "y_meters": 6.873405103727691, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "translation", - "x_meters": 7.486298706737078, - "y_meters": 4.981273399426488, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.435464126589943, - "y_meters": 4.008083838671865, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": 1.5687792814617847, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 5.350841380621367, - "y_meters": 5.581734700216863, - "intermediate_handoff_radius_meters": 0.25 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.3966767371642934, - "y_meters": 5.519212543176577, - "intermediate_handoff_radius_meters": 0.25 - }, - "rotation_target": { - "rotation_radians": -0.9513412642845325, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 4.5, - "start_ordinal": 5, - "end_ordinal": 5 - }, - { - "value": 2.0, - "start_ordinal": 6, - "end_ordinal": 6 - }, - { - "value": 3.0, - "start_ordinal": 7, - "end_ordinal": 8 - }, - { - "value": 2.0, - "start_ordinal": 9, - "end_ordinal": 10 - }, - { - "value": 1.3, - "start_ordinal": 11, - "end_ordinal": 11 - }, - { - "value": 2.2, - "start_ordinal": 0, - "end_ordinal": 4 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/top_start_to_depot_lob.json b/src/main/deploy/autos/paths/top_start_to_depot_lob.json new file mode 100644 index 0000000..316db18 --- /dev/null +++ b/src/main/deploy/autos/paths/top_start_to_depot_lob.json @@ -0,0 +1,66 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.7419635459817737, + "y_meters": 5.465041425020713, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -2.408388728109846, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.5272426392805076, + "y_meters": 5.4650414250207096, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": 2.1225880267303348, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.2624968775668153, + "y_meters": 6.104444904722449, + "intermediate_handoff_radius_meters": 0.28 + }, + "rotation_target": { + "rotation_radians": 0.9950917834394732, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.38145296704486, + "y_meters": 6.874635459817724, + "intermediate_handoff_radius_meters": 0.35 + }, + "rotation_target": { + "rotation_radians": -0.7841533915823613, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 1.5, + "start_ordinal": 0, + "end_ordinal": 1 + }, + { + "value": 3.2, + "start_ordinal": 2, + "end_ordinal": 3 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json b/src/main/deploy/autos/paths/top_start_to_depot_shoot.json new file mode 100644 index 0000000..018f1da --- /dev/null +++ b/src/main/deploy/autos/paths/top_start_to_depot_shoot.json @@ -0,0 +1,66 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.6983678541839273, + "y_meters": 6.86010356255178, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -2.437343434535229, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.2511365912274852, + "y_meters": 6.5113380281690105, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.9101277800434233, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.23343308303491905, + "y_meters": 5.479573322286662, + "intermediate_handoff_radius_meters": 0.28 + }, + "rotation_target": { + "rotation_radians": -0.23221185544933898, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.2070701998534779, + "y_meters": 4.974128528905077, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -0.3267760852994502, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 1.5, + "start_ordinal": 0, + "end_ordinal": 1 + }, + { + "value": 3.5, + "start_ordinal": 2, + "end_ordinal": 3 + } + ] + } +} \ No newline at end of file From 3973dfb55f0ef13062dd35b19371c78f4181b24e Mon Sep 17 00:00:00 2001 From: fishnos Date: Sat, 31 Jan 2026 17:38:37 -0500 Subject: [PATCH 03/13] Shooting while moving implementation! --- simgui-ds.json | 8 + .../autos/paths/bottom_start_to_outpust.json | 16 +- .../paths/bottom_start_to_output_shoot.json | 27 ++- .../paths/middle_start_to_tower_left.json | 12 +- .../paths/middle_start_to_tower_middle.json | 12 +- .../paths/middle_start_to_tower_right.json | 21 +- .../autos/paths/top_start_to_depot_lob.json | 12 +- .../autos/paths/top_start_to_depot_shoot.json | 12 +- src/main/java/frc/robot/RobotContainer.java | 67 ++++-- src/main/java/frc/robot/RobotState.java | 215 +++++++++--------- .../autos/AutoShootingController.java | 96 ++++++++ .../autos/AutoShootingZoneManager.java | 70 ++++++ 12 files changed, 425 insertions(+), 143 deletions(-) create mode 100644 src/main/java/frc/robot/commands/autos/AutoShootingController.java create mode 100644 src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..fe7985a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,13 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + {}, + {}, + {}, + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/deploy/autos/paths/bottom_start_to_outpust.json b/src/main/deploy/autos/paths/bottom_start_to_outpust.json index 7dac895..cdc10ef 100644 --- a/src/main/deploy/autos/paths/bottom_start_to_outpust.json +++ b/src/main/deploy/autos/paths/bottom_start_to_outpust.json @@ -12,10 +12,20 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": -0.0, + "lib_key": "shooting_zone_0_start" + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "translation", - "x_meters": 2.1669875724937864, - "y_meters": 1.0764084507042266, + "x_meters": 1.1729999999999996, + "y_meters": 0.7539999999999998, "intermediate_handoff_radius_meters": 0.25 }, { @@ -34,7 +44,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 1.8, + "value": 1.95, "start_ordinal": 0, "end_ordinal": 1 }, diff --git a/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json b/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json index 4404712..77b468e 100644 --- a/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json +++ b/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json @@ -12,9 +12,19 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "shooting_zone_0_start" + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "translation", - "x_meters": 2.9371781275890623, + "x_meters": 2.5884125932062942, "y_meters": 2.398811101905552, "intermediate_handoff_radius_meters": 0.25 }, @@ -82,19 +92,24 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 4.5, + "value": 1.8, "start_ordinal": 0, - "end_ordinal": 2 + "end_ordinal": 1 }, { - "value": 2.5, - "start_ordinal": 3, - "end_ordinal": 3 + "value": 4.5, + "start_ordinal": 2, + "end_ordinal": 2 }, { "value": 3.5, "start_ordinal": 4, "end_ordinal": 6 + }, + { + "value": 3.0, + "start_ordinal": 3, + "end_ordinal": 3 } ] } diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_left.json b/src/main/deploy/autos/paths/middle_start_to_tower_left.json index 0688884..acd033f 100644 --- a/src/main/deploy/autos/paths/middle_start_to_tower_left.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_left.json @@ -12,6 +12,16 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "shooting_zone_0_start" + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "waypoint", "translation_target": { @@ -28,7 +38,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 4.0, + "value": 2.5, "start_ordinal": 0, "end_ordinal": 1 } diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_middle.json b/src/main/deploy/autos/paths/middle_start_to_tower_middle.json index 7626e22..eb49075 100644 --- a/src/main/deploy/autos/paths/middle_start_to_tower_middle.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_middle.json @@ -12,6 +12,16 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "" + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "waypoint", "translation_target": { @@ -28,7 +38,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 4.0, + "value": 2.5, "start_ordinal": 0, "end_ordinal": 1 } diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_right.json b/src/main/deploy/autos/paths/middle_start_to_tower_right.json index 247494f..62bbbc6 100644 --- a/src/main/deploy/autos/paths/middle_start_to_tower_right.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_right.json @@ -12,6 +12,16 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "shooting_zone_0_start" + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "waypoint", "translation_target": { @@ -24,5 +34,14 @@ "profiled_rotation": true } } - ] + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 2.5, + "start_ordinal": 0, + "end_ordinal": 1 + } + ] + } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/top_start_to_depot_lob.json b/src/main/deploy/autos/paths/top_start_to_depot_lob.json index 316db18..2327900 100644 --- a/src/main/deploy/autos/paths/top_start_to_depot_lob.json +++ b/src/main/deploy/autos/paths/top_start_to_depot_lob.json @@ -12,6 +12,16 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "shooting_zone_0_start" + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "waypoint", "translation_target": { @@ -52,7 +62,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 1.5, + "value": 2.0, "start_ordinal": 0, "end_ordinal": 1 }, diff --git a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json b/src/main/deploy/autos/paths/top_start_to_depot_shoot.json index 018f1da..e0cd10d 100644 --- a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json +++ b/src/main/deploy/autos/paths/top_start_to_depot_shoot.json @@ -12,6 +12,16 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "shooting_zone_0_start" + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "shooting_zone_0_end" + }, { "type": "waypoint", "translation_target": { @@ -52,7 +62,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 1.5, + "value": 2.0, "start_ordinal": 0, "end_ordinal": 1 }, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13734d3..b80ea85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,13 +7,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.commands.autos.AutoShootingController; +import frc.robot.commands.autos.AutoShootingZoneManager; import frc.robot.commands.autos.tower.ScoreL1; import frc.robot.constants.Constants; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; -import frc.robot.lib.BLine.Path.EventTrigger; -import frc.robot.lib.BLine.Path.Waypoint; import frc.robot.lib.input.XboxController; import frc.robot.subsystems.swerve.SwerveDrive; // import frc.robot.subsystems.vision.Vision; @@ -53,10 +54,9 @@ private RobotContainer() { // Configure teleop input suppliers for SwerveDrive FSM // Using normalized inputs (-1 to 1) with deadband applied swerveDrive.setTeleopInputSuppliers( - () -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), - () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND) - ); + () -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), + () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND)); // Set up path supplier for SwerveDrive swerveDrive.setPathSupplier(() -> currentPath, () -> shouldResetPose); @@ -71,6 +71,17 @@ private RobotContainer() { } private void registerEventTriggers() { + // register shooting trigger (up to 5 per path but change if necessary lol) + for (int i = 0; i < 5; i++) { + final int zoneId = i; + FollowPath.registerEventTrigger("shooting_zone_" + i + "_start", () -> { + AutoShootingZoneManager.getInstance().enterShootingZone(zoneId); + }); + FollowPath.registerEventTrigger("shooting_zone_" + i + "_end", () -> { + AutoShootingZoneManager.getInstance().exitShootingZone(zoneId); + }); + } + FollowPath.registerEventTrigger("test1", new InstantCommand(() -> { Logger.recordOutput("In a command", true); })); @@ -82,15 +93,19 @@ private void registerEventTriggers() { private void configureBindings() { // Path toClimb = new Path(new Waypoint(1,3, new Rotation2d(0))); // xboxDriver.getXButton().onTrue( - // new InstantCommand(() -> currentPath = toClimb).andThen( - // new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen( - // new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE)).andThen( - // new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP)) - // ) - // ) + // new InstantCommand(() -> currentPath = toClimb).andThen( + // new InstantCommand(() -> + // swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen( + // new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == + // SwerveDrive.CurrentSystemState.IDLE)).andThen( + // new InstantCommand(() -> + // swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP)) + // ) + // ) // ); - xboxDriver.getXButton().onFalse(new InstantCommand(() -> robotState.resetPose(new Pose2d(0,0, new Rotation2d(0))))); + xboxDriver.getXButton() + .onFalse(new InstantCommand(() -> robotState.resetPose(new Pose2d(0, 0, new Rotation2d(0))))); } public void teleopInit() { @@ -116,19 +131,23 @@ public Command getAutonomousCommand() { Logger.recordOutput("TRIGGER1", false); Logger.recordOutput("TRIGGER2", false); - currentPath = new Path("event_test"); - shouldResetPose = true; + // Reset shooting zone state at start of auto + AutoShootingZoneManager.getInstance().reset(); - new Path( - new Waypoint(0, 0, new Rotation2d(0)), - new EventTrigger(0.5, "test"), - new Waypoint(1, 1, new Rotation2d(0)) - ); + currentPath = new Path("bottom_start_to_output_shoot"); + shouldResetPose = true; - return new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( - new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)).andThen( - new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)) - ); + Command pathFollowingCommand = new InstantCommand( + () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( + new WaitUntilCommand( + () -> swerveDrive + .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) + .andThen( + new InstantCommand( + () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); + return new ParallelCommandGroup( + new AutoShootingController(), + pathFollowingCommand); } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index beabaf7..5a47432 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -25,38 +25,39 @@ // TODO: Make it so that if it is tracking / shooting, the center of rotation for the robot is around the shooter's position public class RobotState { private static RobotState instance; + public static RobotState getInstance() { if (instance == null) { instance = new RobotState(); } return instance; } - - public record OdometryObservation( - double timestampsSeconds, - boolean isGyroConnected, - SwerveModulePosition[] modulePositions, - SwerveModuleState[] moduleStates, - Rotation3d gyroOrientation, - double yawVelocityRadPerSec - ) {} + public record OdometryObservation( + double timestampsSeconds, + boolean isGyroConnected, + SwerveModulePosition[] modulePositions, + SwerveModuleState[] moduleStates, + Rotation3d gyroOrientation, + double yawVelocityRadPerSec) { + } public record VisionObservation( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs - ) {} - + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + } private static final double poseBufferSizeSeconds = 2.0; - private final TimeInterpolatableBuffer poseBuffer = TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds); + private final TimeInterpolatableBuffer poseBuffer = TimeInterpolatableBuffer + .createBuffer(poseBufferSizeSeconds); private SwerveDrivePoseEstimator swerveDrivePoseEstimator; private double lastEstimatedPoseUpdateTime = 0; private int visionObservationsAccepted = 0; private int visionObservationsRejected = 0; + private double lastVisionObservationTimestamp = 0; private double lastGyroResetTime = Timer.getTimestamp(); private double gyroTimeoutSeconds = 1.0; @@ -66,10 +67,10 @@ public record VisionObservation( // Odometry private final SwerveDriveKinematics kinematics; private SwerveModulePosition[] lastWheelPositions = { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private double lastYawVelocityRadPerSec = 0; @@ -84,42 +85,38 @@ private RobotState() { robotStateConfig = ConfigLoader.load("robotState", RobotStateConfig.class); kinematics = new SwerveDriveKinematics( - drivetrainConfig.getFrontLeftPositionMeters(), - drivetrainConfig.getFrontRightPositionMeters(), - drivetrainConfig.getBackLeftPositionMeters(), - drivetrainConfig.getBackRightPositionMeters() - ); + drivetrainConfig.getFrontLeftPositionMeters(), + drivetrainConfig.getFrontRightPositionMeters(), + drivetrainConfig.getBackLeftPositionMeters(), + drivetrainConfig.getBackRightPositionMeters()); swerveDrivePoseEstimator = new SwerveDrivePoseEstimator( - kinematics, - new Rotation2d(), - lastWheelPositions, - new Pose2d(), - VecBuilder.fill( - robotStateConfig.odomTranslationDevBase, - robotStateConfig.odomTranslationDevBase, - robotStateConfig.odomRotationDevBase - - ), - VecBuilder.fill( - robotStateConfig.visionTranslationDevBase, - robotStateConfig.visionTranslationDevBase, - robotStateConfig.visionRotationDevBase - ) - ); - } + kinematics, + new Rotation2d(), + lastWheelPositions, + new Pose2d(), + VecBuilder.fill( + robotStateConfig.odomTranslationDevBase, + robotStateConfig.odomTranslationDevBase, + robotStateConfig.odomRotationDevBase + + ), + VecBuilder.fill( + robotStateConfig.visionTranslationDevBase, + robotStateConfig.visionTranslationDevBase, + robotStateConfig.visionRotationDevBase)); + } /** Add odometry observation */ public void addOdometryObservation(OdometryObservation observation) { observation = new OdometryObservation( - observation.timestampsSeconds(), - observation.isGyroConnected(), - observation.modulePositions().clone(), - observation.moduleStates().clone(), - observation.gyroOrientation(), - observation.yawVelocityRadPerSec() - ); - + observation.timestampsSeconds(), + observation.isGyroConnected(), + observation.modulePositions().clone(), + observation.moduleStates().clone(), + observation.gyroOrientation(), + observation.yawVelocityRadPerSec()); + Logger.recordOutput("RobotState/odometry/timestamp", observation.timestampsSeconds()); Logger.recordOutput("RobotState/odometry/isGyroConnected", observation.isGyroConnected()); Logger.recordOutput("RobotState/odometry/modulePositions", observation.modulePositions()); @@ -129,16 +126,16 @@ public void addOdometryObservation(OdometryObservation observation) { // update robotState member variables lastRobotRelativeSpeeds = kinematics.toChassisSpeeds(observation.moduleStates); - lastRobotRelativeSpeeds.omegaRadiansPerSecond = observation.isGyroConnected ? observation.yawVelocityRadPerSec() : lastRobotRelativeSpeeds.omegaRadiansPerSecond; - lastYawVelocityRadPerSec = observation.isGyroConnected ? observation.yawVelocityRadPerSec() : lastRobotRelativeSpeeds.omegaRadiansPerSecond; - - Rotation3d gyroOrientation = observation.isGyroConnected() ? - observation.gyroOrientation() : - new Rotation3d( - 0.0, - 0.0, - swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians() - ); + lastRobotRelativeSpeeds.omegaRadiansPerSecond = observation.isGyroConnected ? observation.yawVelocityRadPerSec() + : lastRobotRelativeSpeeds.omegaRadiansPerSecond; + lastYawVelocityRadPerSec = observation.isGyroConnected ? observation.yawVelocityRadPerSec() + : lastRobotRelativeSpeeds.omegaRadiansPerSecond; + + Rotation3d gyroOrientation = observation.isGyroConnected() ? observation.gyroOrientation() + : new Rotation3d( + 0.0, + 0.0, + swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians()); lastGyroAngle = gyroOrientation; // Reject odometry if robot is tilted too much @@ -147,33 +144,29 @@ public void addOdometryObservation(OdometryObservation observation) { if (isTilted) { // use old wheel positions swerveDrivePoseEstimator.updateWithTime( - observation.timestampsSeconds(), - observation.isGyroConnected() ? - gyroOrientation.toRotation2d() : - new Rotation2d( - swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians() + - kinematics.toTwist2d(lastWheelPositions, observation.modulePositions()).dtheta - ), - lastWheelPositions - ); + observation.timestampsSeconds(), + observation.isGyroConnected() ? gyroOrientation.toRotation2d() + : new Rotation2d( + swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians() + + kinematics.toTwist2d(lastWheelPositions, + observation.modulePositions()).dtheta), + lastWheelPositions); } else { swerveDrivePoseEstimator.updateWithTime( - observation.timestampsSeconds(), - observation.isGyroConnected() ? - gyroOrientation.toRotation2d() : - new Rotation2d( - swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians() + - kinematics.toTwist2d(lastWheelPositions, observation.modulePositions()).dtheta - ), - observation.modulePositions() - ); + observation.timestampsSeconds(), + observation.isGyroConnected() ? gyroOrientation.toRotation2d() + : new Rotation2d( + swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians() + + kinematics.toTwist2d(lastWheelPositions, + observation.modulePositions()).dtheta), + observation.modulePositions()); lastWheelPositions = observation.modulePositions(); } - - lastEstimatedPoseUpdateTime = observation.timestampsSeconds(); + + lastEstimatedPoseUpdateTime = observation.timestampsSeconds(); // Add pose to buffer at timestamp - poseBuffer.addSample(lastEstimatedPoseUpdateTime, swerveDrivePoseEstimator.getEstimatedPosition()); + poseBuffer.addSample(lastEstimatedPoseUpdateTime, swerveDrivePoseEstimator.getEstimatedPosition()); } public void addVisionObservation(VisionObservation observation) { @@ -184,8 +177,8 @@ public void addVisionObservation(VisionObservation observation) { Logger.recordOutput("RobotState/vision/visionObservationsRejected", visionObservationsRejected); return; } - } - + } + catch (NoSuchElementException ex) { visionObservationsRejected++; Logger.recordOutput("RobotState/vision/visionObservationsRejected", visionObservationsRejected); @@ -193,27 +186,32 @@ public void addVisionObservation(VisionObservation observation) { } visionObservationsAccepted++; + lastVisionObservationTimestamp = observation.timestampSeconds(); - Logger.recordOutput("RobotState/vision/stdDevTranslation", observation.visionMeasurementStdDevs().get(0,0)); - Logger.recordOutput("RobotState/vision/stdDevRotation", observation.visionMeasurementStdDevs().get(2,0)); + Logger.recordOutput("RobotState/vision/stdDevTranslation", observation.visionMeasurementStdDevs().get(0, 0)); + Logger.recordOutput("RobotState/vision/stdDevRotation", observation.visionMeasurementStdDevs().get(2, 0)); Logger.recordOutput("RobotState/vision/visionPose", observation.visionRobotPoseMeters()); Logger.recordOutput("RobotState/vision/visionObservationsAccepted", visionObservationsAccepted); Logger.recordOutput("RobotState/vision/visionObservationsRejected", visionObservationsRejected); - Logger.recordOutput("RobotState/vision/visionLatency", Timer.getFPGATimestamp() - observation.timestampSeconds()); + Logger.recordOutput("RobotState/vision/visionLatency", + Timer.getFPGATimestamp() - observation.timestampSeconds()); + if (DriverStation.isDisabled() && Timer.getTimestamp() - lastGyroResetTime > gyroTimeoutSeconds + && observation.visionMeasurementStdDevs().get(2, 0) < 300) { + resetPose( + new Pose2d(getEstimatedPose().getTranslation(), observation.visionRobotPoseMeters().getRotation())); + lastGyroResetTime = Timer.getTimestamp(); + + Logger.recordOutput("RobotState/vision/gyroReset", true); + Logger.recordOutput("RobotState/vision/gyroRestRotation", + observation.visionRobotPoseMeters().getRotation()); - if (DriverStation.isDisabled() && Timer.getTimestamp() - lastGyroResetTime > gyroTimeoutSeconds && observation.visionMeasurementStdDevs().get(2,0) < 300) { - resetPose(new Pose2d(getEstimatedPose().getTranslation(), observation.visionRobotPoseMeters().getRotation())); - lastGyroResetTime = Timer.getTimestamp(); - - Logger.recordOutput("RobotState/vision/gyroReset", true); - Logger.recordOutput("RobotState/vision/gyroRestRotation", observation.visionRobotPoseMeters().getRotation()); - } else { Logger.recordOutput("RobotState/vision/gyroReset", false); } - swerveDrivePoseEstimator.addVisionMeasurement(observation.visionRobotPoseMeters(), observation.timestampSeconds(), observation.visionMeasurementStdDevs()); + swerveDrivePoseEstimator.addVisionMeasurement(observation.visionRobotPoseMeters(), + observation.timestampSeconds(), observation.visionMeasurementStdDevs()); lastEstimatedPoseUpdateTime = Timer.getTimestamp(); } @@ -247,9 +245,9 @@ public ChassisSpeeds getRobotRelativeSpeeds() { public Rotation3d getLastOrientation() { return new Rotation3d( - lastGyroAngle.getX(), - lastGyroAngle.getY(), - getEstimatedPose().getRotation().getRadians() // pose estim offsets for yaw taken into account + lastGyroAngle.getX(), + lastGyroAngle.getY(), + getEstimatedPose().getRotation().getRadians() // pose estim offsets for yaw taken into account ); } @@ -262,23 +260,30 @@ public Rotation2d getAngleToFloor() { } @AutoLogOutput(key = "RobotState/fieldRelativeSpeeds") - public ChassisSpeeds getFieldRelativeSpeeds() { + public ChassisSpeeds getFieldRelativeSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds(lastRobotRelativeSpeeds, getEstimatedPose().getRotation()); } public Pose2d getPredictedPose(double translationLookaheadS, double rotationLookaheadS) { return getEstimatedPose() - .transformBy( - new Transform2d( - lastRobotRelativeSpeeds.vxMetersPerSecond * translationLookaheadS, - lastRobotRelativeSpeeds.vyMetersPerSecond * translationLookaheadS, - Rotation2d.fromRadians(lastRobotRelativeSpeeds.omegaRadiansPerSecond * rotationLookaheadS) - ) - ); + .transformBy( + new Transform2d( + lastRobotRelativeSpeeds.vxMetersPerSecond * translationLookaheadS, + lastRobotRelativeSpeeds.vyMetersPerSecond * translationLookaheadS, + Rotation2d.fromRadians( + lastRobotRelativeSpeeds.omegaRadiansPerSecond * rotationLookaheadS))); } public Pose2d getPredictedPose(double timestamp) { return getPredictedPose(timestamp - lastEstimatedPoseUpdateTime, timestamp - lastEstimatedPoseUpdateTime); } -} \ No newline at end of file + public boolean hasRecentVisionTarget(double maxAgeSeconds) { + return Timer.getFPGATimestamp() - lastVisionObservationTimestamp < maxAgeSeconds; + } + + @AutoLogOutput(key = "RobotState/hasRecentVisionTarget") + public boolean hasRecentVisionTarget() { + return hasRecentVisionTarget(0.5); + } +} diff --git a/src/main/java/frc/robot/commands/autos/AutoShootingController.java b/src/main/java/frc/robot/commands/autos/AutoShootingController.java new file mode 100644 index 0000000..4b1c489 --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/AutoShootingController.java @@ -0,0 +1,96 @@ +package frc.robot.commands.autos; + +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotState; +import frc.robot.constants.AlignmentConstants; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.CurrentState; +import frc.robot.subsystems.Superstructure.DesiredState; + +public class AutoShootingController extends Command { + private final Superstructure superstructure; + private final RobotState robotState; + private final AutoShootingZoneManager zoneManager; + + public AutoShootingController() { + this.superstructure = Superstructure.getInstance(); + this.robotState = RobotState.getInstance(); + this.zoneManager = AutoShootingZoneManager.getInstance(); + } + + @Override + public void initialize() { + Logger.recordOutput("AutoShootingController/active", true); + } + + @Override + public void execute() { + boolean inShootingZone = zoneManager.isInShootingZone(); + boolean inAllianceZone = isInAllianceZone(); + boolean hasTarget = robotState.hasRecentVisionTarget(); + + Logger.recordOutput("AutoShootingController/inShootingZone", inShootingZone); + Logger.recordOutput("AutoShootingController/inAllianceZone", inAllianceZone); + Logger.recordOutput("AutoShootingController/hasTarget", hasTarget); + + if (inShootingZone && inAllianceZone && hasTarget) { + CurrentState currentState = superstructure.getCurrentState(); + + if (currentState == CurrentState.READY_FOR_SHOT || currentState == CurrentState.SHOOTING) { + superstructure.setDesiredState(DesiredState.SHOOTING); + Logger.recordOutput("AutoShootingController/action", "SHOOTING"); + } else { + superstructure.setDesiredState(DesiredState.READY_FOR_SHOT); + Logger.recordOutput("AutoShootingController/action", "PREPARING"); + } + } else if (inShootingZone && hasTarget) { + superstructure.setDesiredState(DesiredState.TRACKING); + Logger.recordOutput("AutoShootingController/action", "TRACKING"); + } else if (inShootingZone) { + superstructure.setDesiredState(DesiredState.HOME); + Logger.recordOutput("AutoShootingController/action", "NO_TARGET"); + } else { + superstructure.setDesiredState(DesiredState.HOME); + Logger.recordOutput("AutoShootingController/action", "HOME"); + } + } + + @Override + public void end(boolean interrupted) { + superstructure.setDesiredState(DesiredState.HOME); + Logger.recordOutput("AutoShootingController/active", false); + Logger.recordOutput("AutoShootingController/action", "ENDED"); + } + + @Override + public boolean isFinished() { + return false; + } + + /** + * Checks if the robot is currently within its alliance zone. + * Uses the alliance zone boundary from AlignmentConstants. + * + * @return true if robot is behind the alliance zone line + */ + private boolean isInAllianceZone() { + Optional alliance = DriverStation.getAlliance(); + Pose2d currentPose = robotState.getEstimatedPose(); + + double blueZoneX = AlignmentConstants.AllianceBounds.blueZoneLineX; + double redZoneX = AlignmentConstants.AllianceBounds.redZoneLineX; + + if (alliance.isPresent() && alliance.get() == Alliance.Red) { + return currentPose.getX() > redZoneX; + } else { + return currentPose.getX() < blueZoneX; + } + } +} diff --git a/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java b/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java new file mode 100644 index 0000000..f14ca17 --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java @@ -0,0 +1,70 @@ +package frc.robot.commands.autos; + +import java.util.HashSet; +import java.util.Set; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class AutoShootingZoneManager { + private static AutoShootingZoneManager instance; + + public static AutoShootingZoneManager getInstance() { + if (instance == null) { + instance = new AutoShootingZoneManager(); + } + return instance; + } + + private final Set activeZones = new HashSet<>(); + + private AutoShootingZoneManager() { + } + + public void enterShootingZone(int zoneId) { + activeZones.add(zoneId); + + Logger.recordOutput("AutoShootingZone/active", true); + Logger.recordOutput("AutoShootingZone/activeZones", + activeZones.stream().mapToLong(Integer::longValue).toArray()); + Logger.recordOutput("AutoShootingZone/activeZoneCount", activeZones.size()); + } + + /** + * Called when exiting a shooting zone via event trigger. + * + * @param zoneId The unique identifier for this zone + */ + public void exitShootingZone(int zoneId) { + activeZones.remove(zoneId); + + Logger.recordOutput("AutoShootingZone/active", !activeZones.isEmpty()); + Logger.recordOutput("AutoShootingZone/activeZones", + activeZones.stream().mapToLong(Integer::longValue).toArray()); + Logger.recordOutput("AutoShootingZone/activeZoneCount", activeZones.size()); + } + + @AutoLogOutput(key = "AutoShootingZone/isInShootingZone") + public boolean isInShootingZone() { + return !activeZones.isEmpty(); + } + + /** + * @return true if currently within a specific shooting zone + */ + public boolean isInShootingZone(int zoneId) { + return activeZones.contains(zoneId); + } + + /** + * Resets all zone state. Should be called at the start of autonomous. + */ + public void reset() { + activeZones.clear(); + + Logger.recordOutput("AutoShootingZone/active", false); + Logger.recordOutput("AutoShootingZone/activeZones", new long[0]); + Logger.recordOutput("AutoShootingZone/activeZoneCount", 0); + Logger.recordOutput("AutoShootingZone/reset", true); + } +} From 85594383fde9f8e97138bcacb865c5a91cd3eec7 Mon Sep 17 00:00:00 2001 From: fishnos Date: Sat, 31 Jan 2026 17:49:59 -0500 Subject: [PATCH 04/13] Very small fix (whoops) --- src/main/deploy/autos/paths/top_start_to_depot_shoot.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json b/src/main/deploy/autos/paths/top_start_to_depot_shoot.json index e0cd10d..d081d2a 100644 --- a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json +++ b/src/main/deploy/autos/paths/top_start_to_depot_shoot.json @@ -19,7 +19,7 @@ }, { "type": "event_trigger", - "t_ratio": 0.0, + "t_ratio": 1.0, "lib_key": "shooting_zone_0_end" }, { From c498c453151c4de258879d75b6c28ec4ab71601c Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Mon, 2 Feb 2026 16:06:15 -0500 Subject: [PATCH 05/13] Inital dashboard logging --- src/main/java/frc/robot/Dashboard.java | 95 +++++++++++++++++++ src/main/java/frc/robot/Robot.java | 1 + .../robot/subsystems/swerve/SwerveDrive.java | 8 ++ 3 files changed, 104 insertions(+) create mode 100644 src/main/java/frc/robot/Dashboard.java diff --git a/src/main/java/frc/robot/Dashboard.java b/src/main/java/frc/robot/Dashboard.java new file mode 100644 index 0000000..35c79e0 --- /dev/null +++ b/src/main/java/frc/robot/Dashboard.java @@ -0,0 +1,95 @@ +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.swerve.SwerveDrive; + +public class Dashboard { + private static Field2d robotField; + private static SwerveDrive swerveDrive; + + private static StringPublisher superstructureCurrentState; + private static StringPublisher superStructureDesiredState; + + private static boolean initialized = false; + + private static void initialize() { + if (initialized) return; + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable table = inst.getTable("Dashboard"); + + // Create publishers once + superstructureCurrentState = table.getStringTopic("Superstructure/Current State").publish(); + superStructureDesiredState = table.getStringTopic("Superstructure/Desired State").publish(); + + // Initialize Field2d + NetworkTable fieldTable = table.getSubTable("Field"); + robotField = new Field2d(); + + SendableBuilderImpl fieldBuilder = new SendableBuilderImpl(); + fieldBuilder.setTable(fieldTable); + robotField.initSendable(fieldBuilder); + fieldBuilder.startListeners(); + + // Initialize swerve drive sendable + swerveDrive = SwerveDrive.getInstance(); + NetworkTable swerveTable = table.getSubTable("Swerve Drive"); + + Sendable swerveSendable = new Sendable() { + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("SwerveDrive"); + + builder.addDoubleProperty("Front Left Angle", + () -> swerveDrive.getSwerveModulePositions()[0].angle.getRadians(), null); + builder.addDoubleProperty("Front Left Velocity", + () -> swerveDrive.getSwerveModuleStates()[0].speedMetersPerSecond, null); + + builder.addDoubleProperty("Front Right Angle", + () -> swerveDrive.getSwerveModulePositions()[1].angle.getRadians(), null); + builder.addDoubleProperty("Front Right Velocity", + () -> swerveDrive.getSwerveModuleStates()[1].speedMetersPerSecond, null); + + builder.addDoubleProperty("Back Left Angle", + () -> swerveDrive.getSwerveModulePositions()[2].angle.getRadians(), null); + builder.addDoubleProperty("Back Left Velocity", + () -> swerveDrive.getSwerveModuleStates()[2].speedMetersPerSecond, null); + + builder.addDoubleProperty("Back Right Angle", + () -> swerveDrive.getSwerveModulePositions()[3].angle.getRadians(), null); + builder.addDoubleProperty("Back Right Velocity", + () -> swerveDrive.getSwerveModuleStates()[3].speedMetersPerSecond, null); + + builder.addDoubleProperty("Robot Angle", + () -> RobotState.getInstance().getAngleToFloor().getRadians(), null); + } + }; + + SendableBuilderImpl swerveBuilder = new SendableBuilderImpl(); + swerveBuilder.setTable(swerveTable); + swerveSendable.initSendable(swerveBuilder); + swerveBuilder.startListeners(); + + initialized = true; + } + + public static void updateData() { + initialize(); // Initialize on first call + + // Superstructure state logging + Superstructure superstructure = Superstructure.getInstance(); + superstructureCurrentState.set(superstructure.getCurrentState().name()); + superStructureDesiredState.set(superstructure.getDesiredState().name()); + + // Update the field with current robot pose + robotField.setRobotPose(RobotState.getInstance().getEstimatedPose()); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d607706..793569a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -128,6 +128,7 @@ public void robotPeriodic() { // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + Dashboard.updateData(); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 9048b53..62d04bb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -993,6 +993,14 @@ public Rotation2d getGyroAngle() { return gyroInputs.gyroOrientation.toRotation2d(); } + public SwerveModulePosition[] getSwerveModulePositions() { + return modulePositions; + } + + public SwerveModuleState[] getSwerveModuleStates() { + return moduleStates; + } + public void setWheelCoast(boolean isCoast) { for (ModuleIO module : modules) { module.setWheelCoast(isCoast); From 5414195c9cb490c70a6819e29f337b1c10e7d4d4 Mon Sep 17 00:00:00 2001 From: fishnos Date: Tue, 3 Feb 2026 04:17:10 -0500 Subject: [PATCH 06/13] New autos, ready for future auto development. --- ...t_to_depot_lob.json => middle_start_to_depot_lob.json} | 4 ++-- ..._depot_shoot.json => middle_start_to_depot_shoot.json} | 8 ++++---- ...start_to_outpust.json => middle_start_to_outpust.json} | 2 +- .../deploy/autos/paths/middle_start_to_tower_left.json | 2 +- .../deploy/autos/paths/middle_start_to_tower_middle.json | 2 +- .../deploy/autos/paths/middle_start_to_tower_right.json | 6 +++--- ....json => outpost_to_neutral_zone_bottom_left_lob.json} | 0 .../frc/robot/commands/autos/AutoShootingZoneManager.java | 6 +++--- 8 files changed, 15 insertions(+), 15 deletions(-) rename src/main/deploy/autos/paths/{top_start_to_depot_lob.json => middle_start_to_depot_lob.json} (95%) rename src/main/deploy/autos/paths/{top_start_to_depot_shoot.json => middle_start_to_depot_shoot.json} (91%) rename src/main/deploy/autos/paths/{bottom_start_to_outpust.json => middle_start_to_outpust.json} (96%) rename src/main/deploy/autos/paths/{bottom_start_to_output_shoot.json => outpost_to_neutral_zone_bottom_left_lob.json} (100%) diff --git a/src/main/deploy/autos/paths/top_start_to_depot_lob.json b/src/main/deploy/autos/paths/middle_start_to_depot_lob.json similarity index 95% rename from src/main/deploy/autos/paths/top_start_to_depot_lob.json rename to src/main/deploy/autos/paths/middle_start_to_depot_lob.json index 2327900..5a4f2b2 100644 --- a/src/main/deploy/autos/paths/top_start_to_depot_lob.json +++ b/src/main/deploy/autos/paths/middle_start_to_depot_lob.json @@ -3,8 +3,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.7419635459817737, - "y_meters": 5.465041425020713, + "x_meters": 3.7274316487158243, + "y_meters": 4.636723280861641, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { diff --git a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json b/src/main/deploy/autos/paths/middle_start_to_depot_shoot.json similarity index 91% rename from src/main/deploy/autos/paths/top_start_to_depot_shoot.json rename to src/main/deploy/autos/paths/middle_start_to_depot_shoot.json index d081d2a..58d9903 100644 --- a/src/main/deploy/autos/paths/top_start_to_depot_shoot.json +++ b/src/main/deploy/autos/paths/middle_start_to_depot_shoot.json @@ -3,12 +3,12 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.6983678541839273, - "y_meters": 6.86010356255178, + "x_meters": 3.6983678541839264, + "y_meters": 4.9564250207125085, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -2.437343434535229, + "rotation_radians": 2.488496831084017, "profiled_rotation": true } }, @@ -62,7 +62,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 2.0, + "value": 2.1, "start_ordinal": 0, "end_ordinal": 1 }, diff --git a/src/main/deploy/autos/paths/bottom_start_to_outpust.json b/src/main/deploy/autos/paths/middle_start_to_outpust.json similarity index 96% rename from src/main/deploy/autos/paths/bottom_start_to_outpust.json rename to src/main/deploy/autos/paths/middle_start_to_outpust.json index cdc10ef..6de5def 100644 --- a/src/main/deploy/autos/paths/bottom_start_to_outpust.json +++ b/src/main/deploy/autos/paths/middle_start_to_outpust.json @@ -4,7 +4,7 @@ "type": "waypoint", "translation_target": { "x_meters": 3.712899751449876, - "y_meters": 2.050045567522784, + "y_meters": 3.0672783761391855, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_left.json b/src/main/deploy/autos/paths/middle_start_to_tower_left.json index acd033f..3201fb0 100644 --- a/src/main/deploy/autos/paths/middle_start_to_tower_left.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_left.json @@ -4,7 +4,7 @@ "type": "waypoint", "translation_target": { "x_meters": 3.712899751449874, - "y_meters": 3.3143206296603154, + "y_meters": 3.009150787075394, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_middle.json b/src/main/deploy/autos/paths/middle_start_to_tower_middle.json index eb49075..a767b5e 100644 --- a/src/main/deploy/autos/paths/middle_start_to_tower_middle.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_middle.json @@ -4,7 +4,7 @@ "type": "waypoint", "translation_target": { "x_meters": 3.7128997514498776, - "y_meters": 3.677618061309034, + "y_meters": 3.06727837613919, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { diff --git a/src/main/deploy/autos/paths/middle_start_to_tower_right.json b/src/main/deploy/autos/paths/middle_start_to_tower_right.json index 62bbbc6..5803ea8 100644 --- a/src/main/deploy/autos/paths/middle_start_to_tower_right.json +++ b/src/main/deploy/autos/paths/middle_start_to_tower_right.json @@ -3,8 +3,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.6983678541839247, - "y_meters": 4.171702568351285, + "x_meters": 3.712899751449874, + "y_meters": 5.188935376967689, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { @@ -14,7 +14,7 @@ }, { "type": "event_trigger", - "t_ratio": 0.0, + "t_ratio": -0.0, "lib_key": "shooting_zone_0_start" }, { diff --git a/src/main/deploy/autos/paths/bottom_start_to_output_shoot.json b/src/main/deploy/autos/paths/outpost_to_neutral_zone_bottom_left_lob.json similarity index 100% rename from src/main/deploy/autos/paths/bottom_start_to_output_shoot.json rename to src/main/deploy/autos/paths/outpost_to_neutral_zone_bottom_left_lob.json diff --git a/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java b/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java index f14ca17..dbd1700 100644 --- a/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java +++ b/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java @@ -26,7 +26,7 @@ public void enterShootingZone(int zoneId) { Logger.recordOutput("AutoShootingZone/active", true); Logger.recordOutput("AutoShootingZone/activeZones", - activeZones.stream().mapToLong(Integer::longValue).toArray()); + activeZones.stream().mapToDouble(Integer::doubleValue).toArray()); Logger.recordOutput("AutoShootingZone/activeZoneCount", activeZones.size()); } @@ -40,7 +40,7 @@ public void exitShootingZone(int zoneId) { Logger.recordOutput("AutoShootingZone/active", !activeZones.isEmpty()); Logger.recordOutput("AutoShootingZone/activeZones", - activeZones.stream().mapToLong(Integer::longValue).toArray()); + activeZones.stream().mapToDouble(Integer::doubleValue).toArray()); Logger.recordOutput("AutoShootingZone/activeZoneCount", activeZones.size()); } @@ -63,7 +63,7 @@ public void reset() { activeZones.clear(); Logger.recordOutput("AutoShootingZone/active", false); - Logger.recordOutput("AutoShootingZone/activeZones", new long[0]); + Logger.recordOutput("AutoShootingZone/activeZones", new double[0]); Logger.recordOutput("AutoShootingZone/activeZoneCount", 0); Logger.recordOutput("AutoShootingZone/reset", true); } From fae76796b0f6e5b4d4846a4bb0b790a679be7164 Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Tue, 3 Feb 2026 09:55:49 -0500 Subject: [PATCH 07/13] Added auto chooser idk if it works tho --- src/main/java/frc/robot/Dashboard.java | 58 ++++++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 24 +++++---- 2 files changed, 69 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Dashboard.java b/src/main/java/frc/robot/Dashboard.java index 35c79e0..26cd183 100644 --- a/src/main/java/frc/robot/Dashboard.java +++ b/src/main/java/frc/robot/Dashboard.java @@ -1,11 +1,19 @@ package frc.robot; +import java.io.File; +import java.nio.file.Path; +import java.nio.file.Paths; + import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import frc.robot.subsystems.Superstructure; @@ -18,6 +26,9 @@ public class Dashboard { private static StringPublisher superstructureCurrentState; private static StringPublisher superStructureDesiredState; + private static SendableChooser autoChooser; + private static SendableBuilderImpl autoBuilder; // Keep reference to the builder + private static boolean initialized = false; private static void initialize() { @@ -78,18 +89,61 @@ public void initSendable(SendableBuilder builder) { swerveSendable.initSendable(swerveBuilder); swerveBuilder.startListeners(); + // Auto chooser + autoChooser = new SendableChooser(); + autoChooser.setDefaultOption("None", new InstantCommand()); + + // Get all path files from deploy directory + Path pathsDir = Paths.get( + Filesystem.getDeployDirectory().getAbsolutePath(), "autos", "paths" + ); + File dir = pathsDir.toFile(); + + File[] jsonFiles = dir.listFiles((d, name) -> name.endsWith(".json")); + + for (File file : jsonFiles) { + String fileName = file.getName().replace(".json", ""); + Command autoCommand = new InstantCommand( + () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( + new WaitUntilCommand( + () -> swerveDrive + .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) + .andThen( + new InstantCommand( + () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); + autoChooser.addOption(fileName, autoCommand); + } + + + // Add chooser to NetworkTables using SendableBuilder + NetworkTable autoTable = table.getSubTable("Auto Path Chooser"); + autoBuilder = new SendableBuilderImpl(); // Store the builder reference + autoBuilder.setTable(autoTable); + autoChooser.initSendable(autoBuilder); + autoBuilder.startListeners(); + autoBuilder.update(); // Initial update + initialized = true; } + public static Command getCurrentCommand() { + return autoChooser.getSelected(); + } + public static void updateData() { initialize(); // Initialize on first call // Superstructure state logging Superstructure superstructure = Superstructure.getInstance(); superstructureCurrentState.set(superstructure.getCurrentState().name()); - superStructureDesiredState.set(superstructure.getDesiredState().name()); + superStructureDesiredState.set(superstructure.getDesiredState().name()); // Update the field with current robot pose robotField.setRobotPose(RobotState.getInstance().getEstimatedPose()); + + // Update the auto chooser to publish selected value to NetworkTables + if (autoBuilder != null) { + autoBuilder.update(); + } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b80ea85..a3619ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,17 +134,19 @@ public Command getAutonomousCommand() { // Reset shooting zone state at start of auto AutoShootingZoneManager.getInstance().reset(); - currentPath = new Path("bottom_start_to_output_shoot"); - shouldResetPose = true; - - Command pathFollowingCommand = new InstantCommand( - () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( - new WaitUntilCommand( - () -> swerveDrive - .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) - .andThen( - new InstantCommand( - () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); + // currentPath = new Path("bottom_start_to_output_shoot"); + // shouldResetPose = true; + + // Command pathFollowingCommand = new InstantCommand( + // () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( + // new WaitUntilCommand( + // () -> swerveDrive + // .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) + // .andThen( + // new InstantCommand( + // () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); + + Command pathFollowingCommand = Dashboard.getCurrentCommand(); return new ParallelCommandGroup( new AutoShootingController(), From 01fb6cdf3af6cc159fdd75bc9ab88cccd002f8e5 Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Tue, 3 Feb 2026 15:05:29 -0500 Subject: [PATCH 08/13] Updated Dashboard to make it so that user passes in commands to autoChooser --- src/main/java/frc/robot/Dashboard.java | 45 +++++---------------- src/main/java/frc/robot/RobotContainer.java | 24 +++++------ 2 files changed, 21 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/robot/Dashboard.java b/src/main/java/frc/robot/Dashboard.java index 26cd183..8fe641c 100644 --- a/src/main/java/frc/robot/Dashboard.java +++ b/src/main/java/frc/robot/Dashboard.java @@ -1,19 +1,13 @@ package frc.robot; -import java.io.File; -import java.nio.file.Path; -import java.nio.file.Paths; - import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import frc.robot.subsystems.Superstructure; @@ -38,8 +32,8 @@ private static void initialize() { NetworkTable table = inst.getTable("Dashboard"); // Create publishers once - superstructureCurrentState = table.getStringTopic("Superstructure/Current State").publish(); - superStructureDesiredState = table.getStringTopic("Superstructure/Desired State").publish(); + superstructureCurrentState = table.getStringTopic("Superstructure/CurrentState").publish(); + superStructureDesiredState = table.getStringTopic("Superstructure/DesiredState").publish(); // Initialize Field2d NetworkTable fieldTable = table.getSubTable("Field"); @@ -93,40 +87,21 @@ public void initSendable(SendableBuilder builder) { autoChooser = new SendableChooser(); autoChooser.setDefaultOption("None", new InstantCommand()); - // Get all path files from deploy directory - Path pathsDir = Paths.get( - Filesystem.getDeployDirectory().getAbsolutePath(), "autos", "paths" - ); - File dir = pathsDir.toFile(); - - File[] jsonFiles = dir.listFiles((d, name) -> name.endsWith(".json")); - - for (File file : jsonFiles) { - String fileName = file.getName().replace(".json", ""); - Command autoCommand = new InstantCommand( - () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( - new WaitUntilCommand( - () -> swerveDrive - .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) - .andThen( - new InstantCommand( - () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); - autoChooser.addOption(fileName, autoCommand); - } - - - // Add chooser to NetworkTables using SendableBuilder - NetworkTable autoTable = table.getSubTable("Auto Path Chooser"); + NetworkTable autoTable = table.getSubTable("AutoChooser"); autoBuilder = new SendableBuilderImpl(); // Store the builder reference autoBuilder.setTable(autoTable); autoChooser.initSendable(autoBuilder); autoBuilder.startListeners(); - autoBuilder.update(); // Initial update + autoBuilder.update(); initialized = true; } - public static Command getCurrentCommand() { + public static void addAutoChooserOption(String key, Command command) { + autoChooser.addOption(key, command); + } + + public static Command getCurrentAutoCommand() { return autoChooser.getSelected(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a3619ec..b80ea85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,19 +134,17 @@ public Command getAutonomousCommand() { // Reset shooting zone state at start of auto AutoShootingZoneManager.getInstance().reset(); - // currentPath = new Path("bottom_start_to_output_shoot"); - // shouldResetPose = true; - - // Command pathFollowingCommand = new InstantCommand( - // () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( - // new WaitUntilCommand( - // () -> swerveDrive - // .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) - // .andThen( - // new InstantCommand( - // () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); - - Command pathFollowingCommand = Dashboard.getCurrentCommand(); + currentPath = new Path("bottom_start_to_output_shoot"); + shouldResetPose = true; + + Command pathFollowingCommand = new InstantCommand( + () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( + new WaitUntilCommand( + () -> swerveDrive + .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) + .andThen( + new InstantCommand( + () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); return new ParallelCommandGroup( new AutoShootingController(), From 94416513532ab6933e98e540a167275239a87b11 Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Tue, 3 Feb 2026 15:49:55 -0500 Subject: [PATCH 09/13] Added match time --- src/main/java/frc/robot/Dashboard.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Dashboard.java b/src/main/java/frc/robot/Dashboard.java index 8fe641c..799540c 100644 --- a/src/main/java/frc/robot/Dashboard.java +++ b/src/main/java/frc/robot/Dashboard.java @@ -3,6 +3,8 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -19,9 +21,10 @@ public class Dashboard { private static StringPublisher superstructureCurrentState; private static StringPublisher superStructureDesiredState; + private static DoublePublisher matchTimePublisher; private static SendableChooser autoChooser; - private static SendableBuilderImpl autoBuilder; // Keep reference to the builder + private static SendableBuilderImpl autoBuilder; private static boolean initialized = false; @@ -34,6 +37,7 @@ private static void initialize() { // Create publishers once superstructureCurrentState = table.getStringTopic("Superstructure/CurrentState").publish(); superStructureDesiredState = table.getStringTopic("Superstructure/DesiredState").publish(); + matchTimePublisher = table.getDoubleTopic("MatchTime").publish(); // Initialize Field2d NetworkTable fieldTable = table.getSubTable("Field"); @@ -88,7 +92,7 @@ public void initSendable(SendableBuilder builder) { autoChooser.setDefaultOption("None", new InstantCommand()); NetworkTable autoTable = table.getSubTable("AutoChooser"); - autoBuilder = new SendableBuilderImpl(); // Store the builder reference + autoBuilder = new SendableBuilderImpl(); autoBuilder.setTable(autoTable); autoChooser.initSendable(autoBuilder); autoBuilder.startListeners(); @@ -106,17 +110,19 @@ public static Command getCurrentAutoCommand() { } public static void updateData() { - initialize(); // Initialize on first call + initialize(); // Superstructure state logging Superstructure superstructure = Superstructure.getInstance(); superstructureCurrentState.set(superstructure.getCurrentState().name()); superStructureDesiredState.set(superstructure.getDesiredState().name()); + // Update match time + matchTimePublisher.set(DriverStation.getMatchTime()); + // Update the field with current robot pose robotField.setRobotPose(RobotState.getInstance().getEstimatedPose()); - // Update the auto chooser to publish selected value to NetworkTables if (autoBuilder != null) { autoBuilder.update(); } From bd6fc42daf712cda23963d4effbd8bff51b25bdd Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Tue, 3 Feb 2026 15:53:09 -0500 Subject: [PATCH 10/13] Added auto dashboard layout to deploy folder --- src/main/deploy/auto-dashboard-layout | 125 ++++++++++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100644 src/main/deploy/auto-dashboard-layout diff --git a/src/main/deploy/auto-dashboard-layout b/src/main/deploy/auto-dashboard-layout new file mode 100644 index 0000000..b07e04e --- /dev/null +++ b/src/main/deploy/auto-dashboard-layout @@ -0,0 +1,125 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "Superstructure", + "x": 384.0, + "y": 384.0, + "width": 384.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "CurrentState", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/Dashboard/Superstructure/CurrentState", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "DesiredState", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/Dashboard/Superstructure/DesiredState", + "period": 0.06, + "data_type": "string" + } + } + ] + } + ], + "containers": [ + { + "title": "AutoChooser", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/Dashboard/AutoChooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Swerve Drive", + "x": 384.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "SwerveDrive", + "properties": { + "topic": "/Dashboard/Swerve Drive", + "period": 0.06, + "show_robot_rotation": true, + "rotation_unit": "Radians" + } + }, + { + "title": "Field", + "x": 768.0, + "y": 0.0, + "width": 768.0, + "height": 640.0, + "type": "Field", + "properties": { + "topic": "/Dashboard/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "MatchTime", + "x": 0.0, + "y": 512.0, + "width": 384.0, + "height": 128.0, + "type": "Match Time", + "properties": { + "topic": "/Dashboard/MatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + } + ] + } + } + ] +} \ No newline at end of file From 6b0abd599e97ed78fc8094e932d1b10b52388fda Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Tue, 3 Feb 2026 15:54:11 -0500 Subject: [PATCH 11/13] Fixed layout file to be json --- .../deploy/{auto-dashboard-layout => auto-dashboard-layout.json} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/deploy/{auto-dashboard-layout => auto-dashboard-layout.json} (100%) diff --git a/src/main/deploy/auto-dashboard-layout b/src/main/deploy/auto-dashboard-layout.json similarity index 100% rename from src/main/deploy/auto-dashboard-layout rename to src/main/deploy/auto-dashboard-layout.json From c59da0d44af3c975cee45415c563d16f036c7a54 Mon Sep 17 00:00:00 2001 From: Sahil Gupta Date: Tue, 3 Feb 2026 16:00:53 -0500 Subject: [PATCH 12/13] Updated the wpilib thing bc it told me to i lwk dk --- ...enix6-26.1.0.json => Phoenix6-26.1.1.json} | 62 +++++++++---------- vendordeps/photonlib.json | 12 ++-- 2 files changed, 37 insertions(+), 37 deletions(-) rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%) diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 4f9a5d1..afac990 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-4" + "version": "v2026.2.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-4" + "version": "v2026.2.1" } ] } \ No newline at end of file From 98544a2862fdfc358e9d9893ea93b50ec3a1908d Mon Sep 17 00:00:00 2001 From: fishnos Date: Tue, 3 Feb 2026 19:10:58 -0500 Subject: [PATCH 13/13] Auto functionality with path sequences and Elastic dashboard auto chooser. --- simgui-ds.json | 7 +- .../paths/middle_start_to_depot_lob.json | 4 +- src/main/java/frc/robot/RobotContainer.java | 41 +- .../frc/robot/commands/autos/AutoBuilder.java | 69 +++ .../frc/robot/commands/autos/AutoChooser.java | 96 +++++ .../AutoShootingController.java | 2 +- .../AutoShootingZoneManager.java | 2 +- .../java/frc/robot/constants/Constants.java | 2 +- .../robot/subsystems/swerve/SwerveDrive.java | 405 +++++++++--------- 9 files changed, 386 insertions(+), 242 deletions(-) create mode 100644 src/main/java/frc/robot/commands/autos/AutoBuilder.java create mode 100644 src/main/java/frc/robot/commands/autos/AutoChooser.java rename src/main/java/frc/robot/commands/autos/{ => shooting}/AutoShootingController.java (98%) rename src/main/java/frc/robot/commands/autos/{ => shooting}/AutoShootingZoneManager.java (98%) diff --git a/simgui-ds.json b/simgui-ds.json index fe7985a..747dc63 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -10,6 +10,8 @@ "decKey": 87, "incKey": 83 }, + {}, + {}, { "decKey": 69, "decayRate": 0.0, @@ -17,7 +19,7 @@ "keyRate": 0.009999999776482582 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -90,9 +92,6 @@ } ], "robotJoysticks": [ - {}, - {}, - {}, { "guid": "Keyboard0" } diff --git a/src/main/deploy/autos/paths/middle_start_to_depot_lob.json b/src/main/deploy/autos/paths/middle_start_to_depot_lob.json index 5a4f2b2..fcc2510 100644 --- a/src/main/deploy/autos/paths/middle_start_to_depot_lob.json +++ b/src/main/deploy/autos/paths/middle_start_to_depot_lob.json @@ -25,8 +25,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 0.5272426392805076, - "y_meters": 5.4650414250207096, + "x_meters": 0.3642831902206263, + "y_meters": 5.452506082785334, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b80ea85..d91f674 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,14 +7,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.commands.autos.AutoShootingController; -import frc.robot.commands.autos.AutoShootingZoneManager; -import frc.robot.commands.autos.tower.ScoreL1; +import frc.robot.commands.autos.AutoChooser; +import frc.robot.commands.autos.shooting.AutoShootingZoneManager; import frc.robot.constants.Constants; import frc.robot.lib.BLine.FollowPath; -import frc.robot.lib.BLine.Path; import frc.robot.lib.input.XboxController; import frc.robot.subsystems.swerve.SwerveDrive; // import frc.robot.subsystems.vision.Vision; @@ -39,10 +35,7 @@ public static RobotContainer getInstance() { private final SwerveDrive swerveDrive = SwerveDrive.getInstance(); // private final Superstructure superstructure = Superstructure.getInstance(); private final Vision vision = Vision.getInstance(); - - // Path for follow path state - private Path currentPath = null; - private boolean shouldResetPose = false; + private final AutoChooser autoChooser = AutoChooser.getInstance(); private RobotContainer() { registerEventTriggers(); @@ -58,9 +51,6 @@ private RobotContainer() { () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND)); - // Set up path supplier for SwerveDrive - swerveDrive.setPathSupplier(() -> currentPath, () -> shouldResetPose); - // Set default state to TELEOP swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP); @@ -109,8 +99,6 @@ private void configureBindings() { } public void teleopInit() { - shouldResetPose = false; - // Ensure we're in teleop state swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP); // superstructure.setDesiredState(Superstructure.DesiredState.HOME); @@ -128,26 +116,7 @@ public void disabledInit() { } public Command getAutonomousCommand() { - Logger.recordOutput("TRIGGER1", false); - Logger.recordOutput("TRIGGER2", false); - - // Reset shooting zone state at start of auto - AutoShootingZoneManager.getInstance().reset(); - - currentPath = new Path("bottom_start_to_output_shoot"); - shouldResetPose = true; - - Command pathFollowingCommand = new InstantCommand( - () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.PREPARE_FOR_AUTO)).andThen( - new WaitUntilCommand( - () -> swerveDrive - .getCurrentSystemState() == SwerveDrive.CurrentSystemState.READY_FOR_AUTO)) - .andThen( - new InstantCommand( - () -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH))); - - return new ParallelCommandGroup( - new AutoShootingController(), - pathFollowingCommand); + // Use AutoChooser for dashboard-based auto selection + return autoChooser.getSelectedAuto(); } } diff --git a/src/main/java/frc/robot/commands/autos/AutoBuilder.java b/src/main/java/frc/robot/commands/autos/AutoBuilder.java new file mode 100644 index 0000000..cfc7731 --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/AutoBuilder.java @@ -0,0 +1,69 @@ +package frc.robot.commands.autos; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.lib.BLine.Path; +import frc.robot.subsystems.swerve.SwerveDrive; +import frc.robot.subsystems.swerve.SwerveDrive.CurrentSystemState; +import frc.robot.subsystems.swerve.SwerveDrive.DesiredSystemState; +import frc.robot.commands.autos.shooting.AutoShootingController; +import frc.robot.commands.autos.shooting.AutoShootingZoneManager; + +/** + * Simple utility class for creating autonomous routines. + * + * Usage: + * AutoBuilder.path("my_path") // Single path + * AutoBuilder.paths("path1", "path2") // Multiple sequential paths + * AutoBuilder.sequence(path1Cmd, intakeCmd, path2Cmd) // Custom sequence + */ +public class AutoBuilder { + + private static final SwerveDrive swerveDrive = SwerveDrive.getInstance(); + + public static Command path(String pathName) { + return path(pathName, true); + } + + public static Command path(String pathName, boolean resetPose) { + return Commands.sequence( + Commands.runOnce(() -> { + Logger.recordOutput("Auto/CurrentPath", pathName); + Path path = new Path(pathName); + swerveDrive.setPathSupplier(() -> path, () -> resetPose); + swerveDrive.setDesiredSystemState(DesiredSystemState.PREPARE_FOR_AUTO); + }), + + new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == CurrentSystemState.READY_FOR_AUTO), + Commands.runOnce(() -> swerveDrive.setDesiredSystemState(DesiredSystemState.FOLLOW_PATH)), + + new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == CurrentSystemState.IDLE), + Commands.runOnce(() -> Logger.recordOutput("Auto/PathComplete", pathName))); + } + + public static Command paths(String... pathNames) { + SequentialCommandGroup seq = new SequentialCommandGroup(); + for (int i = 0; i < pathNames.length; i++) { + seq.addCommands(path(pathNames[i], i == 0)); + } + return seq; + } + + public static Command sequence(Command... commands) { + return Commands.sequence(commands); + } + public static Command withShooting(Command autoCommand) { + AutoShootingZoneManager.getInstance().reset(); + return Commands.parallel( + new AutoShootingController(), + autoCommand); + } + + public static Command wait(double seconds) { + return Commands.waitSeconds(seconds); + } +} diff --git a/src/main/java/frc/robot/commands/autos/AutoChooser.java b/src/main/java/frc/robot/commands/autos/AutoChooser.java new file mode 100644 index 0000000..aae8d52 --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/AutoChooser.java @@ -0,0 +1,96 @@ +package frc.robot.commands.autos; + +import java.io.File; +import java.util.ArrayList; +import java.util.List; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.util.sendable.SendableRegistry; + +public class AutoChooser { + private static AutoChooser instance; + + public static AutoChooser getInstance() { + if (instance == null) { + instance = new AutoChooser(); + } + return instance; + } + + private final SendableChooser chooser = new SendableChooser<>(); + private final StringSubscriber customPathSub; + private static final String NT_TABLE = "Auto"; + + private AutoChooser() { + registerAutos(); + + var table = NetworkTableInstance.getDefault().getTable(NT_TABLE); + + SendableRegistry.add(chooser, NT_TABLE, "Chooser"); + var builder = new SendableBuilderImpl(); + builder.setTable(table.getSubTable("Chooser")); + chooser.initSendable(builder); + builder.startListeners(); + + table.getEntry("CustomPath").setString(""); + customPathSub = table.getStringTopic("CustomPath").subscribe(""); + } + + private void registerAutos() { + chooser.setDefaultOption("Do Nothing", Commands.none()); + + for (String path : getAvailablePaths()) { + chooser.addOption("Path: " + path, AutoBuilder.path(path)); + } + + chooser.addOption("Tower Left", + AutoBuilder.withShooting(AutoBuilder.path("middle_start_to_tower_left"))); + + chooser.addOption("Tower Middle", + AutoBuilder.withShooting(AutoBuilder.path("middle_start_to_tower_middle"))); + + chooser.addOption("Tower Right", + AutoBuilder.withShooting(AutoBuilder.path("middle_start_to_tower_right"))); + + chooser.addOption("Depot Shoot and Lob", + AutoBuilder.withShooting( + AutoBuilder.paths("middle_start_to_depot_shoot", "outpost_to_neutral_zone_bottom_left_lob"))); + } + + public Command getSelectedAuto() { + String customPath = customPathSub.get(); + + if (customPath != null && !customPath.isEmpty()) { + Logger.recordOutput("Auto/Selected", "Custom: " + customPath); + return AutoBuilder.path(customPath); + } + + Command selected = chooser.getSelected(); + Logger.recordOutput("Auto/Selected", selected != null ? selected.getName() : "None"); + + return selected != null ? selected : Commands.none(); + } + + private List getAvailablePaths() { + List paths = new ArrayList<>(); + File dir = new File(Filesystem.getDeployDirectory(), "autos/paths"); + + if (dir.exists() && dir.isDirectory()) { + File[] files = dir.listFiles((d, n) -> n.endsWith(".json")); + if (files != null) { + for (File f : files) { + paths.add(f.getName().replace(".json", "")); + } + } + } + return paths; + } +} diff --git a/src/main/java/frc/robot/commands/autos/AutoShootingController.java b/src/main/java/frc/robot/commands/autos/shooting/AutoShootingController.java similarity index 98% rename from src/main/java/frc/robot/commands/autos/AutoShootingController.java rename to src/main/java/frc/robot/commands/autos/shooting/AutoShootingController.java index 4b1c489..8efa73d 100644 --- a/src/main/java/frc/robot/commands/autos/AutoShootingController.java +++ b/src/main/java/frc/robot/commands/autos/shooting/AutoShootingController.java @@ -1,4 +1,4 @@ -package frc.robot.commands.autos; +package frc.robot.commands.autos.shooting; import java.util.Optional; diff --git a/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java b/src/main/java/frc/robot/commands/autos/shooting/AutoShootingZoneManager.java similarity index 98% rename from src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java rename to src/main/java/frc/robot/commands/autos/shooting/AutoShootingZoneManager.java index dbd1700..58968b9 100644 --- a/src/main/java/frc/robot/commands/autos/AutoShootingZoneManager.java +++ b/src/main/java/frc/robot/commands/autos/shooting/AutoShootingZoneManager.java @@ -1,4 +1,4 @@ -package frc.robot.commands.autos; +package frc.robot.commands.autos.shooting; import java.util.HashSet; import java.util.Set; diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index b90f628..9e79e45 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -26,7 +26,7 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final Mode currentMode = Mode.COMP; // TODO: change this if sim + public static final Mode currentMode = Mode.SIM; // TODO: change this if sim public static boolean agentMode = false; // public static final boolean isSYSID = true; // TODO: change this if sysid diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 9048b53..3cb71fe 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -49,6 +49,7 @@ public class SwerveDrive extends SubsystemBase { private static SwerveDrive instance = null; + public static SwerveDrive getInstance() { if (instance == null) { instance = new SwerveDrive(); @@ -100,7 +101,6 @@ public enum CurrentTranslationOverrideState { CAPPED } - // FSM State Variables private DesiredSystemState desiredSystemState = DesiredSystemState.DISABLED; private CurrentSystemState currentSystemState = CurrentSystemState.DISABLED; @@ -130,13 +130,15 @@ public enum CurrentTranslationOverrideState { private Rotation2d rotationRangeMax = Rotation2d.fromDegrees(180); private PIDController rangedRotationPIDController; private static final double RANGED_ROTATION_MAX_VELOCITY_FACTOR = 0.6; - private static final double RANGED_ROTATION_BUFFER_RAD = Math.toRadians(15.0); // Buffer to prevent oscillation at boundaries + private static final double RANGED_ROTATION_BUFFER_RAD = Math.toRadians(15.0); // Buffer to prevent oscillation at + // boundaries private boolean shouldOverrideOmega = false; private double omegaOverride = 0.0; private double lastUnoverriddenOmega = 0.0; - // Translational speed freezing (used during shooting) - only vx/vy are frozen, omega remains controlled + // Translational speed freezing (used during shooting) - only vx/vy are frozen, + // omega remains controlled private boolean shouldOverrideTranslationalSpeedsFrozen = false; private double frozenVxMetersPerSec = 0.0; private double frozenVyMetersPerSec = 0.0; @@ -167,16 +169,16 @@ public enum CurrentTranslationOverrideState { private GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private SwerveModulePosition[] modulePositions = new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private SwerveModuleState[] moduleStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() }; private ChassisSpeeds desiredRobotRelativeSpeeds = new ChassisSpeeds(); @@ -200,100 +202,95 @@ private SwerveDrive() { if (RobotBase.isSimulation()) { modules = new ModuleIO[] { - new ModuleIOSim(moduleGeneralConfig, 0), - new ModuleIOSim(moduleGeneralConfig, 1), - new ModuleIOSim(moduleGeneralConfig, 2), - new ModuleIOSim(moduleGeneralConfig, 3) + new ModuleIOSim(moduleGeneralConfig, 0), + new ModuleIOSim(moduleGeneralConfig, 1), + new ModuleIOSim(moduleGeneralConfig, 2), + new ModuleIOSim(moduleGeneralConfig, 3) + }; + gyroIO = new GyroIO() { }; - gyroIO = new GyroIO() {}; } else if (Constants.currentMode == Constants.Mode.REPLAY) { modules = new ModuleIO[] { - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {} + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + } }; gyroIO = new GyroIOPigeon2(); } else { modules = new ModuleIO[] { - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontLeft), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontRight), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backLeft), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backRight) + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontLeft), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontRight), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backLeft), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backRight) }; gyroIO = new GyroIOPigeon2(); PhoenixOdometryThread.getInstance().start(); } kinematics = new SwerveDriveKinematics( - drivetrainConfig.getFrontLeftPositionMeters(), - drivetrainConfig.getFrontRightPositionMeters(), - drivetrainConfig.getBackLeftPositionMeters(), - drivetrainConfig.getBackRightPositionMeters() - ); + drivetrainConfig.getFrontLeftPositionMeters(), + drivetrainConfig.getFrontRightPositionMeters(), + drivetrainConfig.getBackLeftPositionMeters(), + drivetrainConfig.getBackRightPositionMeters()); - // Create the SysId routine - this is going to be in torque current foc units not voltage + // Create the SysId routine - this is going to be in torque current foc units + // not voltage driveCharacterizationSysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(1.5).per(Second), Volts.of(12), Seconds.of(15), // Use default config - (state) -> Logger.recordOutput("DriveCharacterizationSysIdRoutineState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (torqueCurrentFOC) -> { - for (ModuleIO module : modules) { - module.setDriveTorqueCurrentFOC(torqueCurrentFOC.in(Volts), new Rotation2d(0)); - } - }, - null, // No log consumer, since data is recorded by AdvantageKit - this - ) - ); + new SysIdRoutine.Config( + Volts.of(1.5).per(Second), Volts.of(12), Seconds.of(15), // Use default config + (state) -> Logger.recordOutput("DriveCharacterizationSysIdRoutineState", state.toString())), + new SysIdRoutine.Mechanism( + (torqueCurrentFOC) -> { + for (ModuleIO module : modules) { + module.setDriveTorqueCurrentFOC(torqueCurrentFOC.in(Volts), new Rotation2d(0)); + } + }, + null, // No log consumer, since data is recorded by AdvantageKit + this)); steerCharacterizationSysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(1).per(Second), Volts.of(7), Seconds.of(10), // Use default config - (state) -> Logger.recordOutput("SteerCharacterizationSysIdRoutineState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (torqueCurrentFOC) -> { - for (ModuleIO module : modules) { - module.setSteerTorqueCurrentFOC(torqueCurrentFOC.in(Volts), 0); - } - }, - null, // No log consumer, since data is recorded by AdvantageKit - this - ) - ); + new SysIdRoutine.Config( + Volts.of(1).per(Second), Volts.of(7), Seconds.of(10), // Use default config + (state) -> Logger.recordOutput("SteerCharacterizationSysIdRoutineState", state.toString())), + new SysIdRoutine.Mechanism( + (torqueCurrentFOC) -> { + for (ModuleIO module : modules) { + module.setSteerTorqueCurrentFOC(torqueCurrentFOC.in(Volts), 0); + } + }, + null, // No log consumer, since data is recorded by AdvantageKit + this)); // Configure FollowPath builder using drivetrain config followPathBuilder = new FollowPath.Builder( - this, - RobotState.getInstance()::getEstimatedPose, - RobotState.getInstance()::getRobotRelativeSpeeds, - this::driveRobotRelative, - new PIDController( - drivetrainConfig.followPathTranslationKP, - drivetrainConfig.followPathTranslationKI, - drivetrainConfig.followPathTranslationKD - ), - new PIDController( - drivetrainConfig.followPathRotationKP, - drivetrainConfig.followPathRotationKI, - drivetrainConfig.followPathRotationKD - ), - new PIDController( - drivetrainConfig.followPathCrossTrackKP, - drivetrainConfig.followPathCrossTrackKI, - drivetrainConfig.followPathCrossTrackKD - ) - ).withDefaultShouldFlip(); + this, + RobotState.getInstance()::getEstimatedPose, + RobotState.getInstance()::getRobotRelativeSpeeds, + this::driveRobotRelative, + new PIDController( + drivetrainConfig.followPathTranslationKP, + drivetrainConfig.followPathTranslationKI, + drivetrainConfig.followPathTranslationKD), + new PIDController( + drivetrainConfig.followPathRotationKP, + drivetrainConfig.followPathRotationKI, + drivetrainConfig.followPathRotationKD), + new PIDController( + drivetrainConfig.followPathCrossTrackKP, + drivetrainConfig.followPathCrossTrackKI, + drivetrainConfig.followPathCrossTrackKD)) + .withDefaultShouldFlip(); // Configure ranged rotation PID controller with velocity limiting rangedRotationPIDController = new PIDController( - drivetrainConfig.rangedRotationKP, - drivetrainConfig.rangedRotationKI, - drivetrainConfig.rangedRotationKD - ); + drivetrainConfig.rangedRotationKP, + drivetrainConfig.rangedRotationKI, + drivetrainConfig.rangedRotationKD); rangedRotationPIDController.enableContinuousInput(-Math.PI, Math.PI); // rangedRotationPIDController.setTolerance(Math.toRadians(drivetrainConfig.getRangedRotationToleranceDeg())); @@ -301,7 +298,7 @@ private SwerveDrive() { @Override public void periodic() { - double dt = Timer.getTimestamp() - prevLoopTime; + double dt = Timer.getTimestamp() - prevLoopTime; prevLoopTime = Timer.getTimestamp(); Logger.recordOutput("SwerveDrive/dtPeriodic", dt); @@ -324,9 +321,8 @@ public void periodic() { for (int i = 0; i < 4; i++) { moduleStates[i] = new SwerveModuleState( - moduleInputs[i].driveVelocityMetersPerSec, - moduleInputs[i].steerPosition - ); + moduleInputs[i].driveVelocityMetersPerSec, + moduleInputs[i].steerPosition); } ArrayList updatedPoses = new ArrayList(); @@ -335,27 +331,21 @@ public void periodic() { for (int i = 0; i < odometryTimestampsSeconds.length; i++) { for (int j = 0; j < 4; j++) { modulePositions[j] = new SwerveModulePosition( - moduleInputs[j].odometryDrivePositionsMeters[i], - moduleInputs[j].odometrySteerPositions[i] - ); + moduleInputs[j].odometryDrivePositionsMeters[i], + moduleInputs[j].odometrySteerPositions[i]); } - + RobotState.getInstance().addOdometryObservation( - new OdometryObservation( - odometryTimestampsSeconds[i], - gyroInputs.isConnected, - modulePositions, - moduleStates, - gyroInputs.isConnected ? - new Rotation3d( - gyroInputs.gyroOrientation.getX(), - gyroInputs.gyroOrientation.getY(), - gyroInputs.odometryYawPositions[i].getRadians() - ) : - new Rotation3d(), - gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0 - ) - ); + new OdometryObservation( + odometryTimestampsSeconds[i], + gyroInputs.isConnected, + modulePositions, + moduleStates, + gyroInputs.isConnected ? new Rotation3d( + gyroInputs.gyroOrientation.getX(), + gyroInputs.gyroOrientation.getY(), + gyroInputs.odometryYawPositions[i].getRadians()) : new Rotation3d(), + gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0)); updatedPoses.add(RobotState.getInstance().getEstimatedPose()); } @@ -368,7 +358,15 @@ public void periodic() { handleStateTransitions(); handleCurrentState(); - Logger.recordOutput("SwerveDrive/CurrentCommand", this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); + // Debug logging for FSM state and teleop inputs + Logger.recordOutput("SwerveDrive/CurrentSystemState", currentSystemState.toString()); + Logger.recordOutput("SwerveDrive/DesiredSystemState", desiredSystemState.toString()); + Logger.recordOutput("SwerveDrive/TeleopInput/vxNormalized", vxNormalizedSupplier.getAsDouble()); + Logger.recordOutput("SwerveDrive/TeleopInput/vyNormalized", vyNormalizedSupplier.getAsDouble()); + Logger.recordOutput("SwerveDrive/TeleopInput/omegaNormalized", omegaNormalizedSupplier.getAsDouble()); + + Logger.recordOutput("SwerveDrive/CurrentCommand", + this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); } /** @@ -394,7 +392,7 @@ private void handleStateTransitions() { currentSystemState = CurrentSystemState.FOLLOW_PATH; } break; - + case PREPARE_FOR_AUTO: if (pathSupplier.get() != null) { modulesAlignmentTargetRotation = pathSupplier.get().getInitialModuleDirection(); @@ -490,8 +488,8 @@ private void handleCurrentState() { break; } - } + private void cancelPathCommand() { if (currentPathCommand != null && currentPathCommand.isScheduled()) { currentPathCommand.cancel(); @@ -504,7 +502,7 @@ private void handleDISABLEDSystemState() { setWheelCoast(true); } cancelPathCommand(); - + driveFieldRelative(new ChassisSpeeds(0, 0, 0)); previousSystemState = CurrentSystemState.DISABLED; @@ -517,7 +515,8 @@ private void handleIdleSystemState() { cancelPathCommand(); // Only cancel running commands, but don't null out finished commands - // This prevents re-scheduling when path completes while desired state is still FOLLOW_PATH + // This prevents re-scheduling when path completes while desired state is still + // FOLLOW_PATH if (currentPathCommand != null && currentPathCommand.isScheduled()) { currentPathCommand.cancel(); } @@ -535,10 +534,9 @@ private void handleTeleopSystemState() { invert = Constants.shouldFlipPath() ? -1 : 1; ChassisSpeeds desiredFieldRelativeSpeeds = new ChassisSpeeds( - vxNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, - vyNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, - omegaNormalizedSupplier.getAsDouble() * drivetrainConfig.maxAngularVelocityRadiansPerSec - ); + vxNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, + vyNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, + omegaNormalizedSupplier.getAsDouble() * drivetrainConfig.maxAngularVelocityRadiansPerSec); driveFieldRelative(desiredFieldRelativeSpeeds); previousSystemState = CurrentSystemState.TELEOP; @@ -548,10 +546,11 @@ private void handleFollowPathSystemState() { if (previousSystemState == CurrentSystemState.DISABLED) { setWheelCoast(false); } - + // Schedule path command if not already running Path path = pathSupplier.get(); - if (path != null && (currentPathCommand == null || (!currentPathCommand.isScheduled() && !currentPathCommand.isFinished()))) { + if (path != null && (currentPathCommand == null + || (!currentPathCommand.isScheduled() && !currentPathCommand.isFinished()))) { currentPathCommand = buildPathCommand(path); CommandScheduler.getInstance().schedule(currentPathCommand); } @@ -561,7 +560,8 @@ private void handleFollowPathSystemState() { } private void prepareForAuto() { - if (previousSystemState != CurrentSystemState.PREPARE_FOR_AUTO && previousSystemState != CurrentSystemState.READY_FOR_AUTO) { + if (previousSystemState != CurrentSystemState.PREPARE_FOR_AUTO + && previousSystemState != CurrentSystemState.READY_FOR_AUTO) { setWheelCoast(false); } cancelPathCommand(); @@ -598,10 +598,11 @@ private void handleNoneOmegaOverrideState() { previousOmegaOverrideState = CurrentOmegaOverrideState.NONE; } - + private void handleRangedRotationOmegaOverrideState() { shouldOverrideOmega = true; - if (isWithinRotationRange(RANGED_ROTATION_BUFFER_RAD - Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg))) { + if (isWithinRotationRange( + RANGED_ROTATION_BUFFER_RAD - Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg))) { omegaOverride = limitOmegaForRange(lastUnoverriddenOmega); // TODO: BAD FIX } else { omegaOverride = calculateReturnToRangeOmega(); @@ -612,7 +613,7 @@ private void handleRangedRotationNominalOmegaOverrideState() { handleRangedRotationOmegaOverrideState(); previousOmegaOverrideState = CurrentOmegaOverrideState.RANGED_ROTATION_NOMINAL; } - + private void handleRangedRotationReturningOmegaOverrideState() { handleRangedRotationOmegaOverrideState(); previousOmegaOverrideState = CurrentOmegaOverrideState.RANGED_ROTATION_RETURNING; @@ -645,17 +646,17 @@ private void handleCappedTranslationOverrideState() { } /** - * Builds a path command using the followPathBuilder, applying pose reset if configured. + * Builds a path command using the followPathBuilder, applying pose reset if + * configured. */ private Command buildPathCommand(Path path) { if (shouldPoseResetSupplier.get()) { return followPathBuilder.withPoseReset(RobotState.getInstance()::resetPose).build(path); } - return followPathBuilder.withPoseReset((Pose2d pose) -> {}).build(path); + return followPathBuilder.withPoseReset((Pose2d pose) -> { + }).build(path); } - - /** * Checks if robot rotation is within the specified range. */ @@ -664,17 +665,20 @@ private boolean isWithinRotationRange() { } /** - * Checks if robot rotation is within the specified range with an optional buffer. - * @param buffer The buffer in radians to constrict the range by (applied to both min and max) + * Checks if robot rotation is within the specified range with an optional + * buffer. + * + * @param buffer The buffer in radians to constrict the range by (applied to + * both min and max) */ private boolean isWithinRotationRange(double buffer) { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); - + // Normalize angles to -PI to PI for comparison double current = MathUtil.angleModulus(currentRotation.getRadians()); double min = MathUtil.angleModulus(rotationRangeMin.getRadians() + buffer); double max = MathUtil.angleModulus(rotationRangeMax.getRadians() - buffer); - + // Handle wrap-around case if (min <= max) { return current >= min && current <= max; @@ -686,41 +690,44 @@ private boolean isWithinRotationRange(double buffer) { /** * Checks if robot rotation is within the padded/constricted range. - * The padded range is the user-supplied range reduced by RANGED_ROTATION_BUFFER_RAD on each side. + * The padded range is the user-supplied range reduced by + * RANGED_ROTATION_BUFFER_RAD on each side. */ private boolean isWithinPaddedRotationRange() { return isWithinRotationRange(RANGED_ROTATION_BUFFER_RAD); } /** - * Limits omega using sqrt(2*a*d) formula to prevent exceeding the padded rotation bounds. + * Limits omega using sqrt(2*a*d) formula to prevent exceeding the padded + * rotation bounds. * Uses the padded range (user range constricted by buffer) as the boundary. - * Accounts for current robot angular velocity to be more conservative when already + * Accounts for current robot angular velocity to be more conservative when + * already * moving towards a boundary. */ private double limitOmegaForRange(double desiredOmega) { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); double currentOmega = RobotState.getInstance().getYawVelocityRadPerSec(); - + double current = currentRotation.getRadians(); // Use padded range bounds (constricted by buffer) double min = rotationRangeMin.getRadians() + RANGED_ROTATION_BUFFER_RAD; double max = rotationRangeMax.getRadians() - RANGED_ROTATION_BUFFER_RAD; - + // Calculate distance to padded bounds (using Rotation2d for proper wrapping) double distToMin = Math.abs(MathUtil.angleModulus(current - min)); double distToMax = Math.abs(MathUtil.angleModulus(max - current)); - + double maxAngularAccel = drivetrainConfig.maxAngularAccelerationRadiansPerSecSec; - + // Calculate stopping distance from current velocity: d = ω² / (2α) double stoppingDistFromCurrent = (currentOmega * currentOmega) / (2 * maxAngularAccel); - + // Adjust effective distances based on current velocity direction // If moving towards a boundary, reduce effective distance by stopping distance double effectiveDistToMax = distToMax; double effectiveDistToMin = distToMin; - + if (currentOmega > 0) { // Moving towards max, reduce effective distance to max effectiveDistToMax = Math.max(0, distToMax - stoppingDistFromCurrent); @@ -728,8 +735,9 @@ private double limitOmegaForRange(double desiredOmega) { // Moving towards min (negative omega), reduce effective distance to min effectiveDistToMin = Math.max(0, distToMin - stoppingDistFromCurrent); } - - // sqrt(2*a*d) formula for max velocity to stop at boundary using effective distances + + // sqrt(2*a*d) formula for max velocity to stop at boundary using effective + // distances double maxOmegaToMin = Math.sqrt(2 * maxAngularAccel * effectiveDistToMin); double maxOmegaToMax = Math.sqrt(2 * maxAngularAccel * effectiveDistToMax); @@ -738,7 +746,7 @@ private double limitOmegaForRange(double desiredOmega) { } if (currentRotation.getRadians() > max) { return Math.min(desiredOmega, 0); - } + } // Clamp omega based on direction if (desiredOmega > 0) { @@ -751,20 +759,22 @@ private double limitOmegaForRange(double desiredOmega) { } /** - * Calculates omega to return to rotation range using PID with velocity limiting. - * Uses an internal buffer to target slightly inside the range to prevent oscillation at boundaries. + * Calculates omega to return to rotation range using PID with velocity + * limiting. + * Uses an internal buffer to target slightly inside the range to prevent + * oscillation at boundaries. */ private double calculateReturnToRangeOmega() { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); - + double current = currentRotation.getRadians(); double min = rotationRangeMin.getRadians(); double max = rotationRangeMax.getRadians(); - + // Find closest bound double distToMin = Math.abs(MathUtil.angleModulus(current - min)); double distToMax = Math.abs(MathUtil.angleModulus(current - max)); - + double targetAngle; if (distToMin < distToMax) { // Target slightly inside the min boundary (add buffer) @@ -773,24 +783,22 @@ private double calculateReturnToRangeOmega() { // Target slightly inside the max boundary (subtract buffer) targetAngle = max - RANGED_ROTATION_BUFFER_RAD; } - + // Calculate PID output rangedRotationPIDController.setSetpoint(targetAngle); double pidOutput = rangedRotationPIDController.calculate(current); - + // Apply velocity limit (0.6 of max omega) double maxOmega = drivetrainConfig.maxAngularVelocityRadiansPerSec * RANGED_ROTATION_MAX_VELOCITY_FACTOR; - return MathUtil.clamp(pidOutput, -maxOmega, maxOmega); } // Supplier setters public void setTeleopInputSuppliers( - DoubleSupplier vxNormalized, - DoubleSupplier vyNormalized, - DoubleSupplier omegaNormalized - ) { + DoubleSupplier vxNormalized, + DoubleSupplier vyNormalized, + DoubleSupplier omegaNormalized) { this.vxNormalizedSupplier = vxNormalized; this.vyNormalizedSupplier = vyNormalized; this.omegaNormalizedSupplier = omegaNormalized; @@ -869,23 +877,23 @@ public void setDesiredTranslationOverrideState(DesiredTranslationOverrideState d // Existing drive methods private ChassisSpeeds compensateRobotRelativeSpeeds(ChassisSpeeds speeds) { - Rotation2d angularVelocity = new Rotation2d(speeds.omegaRadiansPerSecond * drivetrainConfig.rotationCompensationCoefficient); + Rotation2d angularVelocity = new Rotation2d( + speeds.omegaRadiansPerSecond * drivetrainConfig.rotationCompensationCoefficient); if (angularVelocity.getRadians() != 0.0) { speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - ChassisSpeeds.fromRobotRelativeSpeeds( // why should this be split into two? - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, - RobotState.getInstance().getEstimatedPose().getRotation().plus(angularVelocity) - ), - RobotState.getInstance().getEstimatedPose().getRotation() - ); + ChassisSpeeds.fromRobotRelativeSpeeds( // why should this be split into two? + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + RobotState.getInstance().getEstimatedPose().getRotation().plus(angularVelocity)), + RobotState.getInstance().getEstimatedPose().getRotation()); } return speeds; } - // return a supplier that is true if the modules are aligned within the tolerance + // return a supplier that is true if the modules are aligned within the + // tolerance private void alignModules(Rotation2d targetRotation, double toleranceDeg) { modulesAlignmentTargetRotation = targetRotation; modulesAlignmentToleranceDeg = toleranceDeg; @@ -898,8 +906,10 @@ private void alignModules(Rotation2d targetRotation, double toleranceDeg) { private boolean areModulesAligned() { for (int i = 0; i < 4; i++) { - if (Math.abs(moduleStates[i].angle.minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg && - Math.abs(moduleStates[i].angle.plus(Rotation2d.fromDegrees(180)).minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg) { + if (Math.abs(moduleStates[i].angle.minus(modulesAlignmentTargetRotation) + .getDegrees()) > modulesAlignmentToleranceDeg && + Math.abs(moduleStates[i].angle.plus(Rotation2d.fromDegrees(180)) + .minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg) { return false; } } @@ -907,7 +917,7 @@ private boolean areModulesAligned() { } public void driveRobotRelative(ChassisSpeeds speeds) { - double dt = Timer.getTimestamp() - prevDriveTime; + double dt = Timer.getTimestamp() - prevDriveTime; prevDriveTime = Timer.getTimestamp(); lastUnoverriddenOmega = speeds.omegaRadiansPerSecond; @@ -915,62 +925,62 @@ public void driveRobotRelative(ChassisSpeeds speeds) { if (shouldOverrideOmega) { desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond, - desiredRobotRelativeSpeeds.vyMetersPerSecond, - omegaOverride - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond, + omegaOverride); } - // TODO: SIM BUG WERE robot kind of goes off in a direction and then control comes back. happens randomly in capped state? + // TODO: SIM BUG WERE robot kind of goes off in a direction and then control + // comes back. happens randomly in capped state? if (shouldOverrideVelocityCap) { double maxVelocity = velocityCapMaxVelocityMetersPerSec; - double currentMagnitude = Math.hypot(desiredRobotRelativeSpeeds.vxMetersPerSecond, desiredRobotRelativeSpeeds.vyMetersPerSecond); + double currentMagnitude = Math.hypot(desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond); if (currentMagnitude > maxVelocity && currentMagnitude > 0) { double scale = maxVelocity / currentMagnitude; desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond * scale, - desiredRobotRelativeSpeeds.vyMetersPerSecond * scale, - desiredRobotRelativeSpeeds.omegaRadiansPerSecond - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond * scale, + desiredRobotRelativeSpeeds.vyMetersPerSecond * scale, + desiredRobotRelativeSpeeds.omegaRadiansPerSecond); } } if (shouldOverrideTranslationalSpeedsFrozen) { ChassisSpeeds frozenFieldRelativeSpeeds = new ChassisSpeeds( - frozenVxMetersPerSec, - frozenVyMetersPerSec, - desiredRobotRelativeSpeeds.omegaRadiansPerSecond - ); + frozenVxMetersPerSec, + frozenVyMetersPerSec, + desiredRobotRelativeSpeeds.omegaRadiansPerSecond); - desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(frozenFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(frozenFieldRelativeSpeeds, + RobotState.getInstance().getEstimatedPose().getRotation()); } - ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, + RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/desiredFieldRelativeSpeeds", desiredFieldRelativeSpeeds); Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); - + // Limit acceleration to prevent sudden changes in speed obtainableFieldRelativeSpeeds = ChassisRateLimiter.limit( - desiredFieldRelativeSpeeds, - obtainableFieldRelativeSpeeds, - dt, - drivetrainConfig.maxTranslationalAccelerationMetersPerSecSec, - drivetrainConfig.maxAngularAccelerationRadiansPerSecSec, - drivetrainConfig.maxTranslationalVelocityMetersPerSec, - drivetrainConfig.maxAngularVelocityRadiansPerSec - ); - + desiredFieldRelativeSpeeds, + obtainableFieldRelativeSpeeds, + dt, + drivetrainConfig.maxTranslationalAccelerationMetersPerSecSec, + drivetrainConfig.maxAngularAccelerationRadiansPerSecSec, + drivetrainConfig.maxTranslationalVelocityMetersPerSec, + drivetrainConfig.maxAngularVelocityRadiansPerSec); + Logger.recordOutput("SwerveDrive/obtainableFieldRelativeSpeeds", obtainableFieldRelativeSpeeds); - ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/obtainableRobotRelativeSpeeds", obtainableRobotRelativeSpeeds); SwerveModuleState[] moduleSetpoints = kinematics.toSwerveModuleStates(obtainableRobotRelativeSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( - moduleSetpoints, - drivetrainConfig.maxModuleVelocity - ); + moduleSetpoints, + drivetrainConfig.maxModuleVelocity); Logger.recordOutput("SwerveDrive/desaturatedModuleSetpoints", moduleSetpoints); for (int i = 0; i < 4; i++) { @@ -981,7 +991,8 @@ public void driveRobotRelative(ChassisSpeeds speeds) { } public void driveFieldRelative(ChassisSpeeds speeds) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, RobotState.getInstance().getEstimatedPose().getRotation()); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, + RobotState.getInstance().getEstimatedPose().getRotation()); driveRobotRelative(speeds); }