From 69ecb9c71ce285a0adce7c43536e59504716f473 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 26 Apr 2026 14:17:56 -0700 Subject: [PATCH 001/114] *OFFSEASON PROJECT* not ready to test --- src/main/deploy/autos/config.json | 11 +++ .../deploy/autos/paths/RT-Neutral-ClimbD.json | 46 ++++++++++ src/main/deploy/autos/paths/example_a.json | 31 +++++++ src/main/deploy/autos/paths/example_b.json | 25 ++++++ src/main/java/frc/robot/Robot.java | 23 ++--- .../frc/robot/subsystems/auto/AutoPath.java | 2 +- .../subsystems/auto/AutonomousField.java | 2 +- .../frc/robot/subsystems/auto/BLineLogic.java | 83 +++++++++++++++++++ .../{AutoLogic.java => PathPlannerLogic.java} | 26 +++--- src/test/java/AutosTest.java | 8 +- vendordeps/BLine-Lib.json | 20 +++++ 11 files changed, 248 insertions(+), 29 deletions(-) create mode 100644 src/main/deploy/autos/config.json create mode 100644 src/main/deploy/autos/paths/RT-Neutral-ClimbD.json create mode 100644 src/main/deploy/autos/paths/example_a.json create mode 100644 src/main/deploy/autos/paths/example_b.json create mode 100644 src/main/java/frc/robot/subsystems/auto/BLineLogic.java rename src/main/java/frc/robot/subsystems/auto/{AutoLogic.java => PathPlannerLogic.java} (92%) create mode 100644 vendordeps/BLine-Lib.json diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json new file mode 100644 index 000000000..4b78885f9 --- /dev/null +++ b/src/main/deploy/autos/config.json @@ -0,0 +1,11 @@ +{ + "robot_length_meters": 2.5, + "robot_width_meters": 0.5, + "default_max_velocity_meters_per_sec": 4.5, + "default_max_acceleration_meters_per_sec2": 7.0, + "default_intermediate_handoff_radius_meters": 0.2, + "default_max_velocity_deg_per_sec": 720.0, + "default_max_acceleration_deg_per_sec2": 1500.0, + "default_end_translation_tolerance_meters": 0.03, + "default_end_rotation_tolerance_deg": 2.0 +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json new file mode 100644 index 000000000..5fa3b0371 --- /dev/null +++ b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json @@ -0,0 +1,46 @@ +{ + "path_elements": [ + { + "type": "translation", + "x_meters": 8.775, + "y_meters": 4.025, + "intermediate_handoff_radius_meters": 0.2 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 11.32350256255179, + "y_meters": 2.1816187050359703, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 15.033587858916528, + "y_meters": 3.3432014388489204, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 15.033587858916528, + "y_meters": 5.992711195645313, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/example_a.json b/src/main/deploy/autos/paths/example_a.json new file mode 100644 index 000000000..5d63f7e0f --- /dev/null +++ b/src/main/deploy/autos/paths/example_a.json @@ -0,0 +1,31 @@ +{ + "path_elements": [ + { + "type": "translation", + "x_meters": 2.0, + "y_meters": 2.0 + }, + { + "type": "rotation", + "rotation_radians": 0.0, + "t_ratio": 0.5, + "profiled_rotation": true + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.0, + "y_meters": 4.0 + }, + "rotation_target": { + "rotation_radians": 0.5, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 10.0, + "y_meters": 6.0 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/example_b.json b/src/main/deploy/autos/paths/example_b.json new file mode 100644 index 000000000..de18c6b94 --- /dev/null +++ b/src/main/deploy/autos/paths/example_b.json @@ -0,0 +1,25 @@ +{ + "path_elements": [ + { + "type": "translation", + "x_meters": 1.0, + "y_meters": 7.5 + }, + { + "type": "translation", + "x_meters": 5.0, + "y_meters": 6.0 + }, + { + "type": "rotation", + "rotation_radians": 1.2, + "t_ratio": 0.5, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 12.5, + "y_meters": 3.0 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d26151c75..6b0692148 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,8 +32,9 @@ import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.AutoLogic; +import frc.robot.subsystems.auto.PathPlannerLogic; import frc.robot.subsystems.auto.AutonomousField; +import frc.robot.subsystems.auto.BLineLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; import frc.robot.util.DriveStateNtLogger; @@ -119,9 +120,11 @@ protected Robot() { controls = new Controls(subsystems, m_simWrapper); if (DRIVEBASE_ENABLED) { - AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem, false); + BLineLogic.init(subsystems); + BLineLogic.configure(subsystems); + BLineLogic.runAuto(); } - AutoLogic.init(subsystems); + // PathPlannerLogic.init(subsystems); if (Robot.isSimulation()) { robotSim = new RobotSim(subsystems.drivebaseSubsystem); } else { @@ -144,11 +147,11 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); if (SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.initCommandsAndPaths(false); - AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); + // PathPlannerLogic.initCommandsAndPaths(true); + // AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); - AutoLogic.initSmartDashBoard(); - CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); + // PathPlannerLogic.initSmartDashBoard(); + //CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); } WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); @@ -282,12 +285,12 @@ public void disabledPeriodic() { @Override public void autonomousInit() { subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); - if (AutoLogic.getSelectedAuto() != null) { + if (Robot.isSimulation()) { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(AutoLogic.getSelectedAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.runAuto()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { @@ -300,7 +303,7 @@ public void autonomousInit() { supplyRobotYawToLimelight(Hardware.LIMELIGHT_C, initialYaw); } } - } + } /** This function is called periodically during autonomous. */ diff --git a/src/main/java/frc/robot/subsystems/auto/AutoPath.java b/src/main/java/frc/robot/subsystems/auto/AutoPath.java index 707931a70..e59429fd8 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoPath.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoPath.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.auto.AutoLogic.StartPosition; +import frc.robot.subsystems.auto.PathPlannerLogic.StartPosition; public class AutoPath { diff --git a/src/main/java/frc/robot/subsystems/auto/AutonomousField.java b/src/main/java/frc/robot/subsystems/auto/AutonomousField.java index 50f311a52..d685134c9 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/AutonomousField.java @@ -39,7 +39,7 @@ public static void initSmartDashBoard( addPeriodic.accept( () -> { - autonomousField.update(AutoLogic.getSelectedAutoName()); + autonomousField.update(PathPlannerLogic.getSelectedAutoName()); SmartDashboard.putNumber( "Est. Time (s)", Math.round(autonomousField.autoTotalTime() * 100.0) / 100.0); }, diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java new file mode 100644 index 000000000..b907c29df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -0,0 +1,83 @@ +package frc.robot.subsystems.auto; + import frc.robot.Subsystems; + import frc.robot.lib.BLine.FollowPath; + import frc.robot.lib.BLine.Path; + +import java.util.function.Consumer; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +public class BLineLogic { +private static Subsystems s; + private Consumer chassisConsumer; + +private static FollowPath.Builder pathBuilder; + + public static void init(Subsystems subsystems) { + + s = subsystems; + s.drivebaseSubsystem.resetPose(getStartPose()); + } + + +// 2. Create a reusable path builder +public static void configure(Subsystems s) { + + pathBuilder = new FollowPath.Builder( + s.drivebaseSubsystem, + + // pose supplier + () -> s.drivebaseSubsystem.getState().Pose, + + // robot-relative speeds supplier + () -> s.drivebaseSubsystem.getState().Speeds, + + // robot-relative speeds consumer + (speeds) -> { + s.drivebaseSubsystem.setControl( + new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + ); + }, + + // translation PID + new PIDController(5.0, 0.0, 0.0), + + // rotation PID + new PIDController(3.0, 0.0, 0.0), + + // cross-track PID + new PIDController(2.0, 0.0, 0.0) + ) + .withDefaultShouldFlip() + .withPoseReset((pose) -> getStartPose()); + + }; +public static Command runAuto() { + +Path myPath = new Path("RT-Neutral-ClimbD"); // loads deploy/autos/paths/myPathFile.json + +return pathBuilder.build(myPath); + +} +public static Pose2d getStartPose() { + +Path myPath = new Path("RT-Neutral-ClimbD"); // loads deploy/autos/paths/myPathFile.json + +return myPath.getStartPose(); + +} +public static FollowPath.Builder getBuilder() { + + return pathBuilder; +} + + + +} + + diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java similarity index 92% rename from src/main/java/frc/robot/subsystems/auto/AutoLogic.java rename to src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java index 7b7295aa8..615b6d0a0 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java @@ -28,7 +28,7 @@ import java.util.Map; import org.json.simple.parser.ParseException; -public class AutoLogic { +public class PathPlannerLogic { private static Subsystems s; @@ -112,18 +112,18 @@ private static void initPaths() { physicalRebuiltPaths = List.of( - new AutoPath("C-Outpost-Depot", "C-Outpost-Depot"), - new AutoPath("LeftTrench-Depot", "LeftTrench-Depot"), - new AutoPath("LT-Neutral-Depot", "LT-Neutral-Depot"), - new AutoPath("LT-Neutral", "LT-Neutral"), - new AutoPath("LT-DoubleNeutral", "LT-DoubleNeutral"), - new AutoPath("RightTrench-Outpost", "RightTrench-Outpost"), - new AutoPath("RT-Neutral-Outpost", "RT-Neutral-Outpost"), - new AutoPath("Rotate-RT-Neutral", "Rotate-RT-Neutral"), - new AutoPath("RT-Neutral", "RT-Neutral"), - new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), - new AutoPath("RT-BLOCK", "RT-BLOCK"), - new AutoPath("LT-BLOCK", "LT-BLOCK")); + new AutoPath("C-Outpost-Depot", "C-Outpost-Depot"), + new AutoPath("LeftTrench-Depot", "LeftTrench-Depot"), + new AutoPath("LT-Neutral-Depot", "LT-Neutral-Depot"), + new AutoPath("LT-Neutral", "LT-Neutral"), + new AutoPath("LT-DoubleNeutral", "LT-DoubleNeutral"), + new AutoPath("RightTrench-Outpost", "RightTrench-Outpost"), + new AutoPath("RT-Neutral-Outpost", "RT-Neutral-Outpost"), + new AutoPath("Rotate-RT-Neutral", "Rotate-RT-Neutral"), + new AutoPath("RT-Neutral", "RT-Neutral"), + new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), + new AutoPath("RT-BLOCK", "RT-BLOCK"), + new AutoPath("LT-BLOCK", "LT-BLOCK")); rebuiltPaths = physicalRebuiltPaths; diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index ca88d3e05..edeecb05e 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -3,7 +3,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc.robot.generated.CompTunerConstants; import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.AutoLogic; +import frc.robot.subsystems.auto.PathPlannerLogic; import frc.robot.subsystems.auto.AutoPath; import java.io.IOException; import java.util.Dictionary; @@ -15,7 +15,7 @@ class AutosTest { - @BeforeEach + // @BeforeEach void setup() { AutoBuilderConfig.buildAuto(CompTunerConstants.createDrivetrain(), true); @@ -23,10 +23,10 @@ void setup() { @Test void validateFileName() throws IOException, ParseException { - AutoLogic.initCommandsAndPaths(true); + PathPlannerLogic.initCommandsAndPaths(true); Dictionary pathDictionary = new Hashtable<>(); - for (AutoPath path : AutoLogic.getAutos()) { // Every auto in the auto chooser + for (AutoPath path : PathPlannerLogic.getAutos()) { // Every auto in the auto chooser pathDictionary.put( path.getAutoName(), false); // Putting each auto from the auto chooser in dictionary } diff --git a/vendordeps/BLine-Lib.json b/vendordeps/BLine-Lib.json new file mode 100644 index 000000000..3e6a4877a --- /dev/null +++ b/vendordeps/BLine-Lib.json @@ -0,0 +1,20 @@ +{ + "fileName": "BLine-Lib.json", + "name": "BLine-Lib", + "version": "v0.8.4", + "frcYear": "2026", + "uuid": "4b7270e9-4e8d-4e7b-8cf0-5805f12c3c7d", + "mavenUrls": [ + "https://jitpack.io" + ], + "jsonUrl": "https://raw.githubusercontent.com/edanliahovetsky/BLine-Lib/main/BLine-Lib.json", + "javaDependencies": [ + { + "groupId": "com.github.edanliahovetsky", + "artifactId": "BLine-Lib", + "version": "v0.8.4" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From 53353223663819970311693a5265e8d5978c10de Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 26 Apr 2026 21:50:54 -0700 Subject: [PATCH 002/114] one ish path for practice --- src/main/deploy/autos/config.json | 32 ++++-- src/main/deploy/autos/paths/LT-Neutral.json | 103 ++++++++++++++++++ .../deploy/autos/paths/RT-Neutral-ClimbD.json | 62 +++++------ .../paths/Back-RightTrench-Backwards.path | 2 +- .../pathplanner/paths/Mid-Intake-Right.path | 2 +- .../deploy/pathplanner/paths/rt-block.path | 2 +- src/main/deploy/pathplanner/settings.json | 4 +- .../frc/robot/subsystems/auto/BLineLogic.java | 12 +- 8 files changed, 168 insertions(+), 51 deletions(-) create mode 100644 src/main/deploy/autos/paths/LT-Neutral.json diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 4b78885f9..a7398b2cb 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -1,11 +1,25 @@ { - "robot_length_meters": 2.5, - "robot_width_meters": 0.5, - "default_max_velocity_meters_per_sec": 4.5, - "default_max_acceleration_meters_per_sec2": 7.0, - "default_intermediate_handoff_radius_meters": 0.2, - "default_max_velocity_deg_per_sec": 720.0, - "default_max_acceleration_deg_per_sec2": 1500.0, - "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0 + "gui": { + "robot": { + "length_meters": 0.762, + "width_meters": 0.61 + }, + "protrusions": { + "enabled": true, + "distance_meters": 0.2, + "side": "back", + "default_state": "shown", + "show_on_event_keys": [], + "hide_on_event_keys": [] + } + }, + "kinematic_constraints": { + "default_max_velocity_meters_per_sec": 4.5, + "default_max_acceleration_meters_per_sec2": 4.0, + "default_intermediate_handoff_radius_meters": 0.2, + "default_max_velocity_deg_per_sec": 720.0, + "default_max_acceleration_deg_per_sec2": 1500.0, + "default_end_translation_tolerance_meters": 0.03, + "default_end_rotation_tolerance_deg": 5.0 + } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/LT-Neutral.json b/src/main/deploy/autos/paths/LT-Neutral.json new file mode 100644 index 000000000..f8d8754ac --- /dev/null +++ b/src/main/deploy/autos/paths/LT-Neutral.json @@ -0,0 +1,103 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 7.597, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -3.141592653589793, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 4.914457875253195, + "y_meters": 7.6457764475798005, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "rotation", + "rotation_radians": 1.5707963267948966, + "t_ratio": 0.33413521282430825, + "profiled_rotation": true + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.631800949919661, + "y_meters": 7.086878075015717, + "intermediate_handoff_radius_meters": 1.0 + }, + "rotation_target": { + "rotation_radians": 1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.681312467695745, + "y_meters": 6.191807599357407, + "intermediate_handoff_radius_meters": 1.0 + }, + "rotation_target": { + "rotation_radians": 1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.644575749109444, + "y_meters": 4.553336131871205, + "intermediate_handoff_radius_meters": 1.0 + }, + "rotation_target": { + "rotation_radians": 1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": 3.141592653589793, + "t_ratio": 0.6055237118270185, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 6.978390710344346, + "y_meters": 5.730428539498499, + "intermediate_handoff_radius_meters": 1.5 + }, + { + "type": "translation", + "x_meters": 6.532203101208354, + "y_meters": 6.4339075085562625, + "intermediate_handoff_radius_meters": 2.0 + }, + { + "type": "translation", + "x_meters": 5.881927792135226, + "y_meters": 6.703695061814626, + "intermediate_handoff_radius_meters": 2.0 + }, + { + "type": "translation", + "x_meters": 3.6053986030593093, + "y_meters": 6.63312832297269, + "intermediate_handoff_radius_meters": 2.0 + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 2.5, + "start_ordinal": 2, + "end_ordinal": 4 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json index 5fa3b0371..702f61dc8 100644 --- a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json +++ b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json @@ -1,46 +1,46 @@ { "path_elements": [ - { - "type": "translation", - "x_meters": 8.775, - "y_meters": 4.025, - "intermediate_handoff_radius_meters": 0.2 - }, { "type": "waypoint", "translation_target": { - "x_meters": 11.32350256255179, - "y_meters": 2.1816187050359703, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 4.013, + "y_meters": 7.597, + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true + "rotation_radians": 1.5707963267948966, + "profiled_rotation": false } }, { - "type": "waypoint", - "translation_target": { - "x_meters": 15.033587858916528, - "y_meters": 3.3432014388489204, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } + "type": "translation", + "x_meters": 7.138146396292919, + "y_meters": 7.413526838631876, + "intermediate_handoff_radius_meters": 1.75 }, { - "type": "waypoint", - "translation_target": { - "x_meters": 15.033587858916528, - "y_meters": 5.992711195645313, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } + "type": "translation", + "x_meters": 7.35500242247247, + "y_meters": 5.702217557528438, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "translation", + "x_meters": 7.1295225440923655, + "y_meters": 6.138367623280026, + "intermediate_handoff_radius_meters": 2.0 + }, + { + "type": "translation", + "x_meters": 6.134621366959286, + "y_meters": 7.3150814836381235, + "intermediate_handoff_radius_meters": 1.5 + }, + { + "type": "translation", + "x_meters": 3.2649272760287475, + "y_meters": 6.432707556033254, + "intermediate_handoff_radius_meters": 0.2 } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path b/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path index 0faa0196f..c314ad181 100644 --- a/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path +++ b/src/main/deploy/pathplanner/paths/Back-RightTrench-Backwards.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 7.073284731962036, - "y": 0.45538039519202234 + "y": 0.4553803951920222 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path b/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path index dd366b9ae..180fe2b2c 100644 --- a/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path +++ b/src/main/deploy/pathplanner/paths/Mid-Intake-Right.path @@ -69,7 +69,7 @@ }, "prevControl": { "x": 6.672715268037964, - "y": 0.7546196048079776 + "y": 0.7546196048079777 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/rt-block.path b/src/main/deploy/pathplanner/paths/rt-block.path index ad1f5b8d0..834917506 100644 --- a/src/main/deploy/pathplanner/paths/rt-block.path +++ b/src/main/deploy/pathplanner/paths/rt-block.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 7.5574063670411995, - "y": 0.9227996254681647 + "y": 0.9227996254681649 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index bb094b88f..05a7b5882 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.936625, - "robotLength": 0.78105, + "robotWidth": 0.937, + "robotLength": 0.781, "holonomicMode": true, "pathFolders": [], "autoFolders": [ diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index b907c29df..108271c01 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -45,28 +45,28 @@ public static void configure(Subsystems s) { }, // translation PID - new PIDController(5.0, 0.0, 0.0), + new PIDController(3.0, 0.0, 0.0), // rotation PID - new PIDController(3.0, 0.0, 0.0), + new PIDController(5.0, 0.0, 0.0), // cross-track PID new PIDController(2.0, 0.0, 0.0) ) - .withDefaultShouldFlip() - .withPoseReset((pose) -> getStartPose()); + .withDefaultShouldFlip(). + withPoseReset(s.drivebaseSubsystem::resetPose); }; public static Command runAuto() { -Path myPath = new Path("RT-Neutral-ClimbD"); // loads deploy/autos/paths/myPathFile.json +Path myPath = new Path("LT-Neutral"); // loads deploy/autos/paths/myPathFile.json return pathBuilder.build(myPath); } public static Pose2d getStartPose() { -Path myPath = new Path("RT-Neutral-ClimbD"); // loads deploy/autos/paths/myPathFile.json +Path myPath = new Path("LT-Neutral"); // loads deploy/autos/paths/myPathFile.json return myPath.getStartPose(); From 926fbe32da78523a5134ca35b41ef2fa7d566bf3 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 26 Apr 2026 22:19:17 -0700 Subject: [PATCH 003/114] odometry resetting works --- .../frc/robot/subsystems/auto/BLineLogic.java | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index 108271c01..de7deb31b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -9,16 +9,19 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class BLineLogic { private static Subsystems s; - private Consumer chassisConsumer; -private static FollowPath.Builder pathBuilder; +private static FollowPath.Builder pathBuilder; +private static Path myPath; public static void init(Subsystems subsystems) { - + myPath = new Path("LT-Neutral"); s = subsystems; s.drivebaseSubsystem.resetPose(getStartPose()); } @@ -40,7 +43,7 @@ public static void configure(Subsystems s) { (speeds) -> { s.drivebaseSubsystem.setControl( new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) ); }, @@ -53,22 +56,22 @@ public static void configure(Subsystems s) { // cross-track PID new PIDController(2.0, 0.0, 0.0) ) - .withDefaultShouldFlip(). - withPoseReset(s.drivebaseSubsystem::resetPose); + .withDefaultShouldFlip().withPoseReset(s.drivebaseSubsystem::resetPose); }; -public static Command runAuto() { +public static Command runAuto() { -Path myPath = new Path("LT-Neutral"); // loads deploy/autos/paths/myPathFile.json -return pathBuilder.build(myPath); + + return pathBuilder.build(myPath); + // ... rest of auto } public static Pose2d getStartPose() { -Path myPath = new Path("LT-Neutral"); // loads deploy/autos/paths/myPathFile.json -return myPath.getStartPose(); + +return myPath.getStartPose(Rotation2d.fromDegrees(90)); } public static FollowPath.Builder getBuilder() { From 239b7676d852f615aba81413eb82f49cb3d4e08a Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 27 Apr 2026 22:50:37 -0700 Subject: [PATCH 004/114] 250 --- .../subsystems/launcher/LaunchCalculator.java | 105 ++++++++++++++---- 1 file changed, 81 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java index a9fa467c0..9f4b5aa75 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java @@ -45,6 +45,11 @@ public static LaunchCalculator getInstance() { private double lastWheelSpeedsTimestamp = 0; private ChassisSpeeds filteredAcceleration = new ChassisSpeeds(0, 0, 0); + // Hysteresis state for robotIsMoving gate. + // Requires a higher speed to turn ON than to turn OFF, preventing rapid toggling at the + // boundary and the stale-baseline spike on start/stop. + private boolean robotIsMoving = false; + // Throttling Magic numbers private static final double MIN_DIST_TOLERANCE = Units.inchesToMeters(1); // Meters private static final double MIN_ROTATION_TOLERANCE = Units.degreesToRadians(0.5); // Radians @@ -66,22 +71,41 @@ public static LaunchCalculator getInstance() { private static final double VFF_DIST_TOLERANCE = 0.1; private static final double MIN_DISTANCE_TO_TARGET = 1e-4; - // Pose-differentiation velocity blending. - // Maximum blend weight when poseDt is at its minimum (most trustworthy). - // Only active when the robot is actually moving — see robotIsMoving gate below. - private static final double POSE_VELOCITY_BLEND = 0.9; - // Sanity bounds for pose-differentiated velocity. private static final double MIN_POSE_DT = 0.005; // seconds private static final double MAX_POSE_DT = 0.05; // seconds - // Gate for pose-derived velocity. Below this wheel speed magnitude we consider the - // robot stationary and skip pose-diff to avoid amplifying estimator noise into jitter. - private static final double MIN_MOVING_SPEED = 0.05; // m/s + // Hysteresis thresholds for the robotIsMoving gate. + // Must reach ENABLE threshold to activate; must drop below DISABLE threshold to deactivate. + // The gap prevents rapid toggling and the stale-baseline spike on start/stop. + private static final double MIN_MOVING_SPEED_ENABLE = 0.1; // m/s + private static final double MIN_MOVING_SPEED_DISABLE = 0.03; // m/s // Low-pass filter for acceleration. 0 = no filtering, 1 = effectively disabled. private static final double ACCEL_FILTER_ALPHA = 0.9; + // Variable-alpha low-pass filter for the slip estimate. Vision-correction noise and + // real wheel slip live in different magnitude regimes: + // - Vision noise: <1 cm correction over 20 ms, single-frame transients. + // - Real stuck/push: wheels at 2 m/s vs robot at 0 = 2+ m/s, sustained over many frames. + // We use a heavy filter (slow) below the threshold to reject noise, and a light filter + // (fast) above it to track real events with minimal lag. Hysteresis on the threshold + // prevents flicker between modes. + private static final double SLIP_FILTER_ALPHA_SLOW = 0.85; // noise rejection + private static final double SLIP_FILTER_ALPHA_FAST = 0.20; // fast tracking + private static final double FAST_SLIP_THRESHOLD_ENTER = 2.0; // m/s + private static final double FAST_SLIP_THRESHOLD_EXIT = 0.5; // m/s (hysteresis) + private boolean slipFastMode = false; + + // Safety caps on the filtered slip. Prevents a runaway pose-estimator glitch, such as + // a bad vision measurement that spikes the derivative, from injecting nonsense into the + // calculation. Real robot pushes and rotational disturbances stay well below these. + private static final double MAX_SLIP_MAGNITUDE = 5.0; // m/s + private static final double MAX_SLIP_OMEGA = 8.0; // rad/s (~1.3 rotations/s) + + // Filtered slip state (instance, persists across cycles) + private ChassisSpeeds filteredSlip = new ChassisSpeeds(0, 0, 0); + // Trench stuff private static final AprilTagFieldLayout field = AllianceUtils.FIELD_LAYOUT; private static final double TURRET_TO_TRENCH_TOLERANCE_X = Units.inchesToMeters(12); @@ -244,9 +268,10 @@ public LaunchingParameters getParameters( * of phase delay (launchSpeeds) is treated as constant throughout the Newton loop — post-launch * robot acceleration does not affect ball trajectory. * - *

2. DEFENSE/PUSH COMPENSATION: Pose-differentiated velocity blended with wheel speeds. Blend - * weight scales dynamically with poseDt and is gated off when stationary to prevent estimator - * noise from being amplified into a fake jittery velocity signal. + *

2. DEFENSE/PUSH COMPENSATION: Slip filter blended on top of wheel speeds. The slip is the + * difference between pose-derived velocity and wheel velocity. A variable-alpha low-pass filter + * separates real push events (large, sustained slip) from vision noise (small, transient). Gated + * off when stationary to prevent estimator noise from being amplified into jitter. * * @param driveState the drivebase's SwerveDriveState * @param turretSubsystem the turretSubsystem object @@ -259,29 +284,61 @@ public LaunchingParameters calculate( ChassisSpeeds wheelSpeeds = driveState.Speeds; double timestamp = driveState.Timestamp; - // --- DEFENSE COMPENSATION: Pose-differentiated velocity with dynamic blending --- - // Gate: only use pose-derived velocity when the robot is actually moving. + // --- DEFENSE COMPENSATION: Slip filter with hysteresis gate --- // When still, tiny estimator corrections divided by a small dt produce large fake velocities. double wheelSpeedMagnitude = Math.hypot(wheelSpeeds.vxMetersPerSecond, wheelSpeeds.vyMetersPerSecond); - boolean robotIsMoving = wheelSpeedMagnitude > MIN_MOVING_SPEED; + + if (robotIsMoving) { + robotIsMoving = wheelSpeedMagnitude > MIN_MOVING_SPEED_DISABLE; + } else { + robotIsMoving = wheelSpeedMagnitude > MIN_MOVING_SPEED_ENABLE; + } ChassisSpeeds effectiveSpeeds = wheelSpeeds; double poseDt = timestamp - prevTimestamp; if (robotIsMoving && poseDt > MIN_POSE_DT && poseDt < MAX_POSE_DT) { Twist2d twist = prevPose.log(estimatedPose); - ChassisSpeeds poseDerivedSpeeds = - new ChassisSpeeds(twist.dx / poseDt, twist.dy / poseDt, twist.dtheta / poseDt); - double blendAlpha = - POSE_VELOCITY_BLEND * (1.0 - (poseDt - MIN_POSE_DT) / (MAX_POSE_DT - MIN_POSE_DT)); + + // Slip = (pose-derived velocity) - (wheel velocity). When wheels match pose, + // slip is ~0; when the robot is pushed or wheels slip, slipRaw is the motion delta. + double slipRawX = twist.dx / poseDt - wheelSpeeds.vxMetersPerSecond; + double slipRawY = twist.dy / poseDt - wheelSpeeds.vyMetersPerSecond; + double slipRawOmega = twist.dtheta / poseDt - wheelSpeeds.omegaRadiansPerSecond; + + // Switch to fast tracking when slip is large enough to be a real event + // rather than vision noise. Hysteresis prevents flicker. + double slipRawMag = Math.hypot(slipRawX, slipRawY); + slipFastMode = + slipRawMag > FAST_SLIP_THRESHOLD_EXIT + ? slipRawMag > FAST_SLIP_THRESHOLD_ENTER + : slipFastMode; + double filterAlpha = slipFastMode ? SLIP_FILTER_ALPHA_FAST : SLIP_FILTER_ALPHA_SLOW; + + double newSlipX = filterAlpha * filteredSlip.vxMetersPerSecond + (1 - filterAlpha) * slipRawX; + double newSlipY = filterAlpha * filteredSlip.vyMetersPerSecond + (1 - filterAlpha) * slipRawY; + double newSlipOmega = + filterAlpha * filteredSlip.omegaRadiansPerSecond + (1 - filterAlpha) * slipRawOmega; + + // Cap the filtered slip magnitude + double newSlipMag = Math.hypot(newSlipX, newSlipY); + if (newSlipMag > MAX_SLIP_MAGNITUDE) { + double scale = MAX_SLIP_MAGNITUDE / newSlipMag; + newSlipX *= scale; + newSlipY *= scale; + } + newSlipOmega = Math.max(-MAX_SLIP_OMEGA, Math.min(MAX_SLIP_OMEGA, newSlipOmega)); + filteredSlip = new ChassisSpeeds(newSlipX, newSlipY, newSlipOmega); + effectiveSpeeds = new ChassisSpeeds( - blendAlpha * poseDerivedSpeeds.vxMetersPerSecond - + (1.0 - blendAlpha) * wheelSpeeds.vxMetersPerSecond, - blendAlpha * poseDerivedSpeeds.vyMetersPerSecond - + (1.0 - blendAlpha) * wheelSpeeds.vyMetersPerSecond, - blendAlpha * poseDerivedSpeeds.omegaRadiansPerSecond - + (1.0 - blendAlpha) * wheelSpeeds.omegaRadiansPerSecond); + wheelSpeeds.vxMetersPerSecond + filteredSlip.vxMetersPerSecond, + wheelSpeeds.vyMetersPerSecond + filteredSlip.vyMetersPerSecond, + wheelSpeeds.omegaRadiansPerSecond + filteredSlip.omegaRadiansPerSecond); + } else { + // Reset slip filter when robot stops or poseDt is out of range + filteredSlip = new ChassisSpeeds(0, 0, 0); + slipFastMode = false; } // --- PHASE DELAY PREDICTION (second-order) --- From 58d5589ca485ab59c988041d1771b322d27fefcd Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 27 Apr 2026 22:56:26 -0700 Subject: [PATCH 005/114] updated venderdeps --- ...enix6-26.1.3.json => Phoenix6-26.2.0.json} | 62 +++++++++---------- vendordeps/photonlib.json | 12 ++-- 2 files changed, 37 insertions(+), 37 deletions(-) rename vendordeps/{Phoenix6-26.1.3.json => Phoenix6-26.2.0.json} (92%) diff --git a/vendordeps/Phoenix6-26.1.3.json b/vendordeps/Phoenix6-26.2.0.json similarity index 92% rename from vendordeps/Phoenix6-26.1.3.json rename to vendordeps/Phoenix6-26.2.0.json index d5bc4a240..e4bde9628 100644 --- a/vendordeps/Phoenix6-26.1.3.json +++ b/vendordeps/Phoenix6-26.2.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.3.json", + "fileName": "Phoenix6-26.2.0.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.3", + "version": "26.2.0", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.3" + "version": "26.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.3", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.3", + "version": "26.2.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 6f42ae3f7..6d1e8174c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.3.3", + "version": "v2026.3.4", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.3.3", + "version": "v2026.3.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.3.3", + "version": "v2026.3.4", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.3.3", + "version": "v2026.3.4", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.3.3" + "version": "v2026.3.4" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.3.3" + "version": "v2026.3.4" } ] } \ No newline at end of file From d62e58a2dad855c941852a6c298a7381c4bcd276 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 27 Apr 2026 23:01:34 -0700 Subject: [PATCH 006/114] gemini --- .../frc/robot/subsystems/launcher/LaunchCalculator.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java index 9f4b5aa75..25d83c93b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java @@ -309,10 +309,11 @@ public LaunchingParameters calculate( // Switch to fast tracking when slip is large enough to be a real event // rather than vision noise. Hysteresis prevents flicker. double slipRawMag = Math.hypot(slipRawX, slipRawY); - slipFastMode = - slipRawMag > FAST_SLIP_THRESHOLD_EXIT - ? slipRawMag > FAST_SLIP_THRESHOLD_ENTER - : slipFastMode; + if (slipRawMag > FAST_SLIP_THRESHOLD_ENTER) { + slipFastMode = true; + } else if (slipRawMag < FAST_SLIP_THRESHOLD_EXIT) { + slipFastMode = false; + } double filterAlpha = slipFastMode ? SLIP_FILTER_ALPHA_FAST : SLIP_FILTER_ALPHA_SLOW; double newSlipX = filterAlpha * filteredSlip.vxMetersPerSecond + (1 - filterAlpha) * slipRawX; From abeee4bc89642a1deea37d1555a6e12349a62d34 Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 08:21:49 -0700 Subject: [PATCH 007/114] fudged numbers for worlds --- .../robot/util/tuning/LauncherConstants.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/util/tuning/LauncherConstants.java b/src/main/java/frc/robot/util/tuning/LauncherConstants.java index f20cdbbdf..87033b3cd 100644 --- a/src/main/java/frc/robot/util/tuning/LauncherConstants.java +++ b/src/main/java/frc/robot/util/tuning/LauncherConstants.java @@ -62,16 +62,16 @@ public String toString() { private static final LauncherDistanceDataPoint[] compDistanceData = { new LauncherDistanceDataPoint(1, 1.5, 40, 1.018), - new LauncherDistanceDataPoint(1.5, 2, 42, 1.138), - new LauncherDistanceDataPoint(2, 3, 43, 1.104), - new LauncherDistanceDataPoint(2.55, 3.5, 45, 1.204), - new LauncherDistanceDataPoint(3.2, 4.5, 48, 1.15), - new LauncherDistanceDataPoint(3.5, 4.8, 51, 1.229), - new LauncherDistanceDataPoint(3.75, 5, 51.5, 1.217), - new LauncherDistanceDataPoint(4.2, 5.125, 53.5, 1.275), - new LauncherDistanceDataPoint(4.5, 5.5, 58, 1.305), - new LauncherDistanceDataPoint(5, 5.8, 60, 1.349), - new LauncherDistanceDataPoint(5.8, 6.6, 64, 1.378), + new LauncherDistanceDataPoint(1.5, 2.2, 45, 1.138), + new LauncherDistanceDataPoint(2, 3.4, 48, 1.104), + new LauncherDistanceDataPoint(2.55, 3.5, 50, 1.204), + new LauncherDistanceDataPoint(3.2, 4.5, 52, 1.15), + new LauncherDistanceDataPoint(3.5, 4.8, 55, 1.229), + new LauncherDistanceDataPoint(3.75, 5, 55.5, 1.217), + new LauncherDistanceDataPoint(4.2, 5.125, 57.5, 1.275), + new LauncherDistanceDataPoint(4.5, 5.5, 62, 1.305), + new LauncherDistanceDataPoint(5, 5.8, 64, 1.349), + new LauncherDistanceDataPoint(5.8, 6.6, 68, 1.378), new LauncherDistanceDataPoint(8, 9, 90, 1.53) }; From c59a204ce82ea5e35fb6e529a522ec7485ab8e71 Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 08:27:23 -0700 Subject: [PATCH 008/114] turretskipped saftey --- src/main/java/frc/robot/Controls.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 8042d8165..c0054344a 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -90,6 +90,7 @@ public class Controls { public static IntakeMode intakeMode = IntakeMode.RETRACTED; public static boolean turretKillActive = false; + public static boolean turretSkipped = false; public static final double MaxSpeed = (RobotType.TYPE == RobotTypesEnum.ALPHA) @@ -283,7 +284,7 @@ private void configureLauncherBindings() { .withVelocityX(getDriveX()) .withVelocityY(getDriveY()) .withRotationalRate(getDriveRotate())), - () -> turretKillActive), + () -> (turretKillActive && !turretSkipped)), s.launcherSubsystem.launcherAimCommand(), Commands.runOnce(() -> ledsMode = LEDMode.LAUNCHING), Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) @@ -491,6 +492,8 @@ private void configureTurretBindings() { .onTrue( Commands.runOnce(() -> turretKillActive = !turretKillActive) .withName("Toggle Turret Kill")); + driverController.povRight().onTrue(Commands.runOnce(() -> turretSkipped = !turretSkipped).withName("Toggle Turret Skipped")); + connected(turretTestController) .and(turretTestController.povUp()) .onTrue(s.turretSubsystem.setTurretPosition(TurretSubsystem.FRONT_POSITION)); From d88b20d592e606564926b4c783a333f05b88a29d Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 08:30:35 -0700 Subject: [PATCH 009/114] rezero only on povLeft --- src/main/java/frc/robot/Controls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index c0054344a..bd8dcfa8f 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -476,7 +476,7 @@ private void configureTurretBindings() { s.turretSubsystem.setDefaultCommand( s.turretSubsystem.rotateToTargetWithCalc().withName("Turret Default Command")); - turretAtZero.onTrue( + turretAtZero.and(driverController.povLeft()).onTrue( Commands.runOnce(() -> s.turretSubsystem.zeroTurretPosistion()) .withName("Zero Turret on Limit Switch")); From 79d1d671733d287eb0c3858d3c1eb649226b5318 Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 09:11:59 -0700 Subject: [PATCH 010/114] better rezero --- src/main/java/frc/robot/Controls.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index bd8dcfa8f..f1be2f2ca 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -476,9 +476,11 @@ private void configureTurretBindings() { s.turretSubsystem.setDefaultCommand( s.turretSubsystem.rotateToTargetWithCalc().withName("Turret Default Command")); - turretAtZero.and(driverController.povLeft()).onTrue( - Commands.runOnce(() -> s.turretSubsystem.zeroTurretPosistion()) - .withName("Zero Turret on Limit Switch")); + (turretAtZero + .and(new Trigger(()-> s.turretSubsystem.getTurretPosition() < 0.5))).or(driverController.povLeft()) + .onTrue( + Commands.runOnce(() -> s.turretSubsystem.zeroTurretPosistion()) + .withName("Zero Turret on Limit Switch")); driverController .y() @@ -492,7 +494,11 @@ private void configureTurretBindings() { .onTrue( Commands.runOnce(() -> turretKillActive = !turretKillActive) .withName("Toggle Turret Kill")); - driverController.povRight().onTrue(Commands.runOnce(() -> turretSkipped = !turretSkipped).withName("Toggle Turret Skipped")); + driverController + .povRight() + .onTrue( + Commands.runOnce(() -> turretSkipped = !turretSkipped) + .withName("Toggle Turret Skipped")); connected(turretTestController) .and(turretTestController.povUp()) From ceb2878d581be0699602099eef66028dfe2886dc Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 09:15:00 -0700 Subject: [PATCH 011/114] more fugding --- .../frc/robot/util/tuning/LauncherConstants.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/util/tuning/LauncherConstants.java b/src/main/java/frc/robot/util/tuning/LauncherConstants.java index 87033b3cd..ad7458b3f 100644 --- a/src/main/java/frc/robot/util/tuning/LauncherConstants.java +++ b/src/main/java/frc/robot/util/tuning/LauncherConstants.java @@ -62,12 +62,12 @@ public String toString() { private static final LauncherDistanceDataPoint[] compDistanceData = { new LauncherDistanceDataPoint(1, 1.5, 40, 1.018), - new LauncherDistanceDataPoint(1.5, 2.2, 45, 1.138), - new LauncherDistanceDataPoint(2, 3.4, 48, 1.104), - new LauncherDistanceDataPoint(2.55, 3.5, 50, 1.204), - new LauncherDistanceDataPoint(3.2, 4.5, 52, 1.15), - new LauncherDistanceDataPoint(3.5, 4.8, 55, 1.229), - new LauncherDistanceDataPoint(3.75, 5, 55.5, 1.217), + new LauncherDistanceDataPoint(1.5, 2.2, 43, 1.138), + new LauncherDistanceDataPoint(2, 3.4, 46, 1.104), + new LauncherDistanceDataPoint(2.55, 3.5, 48, 1.204), + new LauncherDistanceDataPoint(3.2, 4.5, 51, 1.15), + new LauncherDistanceDataPoint(3.5, 4.8, 54, 1.229), + new LauncherDistanceDataPoint(3.75, 5, 54.5, 1.217), new LauncherDistanceDataPoint(4.2, 5.125, 57.5, 1.275), new LauncherDistanceDataPoint(4.5, 5.5, 62, 1.305), new LauncherDistanceDataPoint(5, 5.8, 64, 1.349), From 8a497313c6886af8cab8165ddc90275c538cd109 Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 11:53:28 -0700 Subject: [PATCH 012/114] updated launcher constants --- src/main/java/frc/robot/util/tuning/LauncherConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/util/tuning/LauncherConstants.java b/src/main/java/frc/robot/util/tuning/LauncherConstants.java index ad7458b3f..5e733d5d3 100644 --- a/src/main/java/frc/robot/util/tuning/LauncherConstants.java +++ b/src/main/java/frc/robot/util/tuning/LauncherConstants.java @@ -69,9 +69,9 @@ public String toString() { new LauncherDistanceDataPoint(3.5, 4.8, 54, 1.229), new LauncherDistanceDataPoint(3.75, 5, 54.5, 1.217), new LauncherDistanceDataPoint(4.2, 5.125, 57.5, 1.275), - new LauncherDistanceDataPoint(4.5, 5.5, 62, 1.305), + new LauncherDistanceDataPoint(4.5, 5.5, 60, 1.305), new LauncherDistanceDataPoint(5, 5.8, 64, 1.349), - new LauncherDistanceDataPoint(5.8, 6.6, 68, 1.378), + new LauncherDistanceDataPoint(5.8, 6.6, 67, 1.378), new LauncherDistanceDataPoint(8, 9, 90, 1.53) }; From 05bee4106d36b3d97cb69f31dae50a1de9a43bc4 Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Thu, 30 Apr 2026 14:04:53 -0700 Subject: [PATCH 013/114] disable slip checking for now --- .../robot/subsystems/launcher/LaunchCalculator.java | 9 ++++++++- .../frc/robot/util/tuning/LauncherConstants.java | 12 ++++++------ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java index 25d83c93b..b61ce3a0b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java @@ -10,6 +10,8 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import frc.robot.util.AllianceUtils; import frc.robot.util.GetTargetFromPose; @@ -105,6 +107,8 @@ public static LaunchCalculator getInstance() { // Filtered slip state (instance, persists across cycles) private ChassisSpeeds filteredSlip = new ChassisSpeeds(0, 0, 0); + private DoublePublisher filteredSlipXPub = NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredX").publish(); + private DoublePublisher filteredSlipYPub = NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredY").publish(); // Trench stuff private static final AprilTagFieldLayout field = AllianceUtils.FIELD_LAYOUT; @@ -297,7 +301,7 @@ public LaunchingParameters calculate( ChassisSpeeds effectiveSpeeds = wheelSpeeds; double poseDt = timestamp - prevTimestamp; - if (robotIsMoving && poseDt > MIN_POSE_DT && poseDt < MAX_POSE_DT) { + if (robotIsMoving && poseDt > MIN_POSE_DT && poseDt < MAX_POSE_DT && false) { Twist2d twist = prevPose.log(estimatedPose); // Slip = (pose-derived velocity) - (wheel velocity). When wheels match pose, @@ -342,6 +346,9 @@ public LaunchingParameters calculate( slipFastMode = false; } + filteredSlipXPub.set(filteredSlip.vxMetersPerSecond); + filteredSlipYPub.set(filteredSlip.vyMetersPerSecond); + // --- PHASE DELAY PREDICTION (second-order) --- // Predict robot state at end of PHASE_DELAY: x = x0 + v*t + 0.5*a*t^2. // launchSpeeds = velocity at end of phase delay = what the ball actually inherits. diff --git a/src/main/java/frc/robot/util/tuning/LauncherConstants.java b/src/main/java/frc/robot/util/tuning/LauncherConstants.java index 5e733d5d3..4e5596988 100644 --- a/src/main/java/frc/robot/util/tuning/LauncherConstants.java +++ b/src/main/java/frc/robot/util/tuning/LauncherConstants.java @@ -66,12 +66,12 @@ public String toString() { new LauncherDistanceDataPoint(2, 3.4, 46, 1.104), new LauncherDistanceDataPoint(2.55, 3.5, 48, 1.204), new LauncherDistanceDataPoint(3.2, 4.5, 51, 1.15), - new LauncherDistanceDataPoint(3.5, 4.8, 54, 1.229), - new LauncherDistanceDataPoint(3.75, 5, 54.5, 1.217), - new LauncherDistanceDataPoint(4.2, 5.125, 57.5, 1.275), - new LauncherDistanceDataPoint(4.5, 5.5, 60, 1.305), - new LauncherDistanceDataPoint(5, 5.8, 64, 1.349), - new LauncherDistanceDataPoint(5.8, 6.6, 67, 1.378), + new LauncherDistanceDataPoint(3.5, 4.8, 54, 1.189), + new LauncherDistanceDataPoint(3.75, 5, 54.5, 1.197), + new LauncherDistanceDataPoint(4.2, 5.125, 57.5, 1.23), // measured tof + new LauncherDistanceDataPoint(4.5, 5.5, 60, 1.265), + new LauncherDistanceDataPoint(5, 5.8, 64, 1.289), + new LauncherDistanceDataPoint(5.8, 6.6, 67, 1.338), new LauncherDistanceDataPoint(8, 9, 90, 1.53) }; From 229c9894667cd5ebb85241315b3cac73b25b49d3 Mon Sep 17 00:00:00 2001 From: SnappyRiffs Date: Fri, 1 May 2026 09:45:48 -0700 Subject: [PATCH 014/114] autoshoot back --- src/main/java/frc/robot/Controls.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index f1be2f2ca..ccb08a2b0 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -34,6 +34,7 @@ import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.subsystems.launcher.TurretSubsystem; import frc.robot.util.AllianceUtils; +import frc.robot.util.GetTargetFromPose; import frc.robot.util.HubShiftUtil; import frc.robot.util.robotType.RobotType; import frc.robot.util.robotType.RobotTypesEnum; @@ -189,7 +190,7 @@ private void configureDrivebaseBindings() { return; } - // readyToShoot = GetTargetFromPose.autoShoot(s.drivebaseSubsystem); + readyToShoot = GetTargetFromPose.autoShoot(s.drivebaseSubsystem); connected(launcherTuningController) .and(launcherTuningController.y()) From 1e30fe7b9f888c78584c1fa06d9b69035154dcce Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 8 May 2026 10:40:10 -0700 Subject: [PATCH 015/114] implemented event triggers + small cleanup --- src/main/deploy/autos/paths/test1.json | 21 +++ src/main/java/frc/robot/Robot.java | 9 +- .../frc/robot/subsystems/auto/BLineLogic.java | 144 +++++++++++------- .../subsystems/auto/PathPlannerLogic.java | 16 +- .../frc/robot/util/simulation/RobotSim.java | 17 +++ 5 files changed, 136 insertions(+), 71 deletions(-) create mode 100644 src/main/deploy/autos/paths/test1.json diff --git a/src/main/deploy/autos/paths/test1.json b/src/main/deploy/autos/paths/test1.json new file mode 100644 index 000000000..c76132639 --- /dev/null +++ b/src/main/deploy/autos/paths/test1.json @@ -0,0 +1,21 @@ +{ + "path_elements": [ + { + "type": "translation", + "x_meters": 1.0, + "y_meters": 5.0, + "intermediate_handoff_radius_meters": 0.2 + }, + { + "type": "event_trigger", + "t_ratio": 0.1, + "lib_key": "launch" + }, + { + "type": "translation", + "x_meters": 3.0, + "y_meters": 5.0, + "intermediate_handoff_radius_meters": 0.2 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6b0692148..20ab4f006 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -120,13 +120,16 @@ protected Robot() { controls = new Controls(subsystems, m_simWrapper); if (DRIVEBASE_ENABLED) { - BLineLogic.init(subsystems); - BLineLogic.configure(subsystems); - BLineLogic.runAuto(); + // BLineLogic.init(subsystems); + // BLineLogic.configure(subsystems); + // BLineLogic.runAuto(); } // PathPlannerLogic.init(subsystems); if (Robot.isSimulation()) { robotSim = new RobotSim(subsystems.drivebaseSubsystem); + BLineLogic.init(subsystems); + BLineLogic.configure(subsystems); + BLineLogic.runAuto(); } else { robotSim = null; } diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index de7deb31b..db1ea4757 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -1,86 +1,120 @@ package frc.robot.subsystems.auto; - import frc.robot.Subsystems; - import frc.robot.lib.BLine.FollowPath; - import frc.robot.lib.BLine.Path; - -import java.util.function.Consumer; import com.ctre.phoenix6.swerve.SwerveRequest; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Controls; +import frc.robot.Robot; +import frc.robot.Subsystems; +import frc.robot.lib.BLine.FollowPath; +import frc.robot.lib.BLine.Path; +import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; +import frc.robot.util.simulation.RobotSim; + public class BLineLogic { -private static Subsystems s; + private static Subsystems s; + + private static FollowPath.Builder pathBuilder; + private static Path myPath; + public static void init(Subsystems subsystems) { -private static FollowPath.Builder pathBuilder; -private static Path myPath; - public static void init(Subsystems subsystems) { - myPath = new Path("LT-Neutral"); s = subsystems; + registerCommands(); + myPath = new Path("test1"); s.drivebaseSubsystem.resetPose(getStartPose()); - } - - -// 2. Create a reusable path builder -public static void configure(Subsystems s) { - - pathBuilder = new FollowPath.Builder( - s.drivebaseSubsystem, - - // pose supplier - () -> s.drivebaseSubsystem.getState().Pose, + } - // robot-relative speeds supplier - () -> s.drivebaseSubsystem.getState().Speeds, + // 2. Create a reusable path builder + public static void configure(Subsystems s) { - // robot-relative speeds consumer - (speeds) -> { - s.drivebaseSubsystem.setControl( - new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) - ); - }, + pathBuilder = + new FollowPath.Builder( + s.drivebaseSubsystem, - // translation PID - new PIDController(3.0, 0.0, 0.0), + // pose supplier + () -> s.drivebaseSubsystem.getState().Pose, - // rotation PID - new PIDController(5.0, 0.0, 0.0), + // robot-relative speeds supplier + () -> s.drivebaseSubsystem.getState().Speeds, - // cross-track PID - new PIDController(2.0, 0.0, 0.0) - ) - .withDefaultShouldFlip().withPoseReset(s.drivebaseSubsystem::resetPose); + // robot-relative speeds consumer + (speeds) -> { + s.drivebaseSubsystem.setControl( + new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))); + }, - }; -public static Command runAuto() { + // translation PID + new PIDController(3.0, 0.0, 0.0), + // rotation PID + new PIDController(5.0, 0.0, 0.0), + // cross-track PID + new PIDController(2.0, 0.0, 0.0)) + .withDefaultShouldFlip() + .withPoseReset(s.drivebaseSubsystem::resetPose); + } + ; - return pathBuilder.build(myPath); - // ... rest of auto + public static Command runAuto() { -} -public static Pose2d getStartPose() { + return pathBuilder.build(myPath); + // ... rest of auto + } + public static Pose2d getStartPose() { -return myPath.getStartPose(Rotation2d.fromDegrees(90)); + return myPath.getStartPose(Rotation2d.fromDegrees(0)); + } -} -public static FollowPath.Builder getBuilder() { + public static FollowPath.Builder getBuilder() { return pathBuilder; + } + + private static void registerCommands() { + if (s.launcherSubsystem != null && s.indexerSubsystem != null) { + if (Robot.isSimulation()) { + FollowPath.registerEventTrigger("launch", RobotSim.launch(s, 1)); + + } else { + FollowPath.registerEventTrigger("launch", launcherCommand()); + } + } + if (s.indexerSubsystem != null) { + FollowPath.registerEventTrigger("intake", intakeCommand()); + } + FollowPath.registerEventTrigger("climb", climbCommand()); + } + + public static Command intakeCommand() { + return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) + .withName("Auto Intake Command"); + } + + public static Command climbCommand() { + return Commands.none().withName("Auto Climb Command"); + } + + public static Command launcherCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + s.flywheels.resetFuelCheck(); + }), + s.launcherSubsystem.launcherAimCommand(), + Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) + .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) + // .until(() -> s.flywheels.isOutOfFuel()) + .withTimeout(4.5) + .andThen(s.launcherSubsystem.rawStowCommand()) + .withName("Auto Launcher Command"); + } } - - - -} - - diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java index 615b6d0a0..2160706f7 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java @@ -22,6 +22,8 @@ import frc.robot.Subsystems; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.util.simulation.FuelSim; +import frc.robot.util.simulation.RobotSim; + import java.io.IOException; import java.util.HashMap; import java.util.List; @@ -274,19 +276,7 @@ public static Command launcherCommand() { } public static Command launcherSimCommand() { - return Commands.sequence( - AutoDriveRotate.autoRotate( - s.drivebaseSubsystem, () -> 0, () -> 0, () -> 0), // SIM PURPOSES ONLY - Commands.run( - () -> - FuelSim.getInstance() - .launchFuel( - MetersPerSecond.of(6), - Radians.of(s.hood.getHoodPosition()), - Radians.of(s.turretSubsystem.getTurretPosition() + Math.PI), - Meters.of(1.45))) - .withTimeout(3)) - .withName("Auto Launcher Sim Command"); + return RobotSim.launch(s,2); } public static Command intakeCommand() { diff --git a/src/main/java/frc/robot/util/simulation/RobotSim.java b/src/main/java/frc/robot/util/simulation/RobotSim.java index a27f98cbb..2796bd099 100644 --- a/src/main/java/frc/robot/util/simulation/RobotSim.java +++ b/src/main/java/frc/robot/util/simulation/RobotSim.java @@ -2,6 +2,10 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Subsystems; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import frc.robot.util.simulation.FuelSim.Hub; @@ -64,4 +68,17 @@ public void updateFuelSim() { scorePublisher.accept(score); fuelHeld.accept(fuelsHeld); } + + public static Command launch(Subsystems s, double timeout) { + return Commands.run( + () -> + FuelSim.getInstance() + .launchFuel( + Units.MetersPerSecond.of(6), + Units.Radians.of(s.hood.getHoodPosition()), + Units.Radians.of(s.turretSubsystem.getTurretPosition() + Math.PI), + Units.Meters.of(1.45))) + .withTimeout(timeout) + .withName("Auto Launcher Sim Command"); + } } From bacfc6689c334ac349310fa5740836c4d0f90de8 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 8 May 2026 10:40:33 -0700 Subject: [PATCH 016/114] other cleanup --- src/main/java/frc/robot/Robot.java | 65 +++++++++++++----------------- 1 file changed, 29 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 20ab4f006..16e22d3b5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,6 @@ import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; -import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -31,9 +30,6 @@ import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; -import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.PathPlannerLogic; -import frc.robot.subsystems.auto.AutonomousField; import frc.robot.subsystems.auto.BLineLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; @@ -120,19 +116,17 @@ protected Robot() { controls = new Controls(subsystems, m_simWrapper); if (DRIVEBASE_ENABLED) { - // BLineLogic.init(subsystems); - // BLineLogic.configure(subsystems); - // BLineLogic.runAuto(); - } - // PathPlannerLogic.init(subsystems); - if (Robot.isSimulation()) { - robotSim = new RobotSim(subsystems.drivebaseSubsystem); - BLineLogic.init(subsystems); - BLineLogic.configure(subsystems); - BLineLogic.runAuto(); - } else { - robotSim = null; + if (Robot.isSimulation()) { + robotSim = new RobotSim(subsystems.drivebaseSubsystem); + + } else { + robotSim = null; + } + BLineLogic.init(subsystems); + BLineLogic.configure(subsystems); + BLineLogic.runAuto(); } + CommandScheduler.getInstance() .onCommandInitialize( command -> DataLogManager.log("Command initialized: " + command.getName())); @@ -150,11 +144,11 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); if (SubsystemConstants.DRIVEBASE_ENABLED) { - // PathPlannerLogic.initCommandsAndPaths(true); - // AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); + // PathPlannerLogic.initCommandsAndPaths(true); + // AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); - // PathPlannerLogic.initSmartDashBoard(); - //CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); + // PathPlannerLogic.initSmartDashBoard(); + // CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); } WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); @@ -289,24 +283,23 @@ public void disabledPeriodic() { public void autonomousInit() { subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); - if (Robot.isSimulation()) { - robotSim.resetFuelSim(); - } + if (Robot.isSimulation()) { + robotSim.resetFuelSim(); + } - CommandScheduler.getInstance().schedule(BLineLogic.runAuto()); - double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); - if (subsystems.visionSubsystem != null) { - if (subsystems.visionSubsystem.limelightaOnline) { - supplyRobotYawToLimelight(Hardware.LIMELIGHT_A, initialYaw); - } - if (subsystems.visionSubsystem.limelightbOnline) { - supplyRobotYawToLimelight(Hardware.LIMELIGHT_B, initialYaw); - } - if (subsystems.visionSubsystem.limelightcOnline) { - supplyRobotYawToLimelight(Hardware.LIMELIGHT_C, initialYaw); - } + CommandScheduler.getInstance().schedule(BLineLogic.runAuto()); + double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); + if (subsystems.visionSubsystem != null) { + if (subsystems.visionSubsystem.limelightaOnline) { + supplyRobotYawToLimelight(Hardware.LIMELIGHT_A, initialYaw); } - + if (subsystems.visionSubsystem.limelightbOnline) { + supplyRobotYawToLimelight(Hardware.LIMELIGHT_B, initialYaw); + } + if (subsystems.visionSubsystem.limelightcOnline) { + supplyRobotYawToLimelight(Hardware.LIMELIGHT_C, initialYaw); + } + } } /** This function is called periodically during autonomous. */ From 2ce16831db8239e9221c6aff5a81608f8ab0eda1 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 12 May 2026 18:12:58 -0700 Subject: [PATCH 017/114] made event triggers start and end based on percentage position bewteen two points --- src/main/deploy/autos/paths/test1.json | 23 +++++++-- .../frc/robot/subsystems/auto/BLineLogic.java | 51 +++++++++++++------ 2 files changed, 55 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/autos/paths/test1.json b/src/main/deploy/autos/paths/test1.json index c76132639..ec2a8dad9 100644 --- a/src/main/deploy/autos/paths/test1.json +++ b/src/main/deploy/autos/paths/test1.json @@ -8,13 +8,30 @@ }, { "type": "event_trigger", - "t_ratio": 0.1, + "t_ratio": 0.19999999999999984, "lib_key": "launch" }, { "type": "translation", - "x_meters": 3.0, - "y_meters": 5.0, + "x_meters": 2.471270781935646, + "y_meters": 5.036464084004439, + "intermediate_handoff_radius_meters": 1.0 + }, + { + "type": "translation", + "x_meters": 2.342541563871298, + "y_meters": 3.596132765829129, + "intermediate_handoff_radius_meters": 1.0 + }, + { + "type": "event_trigger", + "t_ratio": 0.18, + "lib_key": "cancel" + }, + { + "type": "translation", + "x_meters": 2.614917269855766, + "y_meters": 1.4994479355739276, "intermediate_handoff_radius_meters": 0.2 } ] diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index db1ea4757..21dd614d1 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -1,11 +1,14 @@ package frc.robot.subsystems.auto; +import java.util.concurrent.BlockingDeque; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Controls; import frc.robot.Robot; @@ -20,6 +23,7 @@ public class BLineLogic { private static FollowPath.Builder pathBuilder; private static Path myPath; + private static Command bLineLaunching; public static void init(Subsystems subsystems) { @@ -78,22 +82,9 @@ public static FollowPath.Builder getBuilder() { return pathBuilder; } + public static void main(String[] args) { - private static void registerCommands() { - if (s.launcherSubsystem != null && s.indexerSubsystem != null) { - if (Robot.isSimulation()) { - FollowPath.registerEventTrigger("launch", RobotSim.launch(s, 1)); - - } else { - FollowPath.registerEventTrigger("launch", launcherCommand()); - } - } - if (s.indexerSubsystem != null) { - FollowPath.registerEventTrigger("intake", intakeCommand()); - } - FollowPath.registerEventTrigger("climb", climbCommand()); } - public static Command intakeCommand() { return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) .withName("Auto Intake Command"); @@ -104,6 +95,7 @@ public static Command climbCommand() { } public static Command launcherCommand() { + return Commands.parallel( Commands.runOnce( () -> { @@ -112,9 +104,36 @@ public static Command launcherCommand() { s.launcherSubsystem.launcherAimCommand(), Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) - // .until(() -> s.flywheels.isOutOfFuel()) - .withTimeout(4.5) + //Timeout may not be needed if it only is supposed to run when the launcher can reach the target at certain points during the path? + // .withTimeout(4.5) .andThen(s.launcherSubsystem.rawStowCommand()) .withName("Auto Launcher Command"); } + public static void cancelCommand() { + + CommandScheduler.getInstance().cancel(bLineLaunching); + } + + private static void registerCommands() { + + if (s.launcherSubsystem != null && s.indexerSubsystem != null) { + if (Robot.isSimulation()) { + bLineLaunching = RobotSim.launch(s, 1); + FollowPath.registerEventTrigger("launch", bLineLaunching); + + } else { + bLineLaunching = launcherCommand(); + FollowPath.registerEventTrigger("launch", bLineLaunching); + + } + } + if (s.indexerSubsystem != null) { + + FollowPath.registerEventTrigger("intake", intakeCommand()); + } + FollowPath.registerEventTrigger("climb", climbCommand()); + FollowPath.registerEventTrigger("cancel", Commands.runOnce(() -> cancelCommand()).andThen(Commands.print("IS IT OVER"))); + } + + } From 3645d11c40fc94d8b16a6e44fa857dd6380c1b8c Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 12 May 2026 19:50:28 -0700 Subject: [PATCH 018/114] more trigger testing --- src/main/deploy/autos/config.json | 18 ++-- src/main/deploy/autos/paths/test1.json | 100 ++++++++++++++---- src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/subsystems/auto/BLineLogic.java | 38 +++---- .../subsystems/auto/PathPlannerLogic.java | 32 +++--- src/test/java/AutosTest.java | 5 +- 6 files changed, 126 insertions(+), 68 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index a7398b2cb..4831e7df1 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -1,25 +1,25 @@ { "gui": { "robot": { - "length_meters": 0.762, - "width_meters": 0.61 + "length_meters": 0.5, + "width_meters": 0.5 }, "protrusions": { - "enabled": true, - "distance_meters": 0.2, - "side": "back", - "default_state": "shown", + "enabled": false, + "distance_meters": 0.0, + "side": "none", + "default_state": "", "show_on_event_keys": [], "hide_on_event_keys": [] } }, "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 4.5, - "default_max_acceleration_meters_per_sec2": 4.0, + "default_max_velocity_meters_per_sec": 5.0, + "default_max_acceleration_meters_per_sec2": 4.5, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 5.0 + "default_end_rotation_tolerance_deg": 2.0 } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/test1.json b/src/main/deploy/autos/paths/test1.json index ec2a8dad9..a9f50094c 100644 --- a/src/main/deploy/autos/paths/test1.json +++ b/src/main/deploy/autos/paths/test1.json @@ -1,38 +1,102 @@ { "path_elements": [ { - "type": "translation", - "x_meters": 1.0, - "y_meters": 5.0, - "intermediate_handoff_radius_meters": 0.2 + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 7.597, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5707963267948966, + "profiled_rotation": true + } }, { "type": "event_trigger", - "t_ratio": 0.19999999999999984, - "lib_key": "launch" + "t_ratio": 0.7499999999999997, + "lib_key": "intake" + }, + { + "type": "rotation", + "rotation_radians": 2.2689280275926285, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 5.792151516199503, + "y_meters": 7.434737073115289, + "intermediate_handoff_radius_meters": 0.85 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.82554798949896, + "y_meters": 6.565075094034977, + "intermediate_handoff_radius_meters": 0.8 + }, + "rotation_target": { + "rotation_radians": 2.2689280275926285, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.837569595453786, + "y_meters": 5.69813166039904, + "intermediate_handoff_radius_meters": 0.6 + }, + "rotation_target": { + "rotation_radians": 2.2689280275926285, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": 1.5707963267948966, + "t_ratio": 0.7000000000000001, + "profiled_rotation": true }, { "type": "translation", - "x_meters": 2.471270781935646, - "y_meters": 5.036464084004439, - "intermediate_handoff_radius_meters": 1.0 + "x_meters": 7.773425631652618, + "y_meters": 4.481957944131684, + "intermediate_handoff_radius_meters": 0.35 }, { "type": "translation", - "x_meters": 2.342541563871298, - "y_meters": 3.596132765829129, - "intermediate_handoff_radius_meters": 1.0 + "x_meters": 6.609706979663266, + "y_meters": 7.133880635410148, + "intermediate_handoff_radius_meters": 0.4 }, { "type": "event_trigger", - "t_ratio": 0.18, - "lib_key": "cancel" + "t_ratio": 0.9500000000000002, + "lib_key": "launch" }, { "type": "translation", - "x_meters": 2.614917269855766, - "y_meters": 1.4994479355739276, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 3.778718855753052, + "y_meters": 7.5178321629397615, + "intermediate_handoff_radius_meters": 1.2500000000000007 } - ] + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 2.0, + "start_ordinal": 3, + "end_ordinal": 6 + } + ], + "max_acceleration_meters_per_sec2": [ + { + "value": 1.75, + "start_ordinal": 4, + "end_ordinal": 4 + } + ] + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 16e22d3b5..d0bcdf158 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -124,7 +124,6 @@ protected Robot() { } BLineLogic.init(subsystems); BLineLogic.configure(subsystems); - BLineLogic.runAuto(); } CommandScheduler.getInstance() diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index 21dd614d1..64424ca5f 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.auto; -import java.util.concurrent.BlockingDeque; - import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -30,7 +28,6 @@ public static void init(Subsystems subsystems) { s = subsystems; registerCommands(); myPath = new Path("test1"); - s.drivebaseSubsystem.resetPose(getStartPose()); } // 2. Create a reusable path builder @@ -62,13 +59,13 @@ public static void configure(Subsystems s) { // cross-track PID new PIDController(2.0, 0.0, 0.0)) .withDefaultShouldFlip() - .withPoseReset(s.drivebaseSubsystem::resetPose); + .withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); } ; public static Command runAuto() { - return pathBuilder.build(myPath); + // ... rest of auto } @@ -82,9 +79,9 @@ public static FollowPath.Builder getBuilder() { return pathBuilder; } - public static void main(String[] args) { - } + public static void main(String[] args) {} + public static Command intakeCommand() { return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) .withName("Auto Intake Command"); @@ -104,27 +101,33 @@ public static Command launcherCommand() { s.launcherSubsystem.launcherAimCommand(), Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) - //Timeout may not be needed if it only is supposed to run when the launcher can reach the target at certain points during the path? - // .withTimeout(4.5) + // Timeout may not be needed if it only is supposed to run when the launcher can reach the + // target at certain points during the path? + // .withTimeout(4.5) .andThen(s.launcherSubsystem.rawStowCommand()) .withName("Auto Launcher Command"); } + public static void cancelCommand() { - CommandScheduler.getInstance().cancel(bLineLaunching); + CommandScheduler.getInstance().cancel(bLineLaunching); } private static void registerCommands() { if (s.launcherSubsystem != null && s.indexerSubsystem != null) { if (Robot.isSimulation()) { - bLineLaunching = RobotSim.launch(s, 1); - FollowPath.registerEventTrigger("launch", bLineLaunching); + bLineLaunching = RobotSim.launch(s, 1); - } else { - bLineLaunching = launcherCommand(); - FollowPath.registerEventTrigger("launch", bLineLaunching); + FollowPath.registerEventTrigger( + "launch", bLineLaunching.andThen(Commands.print("LAUNCH FINISHED"))); + FollowPath.registerEventTrigger("climb", Commands.print("CLIMB FINISHED")); + FollowPath.registerEventTrigger("cancel", Commands.print("IS IT OVER")); + FollowPath.registerEventTrigger("intake", Commands.print("INTAKE FINISHED")); + } else { + bLineLaunching = launcherCommand(); + FollowPath.registerEventTrigger("launch", bLineLaunching); } } if (s.indexerSubsystem != null) { @@ -132,8 +135,7 @@ private static void registerCommands() { FollowPath.registerEventTrigger("intake", intakeCommand()); } FollowPath.registerEventTrigger("climb", climbCommand()); - FollowPath.registerEventTrigger("cancel", Commands.runOnce(() -> cancelCommand()).andThen(Commands.print("IS IT OVER"))); + FollowPath.registerEventTrigger( + "cancel", Commands.runOnce(() -> cancelCommand()).andThen(Commands.print("IS IT OVER"))); } - - } diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java index 2160706f7..5313f3691 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java @@ -1,9 +1,5 @@ package frc.robot.subsystems.auto; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Radians; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; @@ -21,9 +17,7 @@ import frc.robot.Robot; import frc.robot.Subsystems; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; -import frc.robot.util.simulation.FuelSim; import frc.robot.util.simulation.RobotSim; - import java.io.IOException; import java.util.HashMap; import java.util.List; @@ -114,18 +108,18 @@ private static void initPaths() { physicalRebuiltPaths = List.of( - new AutoPath("C-Outpost-Depot", "C-Outpost-Depot"), - new AutoPath("LeftTrench-Depot", "LeftTrench-Depot"), - new AutoPath("LT-Neutral-Depot", "LT-Neutral-Depot"), - new AutoPath("LT-Neutral", "LT-Neutral"), - new AutoPath("LT-DoubleNeutral", "LT-DoubleNeutral"), - new AutoPath("RightTrench-Outpost", "RightTrench-Outpost"), - new AutoPath("RT-Neutral-Outpost", "RT-Neutral-Outpost"), - new AutoPath("Rotate-RT-Neutral", "Rotate-RT-Neutral"), - new AutoPath("RT-Neutral", "RT-Neutral"), - new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), - new AutoPath("RT-BLOCK", "RT-BLOCK"), - new AutoPath("LT-BLOCK", "LT-BLOCK")); + new AutoPath("C-Outpost-Depot", "C-Outpost-Depot"), + new AutoPath("LeftTrench-Depot", "LeftTrench-Depot"), + new AutoPath("LT-Neutral-Depot", "LT-Neutral-Depot"), + new AutoPath("LT-Neutral", "LT-Neutral"), + new AutoPath("LT-DoubleNeutral", "LT-DoubleNeutral"), + new AutoPath("RightTrench-Outpost", "RightTrench-Outpost"), + new AutoPath("RT-Neutral-Outpost", "RT-Neutral-Outpost"), + new AutoPath("Rotate-RT-Neutral", "Rotate-RT-Neutral"), + new AutoPath("RT-Neutral", "RT-Neutral"), + new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), + new AutoPath("RT-BLOCK", "RT-BLOCK"), + new AutoPath("LT-BLOCK", "LT-BLOCK")); rebuiltPaths = physicalRebuiltPaths; @@ -276,7 +270,7 @@ public static Command launcherCommand() { } public static Command launcherSimCommand() { - return RobotSim.launch(s,2); + return RobotSim.launch(s, 2); } public static Command intakeCommand() { diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index edeecb05e..4c1a53ce6 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -3,19 +3,18 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc.robot.generated.CompTunerConstants; import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.PathPlannerLogic; import frc.robot.subsystems.auto.AutoPath; +import frc.robot.subsystems.auto.PathPlannerLogic; import java.io.IOException; import java.util.Dictionary; import java.util.Enumeration; import java.util.Hashtable; import org.json.simple.parser.ParseException; -import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; class AutosTest { - // @BeforeEach + // @BeforeEach void setup() { AutoBuilderConfig.buildAuto(CompTunerConstants.createDrivetrain(), true); From 74821c87a58a5fb8459ab7d86dd47edf10890de3 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 21 May 2026 11:04:40 -0700 Subject: [PATCH 019/114] semi working bline chooser --- src/main/deploy/autos/paths/sample1.json | 45 ++ src/main/deploy/autos/paths/sample2.json | 33 ++ src/main/deploy/autos/paths/sample3.json | 34 ++ src/main/deploy/autos/paths/sample4.json | 22 + src/main/deploy/autos/paths/test1.json | 10 + src/main/java/frc/robot/Robot.java | 6 +- .../frc/robot/subsystems/auto/BLineLogic.java | 392 +++++++++++++++--- .../frc/robot/subsystems/auto/BLinePath.java | 46 ++ 8 files changed, 527 insertions(+), 61 deletions(-) create mode 100644 src/main/deploy/autos/paths/sample1.json create mode 100644 src/main/deploy/autos/paths/sample2.json create mode 100644 src/main/deploy/autos/paths/sample3.json create mode 100644 src/main/deploy/autos/paths/sample4.json create mode 100644 src/main/java/frc/robot/subsystems/auto/BLinePath.java diff --git a/src/main/deploy/autos/paths/sample1.json b/src/main/deploy/autos/paths/sample1.json new file mode 100644 index 000000000..53ecfb229 --- /dev/null +++ b/src/main/deploy/autos/paths/sample1.json @@ -0,0 +1,45 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": -1.5707963267948966, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "event_trigger", + "t_ratio": 0.7, + "lib_key": "intake" + }, + { + "type": "translation", + "x_meters": 5.594848920863306, + "y_meters": 0.5519712230215834, + "intermediate_handoff_radius_meters": 0.2 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.6, + "y_meters": 1.4, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample2.json b/src/main/deploy/autos/paths/sample2.json new file mode 100644 index 000000000..45639dd98 --- /dev/null +++ b/src/main/deploy/autos/paths/sample2.json @@ -0,0 +1,33 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.6, + "y_meters": 1.4, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "launch" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.8, + "y_meters": 3.5, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -2.0943951023931953, + "profiled_rotation": true + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample3.json b/src/main/deploy/autos/paths/sample3.json new file mode 100644 index 000000000..f6bd50c65 --- /dev/null +++ b/src/main/deploy/autos/paths/sample3.json @@ -0,0 +1,34 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.8, + "y_meters": 3.5, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -2.0943951023931953, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": -1.5707963267948966, + "t_ratio": 0.5999999999999998, + "profiled_rotation": true + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.0, + "y_meters": 0.6, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample4.json b/src/main/deploy/autos/paths/sample4.json new file mode 100644 index 000000000..7d8c56624 --- /dev/null +++ b/src/main/deploy/autos/paths/sample4.json @@ -0,0 +1,22 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.0, + "y_meters": 0.6, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 3.3, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.2 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/test1.json b/src/main/deploy/autos/paths/test1.json index a9f50094c..fbe912553 100644 --- a/src/main/deploy/autos/paths/test1.json +++ b/src/main/deploy/autos/paths/test1.json @@ -88,6 +88,16 @@ { "value": 2.0, "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 2.0, + "start_ordinal": 5, "end_ordinal": 6 } ], diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d0bcdf158..b50144b57 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -124,6 +124,7 @@ protected Robot() { } BLineLogic.init(subsystems); BLineLogic.configure(subsystems); + BLineLogic.initSmartDashboard(); } CommandScheduler.getInstance() @@ -144,9 +145,8 @@ protected Robot() { if (SubsystemConstants.DRIVEBASE_ENABLED) { // PathPlannerLogic.initCommandsAndPaths(true); - // AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); + - // PathPlannerLogic.initSmartDashBoard(); // CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); } WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); @@ -286,7 +286,7 @@ public void autonomousInit() { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(BLineLogic.runAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.getSelectedAuto()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index 64424ca5f..0b56a416b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -1,141 +1,417 @@ +// ========================= BLineLogic.java ========================= + package frc.robot.subsystems.auto; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.FileVersionException; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; + import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; + import frc.robot.lib.BLine.FollowPath; -import frc.robot.lib.BLine.Path; + import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; + import frc.robot.util.simulation.RobotSim; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +import org.json.simple.parser.ParseException; + public class BLineLogic { + private static Subsystems s; - private static FollowPath.Builder pathBuilder; - private static Path myPath; + // ========================= START POSITIONS ========================= + + public enum StartPosition { + + LEFT_TRENCH( + "Left Trench", + new Pose2d( + 4.013, + 7.597, + Rotation2d.fromDegrees(90))), + + CENTER( + "Center", + new Pose2d( + 3.600, + 4.035, + Rotation2d.fromDegrees(0))), + + RIGHT_TRENCH( + "Right Trench", + new Pose2d( + 4.013, + 0.473, + Rotation2d.fromDegrees(-90))), + + MISC("Misc", null); + + public final String title; + public final Pose2d startPose; + + StartPosition(String title, Pose2d startPose) { + this.title = title; + this.startPose = startPose; + } + } + + // ========================= AUTOS ========================= + + private static final List autos = new ArrayList<>(); + + // ========================= CHOOSERS ========================= + + private static final SendableChooser startPositionChooser = + new SendableChooser<>(); + + private static final SendableChooser autoChooser = + new SendableChooser<>(); + + private static final SendableChooser gameObjects = + new SendableChooser<>(); + + private static final NetworkTableEntry autoDelayEntry = + NetworkTableInstance.getDefault() + .getTable("Autos") + .getEntry("Auto Delay"); + + public static final String keys = + "RB=Right Bump, LB=Left Bump, LT=Left Trench, RT=Right Trench"; + + // ========================= PATH BUILDER ========================= + + public static FollowPath.Builder pathBuilder; + private static Command bLineLaunching; + // ========================= INIT ========================= + public static void init(Subsystems subsystems) { s = subsystems; + registerCommands(); - myPath = new Path("test1"); + + autos.clear(); + + // ========================= DEFINE AUTOS HERE ========================= + + autos.add(new BLinePath( + "Sample 1", + "sample1")); + + autos.add(new BLinePath( + "Sample 2", + "sample2")); + + autos.add(new BLinePath( + "Sample 3", + "sample3")); } - // 2. Create a reusable path builder + // ========================= CONFIGURE ========================= + public static void configure(Subsystems s) { pathBuilder = new FollowPath.Builder( s.drivebaseSubsystem, - // pose supplier () -> s.drivebaseSubsystem.getState().Pose, - // robot-relative speeds supplier () -> s.drivebaseSubsystem.getState().Speeds, - // robot-relative speeds consumer - (speeds) -> { - s.drivebaseSubsystem.setControl( - new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))); - }, + (speeds) -> + s.drivebaseSubsystem.setControl( + new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds( + ChassisSpeeds.discretize( + speeds, + 0.020))), - // translation PID new PIDController(3.0, 0.0, 0.0), - // rotation PID new PIDController(5.0, 0.0, 0.0), - // cross-track PID new PIDController(2.0, 0.0, 0.0)) + .withDefaultShouldFlip() - .withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); + + .withPoseReset( + pose -> s.drivebaseSubsystem.resetPose(pose)); + } + + // ========================= SMARTDASHBOARD ========================= + + public static void initSmartDashboard() { + + // starting positions + + startPositionChooser.setDefaultOption( + StartPosition.MISC.title, + StartPosition.MISC); + + for (StartPosition pos : StartPosition.values()) { + + startPositionChooser.addOption( + pos.title, + pos); + } + + // game objects chooser + + gameObjects.setDefaultOption("0", 0); + + // auto chooser + + refreshAutoChooser(); + + // NT / Elastic publishing + + SmartDashboard.putData( + "Starting Position", + startPositionChooser); + + SmartDashboard.putData( + "Auto Mode", + gameObjects); + + SmartDashboard.putData( + "Available Auto Variants", + autoChooser); + + SmartDashboard.putString( + "Auto Key", + keys); + + autoDelayEntry.setDouble(0.0); + + // update chooser when selection changes + + startPositionChooser.onChange( + v -> refreshAutoChooser()); + + // debug + + System.out.println("========== REGISTERED AUTOS =========="); + + for (BLinePath auto : autos) { + + System.out.println( + auto.getDisplayName() + + " | " + + auto.getStartPose2d()); + } + + System.out.println("======================================"); } - ; - public static Command runAuto() { - return pathBuilder.build(myPath); + // ========================= AUTO FILTERING ========================= + + private static void refreshAutoChooser() { + + StartPosition selectedStart = + startPositionChooser.getSelected(); + + if (selectedStart == null) { + selectedStart = StartPosition.MISC; + } + + boolean first = true; + + for (BLinePath auto : autos) { - // ... rest of auto + boolean matches = + selectedStart == StartPosition.MISC + || + auto.getStartPose2d() == selectedStart.startPose; + if (!matches) { + continue; + } + + if (first) { + + autoChooser.setDefaultOption( + auto.getDisplayName(), + auto.getDisplayName()); + + first = false; + + } else { + + autoChooser.addOption( + auto.getDisplayName(), + auto.getDisplayName()); + } + } + + // safety fallback + + if (first && !autos.isEmpty()) { + + autoChooser.setDefaultOption( + autos.get(0).getDisplayName(), + autos.get(0).getDisplayName()); + } } - public static Pose2d getStartPose() { + // ========================= GETTERS ========================= + + public static String getSelectedAutoName() { - return myPath.getStartPose(Rotation2d.fromDegrees(0)); + return autoChooser.getSelected(); } - public static FollowPath.Builder getBuilder() { + public static BLinePath getSelectedAutoPath() { + + String selectedName = + autoChooser.getSelected(); + + if (selectedName == null) { + return null; + } + + for (BLinePath auto : autos) { + + if (auto.getDisplayName().equals(selectedName)) { + return auto; + } + } - return pathBuilder; + return null; } - public static void main(String[] args) {} + public static Pose2d getSelectedAutoStartingPose() { + + BLinePath selected = + getSelectedAutoPath(); + + if (selected == null) { + return Pose2d.kZero; + } + + return selected.getStartPose2d(); + } + + public static Command getSelectedAuto() { + + double delay = + autoDelayEntry.getDouble(0.0); + + BLinePath selected = + getSelectedAutoPath(); + + if (selected == null) { + + System.out.println( + "NO AUTO SELECTED"); + + return Commands.none(); + } + + System.out.println( + "RUNNING AUTO: " + + selected.getDisplayName()); + + return Commands.waitSeconds(delay) + + .andThen( + AutoBuilder.buildAuto( + selected.getAutoName())) + + .withName( + selected.getDisplayName()); + } + + // ========================= PATHPLANNER DIRECT ========================= + + + + // ========================= COMMANDS ========================= public static Command intakeCommand() { - return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) + + return Commands.runOnce( + () -> + Controls.intakeMode = + IntakeMode.INTAKE) + .withName("Auto Intake Command"); } public static Command climbCommand() { - return Commands.none().withName("Auto Climb Command"); - } - public static Command launcherCommand() { - - return Commands.parallel( - Commands.runOnce( - () -> { - s.flywheels.resetFuelCheck(); - }), - s.launcherSubsystem.launcherAimCommand(), - Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) - .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) - // Timeout may not be needed if it only is supposed to run when the launcher can reach the - // target at certain points during the path? - // .withTimeout(4.5) - .andThen(s.launcherSubsystem.rawStowCommand()) - .withName("Auto Launcher Command"); + return Commands.none() + .withName("Auto Climb Command"); } public static void cancelCommand() { - CommandScheduler.getInstance().cancel(bLineLaunching); + CommandScheduler.getInstance() + .cancel(bLineLaunching); } + // ========================= EVENT TRIGGERS ========================= + private static void registerCommands() { - if (s.launcherSubsystem != null && s.indexerSubsystem != null) { + if (s.launcherSubsystem != null + && s.indexerSubsystem != null) { + if (Robot.isSimulation()) { - bLineLaunching = RobotSim.launch(s, 1); + + bLineLaunching = + RobotSim.launch(s,1); FollowPath.registerEventTrigger( - "launch", bLineLaunching.andThen(Commands.print("LAUNCH FINISHED"))); - FollowPath.registerEventTrigger("climb", Commands.print("CLIMB FINISHED")); - FollowPath.registerEventTrigger("cancel", Commands.print("IS IT OVER")); - FollowPath.registerEventTrigger("intake", Commands.print("INTAKE FINISHED")); + "launch", + + bLineLaunching.andThen( + Commands.print( + "LAUNCH FINISHED"))); } else { - bLineLaunching = launcherCommand(); - FollowPath.registerEventTrigger("launch", bLineLaunching); + + bLineLaunching = + Commands.none(); + + FollowPath.registerEventTrigger( + "launch", + bLineLaunching); } } - if (s.indexerSubsystem != null) { - FollowPath.registerEventTrigger("intake", intakeCommand()); - } - FollowPath.registerEventTrigger("climb", climbCommand()); FollowPath.registerEventTrigger( - "cancel", Commands.runOnce(() -> cancelCommand()).andThen(Commands.print("IS IT OVER"))); + "intake", + intakeCommand()); + + FollowPath.registerEventTrigger( + "climb", + climbCommand()); } } + diff --git a/src/main/java/frc/robot/subsystems/auto/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLinePath.java new file mode 100644 index 000000000..1a247daee --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLinePath.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.lib.BLine.Path; + +public class BLinePath { + + private final String displayName; + private final String autoName; + private final Pose2d startPose; + private final boolean vision; + private final Path path; + + public BLinePath(String displayName, String autoName) { + this(displayName, autoName, false); + } + + public BLinePath(String displayName, String autoName, boolean vision) { + this.displayName = displayName; + this.autoName = autoName; + this.vision = vision; + + this.path = new Path(autoName); + this.startPose = path.getStartPose(); + + if (startPose == null) { + throw new IllegalStateException("Path missing start pose: " + autoName); + } + } + + public String getDisplayName() { + return displayName; + } + + public String getAutoName() { + return autoName; + } + + public Pose2d getStartPose2d() { + return startPose; + } + + public boolean isVision() { + return vision; + } +} From 2ba4813eb34e5a55108eb0c7dad096e41fc31014 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 21 May 2026 11:26:42 -0700 Subject: [PATCH 020/114] fixed bug --- src/main/java/frc/robot/subsystems/auto/BLineLogic.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index 0b56a416b..edd593892 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -27,7 +27,7 @@ import frc.robot.Subsystems; import frc.robot.lib.BLine.FollowPath; - +import frc.robot.lib.BLine.Path; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.util.simulation.RobotSim; @@ -339,12 +339,9 @@ public static Command getSelectedAuto() { return Commands.waitSeconds(delay) - .andThen( - AutoBuilder.buildAuto( - selected.getAutoName())) + .andThen(pathBuilder.build(new Path(selected.getAutoName()))); - .withName( - selected.getDisplayName()); + } // ========================= PATHPLANNER DIRECT ========================= From be7c58e20e56d731d793f54c993c79676b5e7b51 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 21 May 2026 15:37:02 -0700 Subject: [PATCH 021/114] new slip attempt --- src/main/java/frc/robot/Controls.java | 7 +- .../subsystems/launcher/LaunchCalculator.java | 129 +++++++++++++++++- 2 files changed, 125 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index ccb08a2b0..311555d22 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -34,7 +34,6 @@ import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.subsystems.launcher.TurretSubsystem; import frc.robot.util.AllianceUtils; -import frc.robot.util.GetTargetFromPose; import frc.robot.util.HubShiftUtil; import frc.robot.util.robotType.RobotType; import frc.robot.util.robotType.RobotTypesEnum; @@ -190,7 +189,7 @@ private void configureDrivebaseBindings() { return; } - readyToShoot = GetTargetFromPose.autoShoot(s.drivebaseSubsystem); + // readyToShoot = GetTargetFromPose.autoShoot(s.drivebaseSubsystem); connected(launcherTuningController) .and(launcherTuningController.y()) @@ -477,8 +476,8 @@ private void configureTurretBindings() { s.turretSubsystem.setDefaultCommand( s.turretSubsystem.rotateToTargetWithCalc().withName("Turret Default Command")); - (turretAtZero - .and(new Trigger(()-> s.turretSubsystem.getTurretPosition() < 0.5))).or(driverController.povLeft()) + (turretAtZero.and(new Trigger(() -> s.turretSubsystem.getTurretPosition() < 0.5))) + .or(driverController.povLeft()) .onTrue( Commands.runOnce(() -> s.turretSubsystem.zeroTurretPosistion()) .withName("Zero Turret on Limit Switch")); diff --git a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java index b61ce3a0b..7e326f123 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.launcher; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import com.ctre.phoenix6.swerve.SwerveModule; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -10,6 +11,9 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.BooleanArrayPublisher; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTableInstance; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; @@ -107,8 +111,49 @@ public static LaunchCalculator getInstance() { // Filtered slip state (instance, persists across cycles) private ChassisSpeeds filteredSlip = new ChassisSpeeds(0, 0, 0); - private DoublePublisher filteredSlipXPub = NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredX").publish(); - private DoublePublisher filteredSlipYPub = NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredY").publish(); + + // ---- SLIP DETECTION (stator current + velocity spike) ---- + // A module is considered slipping when BOTH conditions hold simultaneously: + // 1. Stator current drops below SLIP_CURRENT_THRESHOLD — wheel is unloaded. + // 2. Velocity derivative exceeds SLIP_VELOCITY_DELTA_THRESHOLD — wheel spun up suddenly. + // Requiring both avoids false positives from normal low-load coasting (current alone) + // or from smooth acceleration (velocity delta alone). + // + // TUNING GUIDE (log /LaunchCalculator/slip/* during a match): + // - SLIP_CURRENT_THRESHOLD: watch perModuleCurrent during normal driving. Set this + // just below the minimum you see while pushing hard. Start around 10–15 A. + // - SLIP_VELOCITY_DELTA_THRESHOLD: watch perModuleVelocityDelta. A real slip event + // at 50 Hz on carpet typically spikes > 3–5 rot/s per 20 ms. Start around 3.0. + // - SLIP_MODULE_COUNT_THRESHOLD: 1 = very sensitive (any single wheel), 2 = more + // conservative. Start at 1 and raise if you see false positives during defense. + private static final double SLIP_CURRENT_THRESHOLD = 15.0; // Amps — tune this + private static final double SLIP_VELOCITY_DELTA_THRESHOLD = 3.0; // rot/s per 20ms — tune this + private static final int SLIP_MODULE_COUNT_THRESHOLD = 1; // modules slipping to confirm + + // Per-module velocity history for delta computation (rot/s, one entry per module) + private double[] lastModuleVelocity = null; // lazy-initialized on first call + + // NT publishers + private final DoublePublisher filteredSlipXPub = + NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredX").publish(); + private final DoublePublisher filteredSlipYPub = + NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredY").publish(); + private final BooleanPublisher isSlippingPub = + NetworkTableInstance.getDefault() + .getBooleanTopic("/LaunchCalculator/slip/isSlipping") + .publish(); + private final DoubleArrayPublisher perModuleCurrentPub = + NetworkTableInstance.getDefault() + .getDoubleArrayTopic("/LaunchCalculator/slip/perModuleCurrent") + .publish(); + private final DoubleArrayPublisher perModuleVelocityDeltaPub = + NetworkTableInstance.getDefault() + .getDoubleArrayTopic("/LaunchCalculator/slip/perModuleVelocityDelta") + .publish(); + private final BooleanArrayPublisher perModuleSlippingPub = + NetworkTableInstance.getDefault() + .getBooleanArrayTopic("/LaunchCalculator/slip/perModuleSlipping") + .publish(); // Trench stuff private static final AprilTagFieldLayout field = AllianceUtils.FIELD_LAYOUT; @@ -151,6 +196,62 @@ public record LaunchingParameters( } } + // ------ SLIP DETECTION ------ // + + /** + * Checks each swerve module for wheel slip using stator current drop + velocity spike. Both + * conditions must be true simultaneously on the same module to count as a slip event. Returns + * true if the number of slipping modules meets SLIP_MODULE_COUNT_THRESHOLD. + * + *

Also publishes per-module diagnostics and the aggregate flag to NetworkTables every cycle so + * thresholds can be tuned from logs without redeploying. + * + * @param modules swerve modules from drivetrain.getModules() + * @return true if slip is confidently detected + */ + private boolean detectSlip(SwerveModule[] modules) { + int moduleCount = modules.length; + + // Lazy-initialize per-module velocity history + if (lastModuleVelocity == null) { + lastModuleVelocity = new double[moduleCount]; + } + + double[] currents = new double[moduleCount]; + double[] velocityDeltas = new double[moduleCount]; + boolean[] moduleSlipping = new boolean[moduleCount]; + int slippingCount = 0; + + for (int i = 0; i < moduleCount; i++) { + var driveMotor = modules[i].getDriveMotor(); + + double statorCurrent = driveMotor.getStatorCurrent().getValueAsDouble(); + double velocity = driveMotor.getVelocity().getValueAsDouble(); // rot/s + double velocityDelta = Math.abs(velocity - lastModuleVelocity[i]); + + currents[i] = statorCurrent; + velocityDeltas[i] = velocityDelta; + + // Both conditions required: unloaded wheel AND sudden velocity spike + boolean currentDrop = statorCurrent < SLIP_CURRENT_THRESHOLD; + boolean velocitySpike = velocityDelta > SLIP_VELOCITY_DELTA_THRESHOLD; + moduleSlipping[i] = currentDrop && velocitySpike; + + if (moduleSlipping[i]) slippingCount++; + lastModuleVelocity[i] = velocity; + } + + boolean isSlipping = slippingCount >= SLIP_MODULE_COUNT_THRESHOLD; + + // Publish diagnostics every cycle for tuning + isSlippingPub.set(isSlipping); + perModuleCurrentPub.set(currents); + perModuleVelocityDeltaPub.set(velocityDeltas); + perModuleSlippingPub.set(moduleSlipping); + + return isSlipping; + } + // ------ MAIN LOGIC ------ // /** @@ -176,6 +277,10 @@ public LaunchingParameters getParameters( double currentTurretOmega = turretSubsystem.getOmega(); double timestamp = driveState.Timestamp; + // Run slip detection every cycle regardless of cache — keeps lastModuleVelocity + // fresh and NT logs continuous even when the cache is hit. + boolean isSlipping = detectSlip(drivetrain.getModules()); + if (cachedParams != null && timestamp == prevTimestamp) { return cachedParams; } @@ -251,7 +356,7 @@ public LaunchingParameters getParameters( lastPose = currentPose; lastTurretOmega = currentTurretOmega; - cachedParams = calculate(driveState, turretSubsystem); + cachedParams = calculate(driveState, turretSubsystem, isSlipping); // Update differentiation state AFTER calculate() so that calculate() sees the previous // cycle's state (giving a real non-zero poseDt / speedsDt), and the next cycle sees @@ -275,14 +380,17 @@ public LaunchingParameters getParameters( *

2. DEFENSE/PUSH COMPENSATION: Slip filter blended on top of wheel speeds. The slip is the * difference between pose-derived velocity and wheel velocity. A variable-alpha low-pass filter * separates real push events (large, sustained slip) from vision noise (small, transient). Gated - * off when stationary to prevent estimator noise from being amplified into jitter. + * off when stationary to prevent estimator noise from being amplified into jitter. The filter is + * only active when isSlipping is true, confirmed by stator current drop + velocity spike on at + * least SLIP_MODULE_COUNT_THRESHOLD modules. * * @param driveState the drivebase's SwerveDriveState * @param turretSubsystem the turretSubsystem object + * @param isSlipping whether wheel slip has been confirmed this cycle by motor signals * @return LaunchingParameters record holding all the target values */ public LaunchingParameters calculate( - SwerveDriveState driveState, TurretSubsystem turretSubsystem) { + SwerveDriveState driveState, TurretSubsystem turretSubsystem, boolean isSlipping) { Pose2d estimatedPose = driveState.Pose; ChassisSpeeds wheelSpeeds = driveState.Speeds; @@ -301,7 +409,12 @@ public LaunchingParameters calculate( ChassisSpeeds effectiveSpeeds = wheelSpeeds; double poseDt = timestamp - prevTimestamp; - if (robotIsMoving && poseDt > MIN_POSE_DT && poseDt < MAX_POSE_DT && false) { + + // Slip filter is gated on three conditions: + // 1. robotIsMoving — prevents estimator noise amplification when stationary + // 2. poseDt in valid range — ensures pose-diff gives a meaningful velocity + // 3. isSlipping — confirmed by stator current drop + velocity spike this cycle + if (robotIsMoving && poseDt > MIN_POSE_DT && poseDt < MAX_POSE_DT && isSlipping) { Twist2d twist = prevPose.log(estimatedPose); // Slip = (pose-derived velocity) - (wheel velocity). When wheels match pose, @@ -341,7 +454,9 @@ public LaunchingParameters calculate( wheelSpeeds.vyMetersPerSecond + filteredSlip.vyMetersPerSecond, wheelSpeeds.omegaRadiansPerSecond + filteredSlip.omegaRadiansPerSecond); } else { - // Reset slip filter when robot stops or poseDt is out of range + // Reset slip filter when robot stops, poseDt is out of range, or no slip confirmed. + // Resetting on !isSlipping ensures stale slip state from a previous push event + // doesn't persist and bias effectiveSpeeds during normal driving. filteredSlip = new ChassisSpeeds(0, 0, 0); slipFastMode = false; } From f868a8ee1b4bdac43d41540b36721c3ae4a49111 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 21 May 2026 19:26:39 -0700 Subject: [PATCH 022/114] increased thresholds --- .../java/frc/robot/subsystems/launcher/LaunchCalculator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java index 7e326f123..f7d18d8fa 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java @@ -126,8 +126,8 @@ public static LaunchCalculator getInstance() { // at 50 Hz on carpet typically spikes > 3–5 rot/s per 20 ms. Start around 3.0. // - SLIP_MODULE_COUNT_THRESHOLD: 1 = very sensitive (any single wheel), 2 = more // conservative. Start at 1 and raise if you see false positives during defense. - private static final double SLIP_CURRENT_THRESHOLD = 15.0; // Amps — tune this - private static final double SLIP_VELOCITY_DELTA_THRESHOLD = 3.0; // rot/s per 20ms — tune this + private static final double SLIP_CURRENT_THRESHOLD = 75.0; // Amps — tune this + private static final double SLIP_VELOCITY_DELTA_THRESHOLD = 1.0; // rot/s per 20ms — tune this private static final int SLIP_MODULE_COUNT_THRESHOLD = 1; // modules slipping to confirm // Per-module velocity history for delta computation (rot/s, one entry per module) From 0a89655ebb83871117c54bd59c1c698f4dd37ba7 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 25 May 2026 15:46:32 -0700 Subject: [PATCH 023/114] mostly working start pose filtering --- src/main/deploy/autos/paths/Sample 5.json | 22 ++ .../frc/robot/subsystems/auto/BLineLogic.java | 213 +++++------------- .../frc/robot/subsystems/auto/BLinePath.java | 5 + 3 files changed, 83 insertions(+), 157 deletions(-) create mode 100644 src/main/deploy/autos/paths/Sample 5.json diff --git a/src/main/deploy/autos/paths/Sample 5.json b/src/main/deploy/autos/paths/Sample 5.json new file mode 100644 index 000000000..4ac47de09 --- /dev/null +++ b/src/main/deploy/autos/paths/Sample 5.json @@ -0,0 +1,22 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.6, + "y_meters": 4.035, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 6.006429555414962, + "y_meters": 3.802019247398851, + "intermediate_handoff_radius_meters": 0.2 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index edd593892..f7a70dc48 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -3,41 +3,27 @@ package frc.robot.subsystems.auto; import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.FileVersionException; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; - import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; - import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; - import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; - import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; - import frc.robot.util.simulation.RobotSim; - -import java.io.IOException; import java.util.ArrayList; import java.util.List; -import org.json.simple.parser.ParseException; - public class BLineLogic { private static Subsystems s; @@ -45,27 +31,11 @@ public class BLineLogic { // ========================= START POSITIONS ========================= public enum StartPosition { + LEFT_TRENCH("Left Trench", new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90))), + + CENTER("Center", new Pose2d(3.600, 4.035, Rotation2d.fromDegrees(0))), - LEFT_TRENCH( - "Left Trench", - new Pose2d( - 4.013, - 7.597, - Rotation2d.fromDegrees(90))), - - CENTER( - "Center", - new Pose2d( - 3.600, - 4.035, - Rotation2d.fromDegrees(0))), - - RIGHT_TRENCH( - "Right Trench", - new Pose2d( - 4.013, - 0.473, - Rotation2d.fromDegrees(-90))), + RIGHT_TRENCH("Right Trench", new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90))), MISC("Misc", null); @@ -87,19 +57,14 @@ public enum StartPosition { private static final SendableChooser startPositionChooser = new SendableChooser<>(); - private static final SendableChooser autoChooser = - new SendableChooser<>(); + private static final DynamicSendableChooser autoChooser = new DynamicSendableChooser<>(); - private static final SendableChooser gameObjects = - new SendableChooser<>(); + private static final SendableChooser gameObjects = new SendableChooser<>(); private static final NetworkTableEntry autoDelayEntry = - NetworkTableInstance.getDefault() - .getTable("Autos") - .getEntry("Auto Delay"); + NetworkTableInstance.getDefault().getTable("Autos").getEntry("Auto Delay"); - public static final String keys = - "RB=Right Bump, LB=Left Bump, LT=Left Trench, RT=Right Trench"; + public static final String keys = "RB=Right Bump, LB=Left Bump, LT=Left Trench, RT=Right Trench"; // ========================= PATH BUILDER ========================= @@ -119,17 +84,11 @@ public static void init(Subsystems subsystems) { // ========================= DEFINE AUTOS HERE ========================= - autos.add(new BLinePath( - "Sample 1", - "sample1")); + autos.add(new BLinePath("Sample 1", "sample1")); - autos.add(new BLinePath( - "Sample 2", - "sample2")); + autos.add(new BLinePath("Sample 2", "sample2")); - autos.add(new BLinePath( - "Sample 3", - "sample3")); + autos.add(new BLinePath("Sample 5", "Sample 5")); } // ========================= CONFIGURE ========================= @@ -139,29 +98,17 @@ public static void configure(Subsystems s) { pathBuilder = new FollowPath.Builder( s.drivebaseSubsystem, - () -> s.drivebaseSubsystem.getState().Pose, - () -> s.drivebaseSubsystem.getState().Speeds, - (speeds) -> s.drivebaseSubsystem.setControl( new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds( - ChassisSpeeds.discretize( - speeds, - 0.020))), - + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))), new PIDController(3.0, 0.0, 0.0), - new PIDController(5.0, 0.0, 0.0), - new PIDController(2.0, 0.0, 0.0)) - .withDefaultShouldFlip() - - .withPoseReset( - pose -> s.drivebaseSubsystem.resetPose(pose)); + .withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); } // ========================= SMARTDASHBOARD ========================= @@ -170,15 +117,11 @@ public static void initSmartDashboard() { // starting positions - startPositionChooser.setDefaultOption( - StartPosition.MISC.title, - StartPosition.MISC); + startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); for (StartPosition pos : StartPosition.values()) { - startPositionChooser.addOption( - pos.title, - pos); + startPositionChooser.addOption(pos.title, pos); } // game objects chooser @@ -188,31 +131,23 @@ public static void initSmartDashboard() { // auto chooser refreshAutoChooser(); + Commands.print("started" + startPositionChooser.getSelected().toString()); // NT / Elastic publishing - SmartDashboard.putData( - "Starting Position", - startPositionChooser); + SmartDashboard.putData("Starting Position", startPositionChooser); - SmartDashboard.putData( - "Auto Mode", - gameObjects); + SmartDashboard.putData("Auto Mode", gameObjects); - SmartDashboard.putData( - "Available Auto Variants", - autoChooser); + SmartDashboard.putData("Available Auto Variants", autoChooser); - SmartDashboard.putString( - "Auto Key", - keys); + SmartDashboard.putString("Auto Key", keys); autoDelayEntry.setDouble(0.0); // update chooser when selection changes - startPositionChooser.onChange( - v -> refreshAutoChooser()); + startPositionChooser.onChange(v -> refreshAutoChooser()); // debug @@ -220,10 +155,7 @@ public static void initSmartDashboard() { for (BLinePath auto : autos) { - System.out.println( - auto.getDisplayName() - + " | " - + auto.getStartPose2d()); + System.out.println(auto.getDisplayName() + " | " + auto.getStartPose2d()); } System.out.println("======================================"); @@ -233,21 +165,32 @@ public static void initSmartDashboard() { private static void refreshAutoChooser() { - StartPosition selectedStart = - startPositionChooser.getSelected(); + autoChooser.clearOptions(); + + StartPosition selectedStart = startPositionChooser.getSelected(); if (selectedStart == null) { + + System.out.println("START POSITION NULL"); + selectedStart = StartPosition.MISC; } + + boolean first = true; for (BLinePath auto : autos) { + Pose2d autoPose = auto.getStartPose2d(); + boolean matches = selectedStart == StartPosition.MISC - || - auto.getStartPose2d() == selectedStart.startPose; + || (autoPose != null + && selectedStart.startPose != null + && autoPose.getTranslation().getDistance(selectedStart.startPose.getTranslation()) + < 0.05); + if (!matches) { continue; @@ -255,28 +198,17 @@ private static void refreshAutoChooser() { if (first) { - autoChooser.setDefaultOption( - auto.getDisplayName(), - auto.getDisplayName()); + autoChooser.setDefaultOption(auto.getDisplayName(), auto.getDisplayName()); first = false; } else { - autoChooser.addOption( - auto.getDisplayName(), - auto.getDisplayName()); + autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); } } - // safety fallback - - if (first && !autos.isEmpty()) { - autoChooser.setDefaultOption( - autos.get(0).getDisplayName(), - autos.get(0).getDisplayName()); - } } // ========================= GETTERS ========================= @@ -288,8 +220,7 @@ public static String getSelectedAutoName() { public static BLinePath getSelectedAutoPath() { - String selectedName = - autoChooser.getSelected(); + String selectedName = autoChooser.getSelected(); if (selectedName == null) { return null; @@ -307,8 +238,7 @@ public static BLinePath getSelectedAutoPath() { public static Pose2d getSelectedAutoStartingPose() { - BLinePath selected = - getSelectedAutoPath(); + BLinePath selected = getSelectedAutoPath(); if (selected == null) { return Pose2d.kZero; @@ -319,96 +249,65 @@ public static Pose2d getSelectedAutoStartingPose() { public static Command getSelectedAuto() { - double delay = - autoDelayEntry.getDouble(0.0); + double delay = autoDelayEntry.getDouble(0.0); - BLinePath selected = - getSelectedAutoPath(); + BLinePath selected = getSelectedAutoPath(); if (selected == null) { - System.out.println( - "NO AUTO SELECTED"); + System.out.println("NO AUTO SELECTED"); return Commands.none(); } - System.out.println( - "RUNNING AUTO: " - + selected.getDisplayName()); - - return Commands.waitSeconds(delay) - - .andThen(pathBuilder.build(new Path(selected.getAutoName()))); + System.out.println("RUNNING AUTO: " + selected.getDisplayName()); - + return Commands.waitSeconds(delay).andThen(pathBuilder.build(new Path(selected.getAutoName()))); } // ========================= PATHPLANNER DIRECT ========================= - - // ========================= COMMANDS ========================= public static Command intakeCommand() { - return Commands.runOnce( - () -> - Controls.intakeMode = - IntakeMode.INTAKE) - + return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) .withName("Auto Intake Command"); } public static Command climbCommand() { - return Commands.none() - .withName("Auto Climb Command"); + return Commands.none().withName("Auto Climb Command"); } public static void cancelCommand() { - CommandScheduler.getInstance() - .cancel(bLineLaunching); + CommandScheduler.getInstance().cancel(bLineLaunching); } // ========================= EVENT TRIGGERS ========================= private static void registerCommands() { - if (s.launcherSubsystem != null - && s.indexerSubsystem != null) { + if (s.launcherSubsystem != null && s.indexerSubsystem != null) { if (Robot.isSimulation()) { - bLineLaunching = - RobotSim.launch(s,1); + bLineLaunching = RobotSim.launch(s, 1); FollowPath.registerEventTrigger( - "launch", - - bLineLaunching.andThen( - Commands.print( - "LAUNCH FINISHED"))); + "launch", bLineLaunching.andThen(Commands.print("LAUNCH FINISHED"))); } else { - bLineLaunching = - Commands.none(); + bLineLaunching = Commands.none(); - FollowPath.registerEventTrigger( - "launch", - bLineLaunching); + FollowPath.registerEventTrigger("launch", bLineLaunching); } } - FollowPath.registerEventTrigger( - "intake", - intakeCommand()); + FollowPath.registerEventTrigger("intake", intakeCommand()); - FollowPath.registerEventTrigger( - "climb", - climbCommand()); + FollowPath.registerEventTrigger("climb", climbCommand()); } } - diff --git a/src/main/java/frc/robot/subsystems/auto/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLinePath.java index 1a247daee..48dff47bc 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLinePath.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import frc.robot.lib.BLine.Path; +import frc.robot.lib.BLine.Path.PathElement; public class BLinePath { @@ -37,9 +38,13 @@ public String getAutoName() { } public Pose2d getStartPose2d() { + + + return startPose; } + public boolean isVision() { return vision; } From 4fd8f68899c412150bbfa76088ad0c5bb120fb0a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 26 May 2026 20:02:39 -0700 Subject: [PATCH 024/114] auto chooser is automatically populated via list --- src/main/deploy/autos/paths/sample1.json | 51 +++++-- src/main/java/frc/robot/Robot.java | 3 +- .../frc/robot/subsystems/auto/BLineLogic.java | 144 +++++++++--------- 3 files changed, 110 insertions(+), 88 deletions(-) diff --git a/src/main/deploy/autos/paths/sample1.json b/src/main/deploy/autos/paths/sample1.json index 53ecfb229..811fa7fd0 100644 --- a/src/main/deploy/autos/paths/sample1.json +++ b/src/main/deploy/autos/paths/sample1.json @@ -12,34 +12,55 @@ "profiled_rotation": true } }, - { - "type": "rotation", - "rotation_radians": -1.5707963267948966, - "t_ratio": 1.0, - "profiled_rotation": true - }, { "type": "event_trigger", - "t_ratio": 0.7, + "t_ratio": 0.6999999999999996, "lib_key": "intake" }, { - "type": "translation", - "x_meters": 5.594848920863306, - "y_meters": 0.5519712230215834, - "intermediate_handoff_radius_meters": 0.2 + "type": "waypoint", + "translation_target": { + "x_meters": 5.793757559634994, + "y_meters": 0.5707461300309609, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.6, - "y_meters": 1.4, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 7.980123839009286, + "y_meters": 1.2696718266253875, + "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { "rotation_radians": -1.5707963267948966, "profiled_rotation": true } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.7444223890237645, + "y_meters": 3.466294139920257, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 3.141592653589793, + "profiled_rotation": true + } } - ] + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 3 + } + ] + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b50144b57..b6d5f579d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -124,6 +124,7 @@ protected Robot() { } BLineLogic.init(subsystems); BLineLogic.configure(subsystems); + BLineLogic.initSmartDashboard(); } @@ -145,7 +146,7 @@ protected Robot() { if (SubsystemConstants.DRIVEBASE_ENABLED) { // PathPlannerLogic.initCommandsAndPaths(true); - + // CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); } diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index f7a70dc48..2eed88b02 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -22,7 +22,9 @@ import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.util.simulation.RobotSim; import java.util.ArrayList; +import java.util.HashMap; import java.util.List; +import java.util.Map; public class BLineLogic { @@ -48,11 +50,11 @@ public enum StartPosition { } } - // ========================= AUTOS ========================= + private static final List autos = new ArrayList<>(); - // ========================= CHOOSERS ========================= + private static final SendableChooser startPositionChooser = new SendableChooser<>(); @@ -66,7 +68,15 @@ public enum StartPosition { public static final String keys = "RB=Right Bump, LB=Left Bump, LT=Left Trench, RT=Right Trench"; - // ========================= PATH BUILDER ========================= + private static BLinePath defaultPath; + + private static List rebuiltPaths = List.of(); + + private static Map> commandsMap = Map.of(); + + private static final Map namesToAuto = new HashMap<>(); + private static boolean pathsInitialized = false; + public static FollowPath.Builder pathBuilder; @@ -74,24 +84,46 @@ public enum StartPosition { // ========================= INIT ========================= - public static void init(Subsystems subsystems) { +public static void init(Subsystems subsystems) { - s = subsystems; + s = subsystems; - registerCommands(); + registerCommands(); - autos.clear(); + autos.clear(); - // ========================= DEFINE AUTOS HERE ========================= + if (pathsInitialized) { + return; + } + + defaultPath = new BLinePath("test1", "test1"); + + List physicalRebuiltPaths = + List.of( + defaultPath, + new BLinePath("Sample 1", "sample1"), + new BLinePath("Sample 2", "sample2"), + new BLinePath("Sample 5", "Sample 5")); - autos.add(new BLinePath("Sample 1", "sample1")); + rebuiltPaths = physicalRebuiltPaths; - autos.add(new BLinePath("Sample 2", "sample2")); - autos.add(new BLinePath("Sample 5", "Sample 5")); + autos.clear(); + autos.addAll(rebuiltPaths); + + commandsMap = Map.of(0, rebuiltPaths); + + namesToAuto.clear(); + for (List list : commandsMap.values()) { + for (BLinePath auto : list) { + namesToAuto.put(auto.getDisplayName(), auto); + } } - // ========================= CONFIGURE ========================= + pathsInitialized = true; +} + + public static void configure(Subsystems s) { @@ -111,7 +143,7 @@ public static void configure(Subsystems s) { .withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); } - // ========================= SMARTDASHBOARD ========================= + public static void initSmartDashboard() { @@ -124,16 +156,15 @@ public static void initSmartDashboard() { startPositionChooser.addOption(pos.title, pos); } - // game objects chooser + gameObjects.setDefaultOption("0", 0); - // auto chooser - refreshAutoChooser(); - Commands.print("started" + startPositionChooser.getSelected().toString()); + filterAutos(0); + + - // NT / Elastic publishing SmartDashboard.putData("Starting Position", startPositionChooser); @@ -145,73 +176,45 @@ public static void initSmartDashboard() { autoDelayEntry.setDouble(0.0); - // update chooser when selection changes - startPositionChooser.onChange(v -> refreshAutoChooser()); + startPositionChooser.onChange(v -> filterAutos(gameObjects.getSelected())); - // debug - System.out.println("========== REGISTERED AUTOS =========="); - for (BLinePath auto : autos) { - System.out.println(auto.getDisplayName() + " | " + auto.getStartPose2d()); - } - System.out.println("======================================"); } - // ========================= AUTO FILTERING ========================= - private static void refreshAutoChooser() { - - autoChooser.clearOptions(); - StartPosition selectedStart = startPositionChooser.getSelected(); + public static void filterAutos(int numGameObjects) { - if (selectedStart == null) { + autoChooser.clearOptions(); - System.out.println("START POSITION NULL"); + StartPosition selected = startPositionChooser.getSelected(); - selectedStart = StartPosition.MISC; - } - - - - boolean first = true; - - for (BLinePath auto : autos) { - - Pose2d autoPose = auto.getStartPose2d(); - - boolean matches = - selectedStart == StartPosition.MISC - || (autoPose != null - && selectedStart.startPose != null - && autoPose.getTranslation().getDistance(selectedStart.startPose.getTranslation()) - < 0.05); + if (selected == null) { + selected = StartPosition.MISC; + } + for (BLinePath auto : autos) { - if (!matches) { - continue; - } + Pose2d autoPose = auto.getStartPose2d(); - if (first) { - autoChooser.setDefaultOption(auto.getDisplayName(), auto.getDisplayName()); - first = false; - } else { + if (autoPose.equals(startPositionChooser.getSelected().startPose)) { - autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); - } + autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); } + else { - + } } +} + - // ========================= GETTERS ========================= public static String getSelectedAutoName() { @@ -249,23 +252,20 @@ public static Pose2d getSelectedAutoStartingPose() { public static Command getSelectedAuto() { - double delay = autoDelayEntry.getDouble(0.0); + double delay = autoDelayEntry.getDouble(0.0); - BLinePath selected = getSelectedAutoPath(); + BLinePath path = namesToAuto.get(getSelectedAutoName()); - if (selected == null) { + if (path == null) { - System.out.println("NO AUTO SELECTED"); - - return Commands.none(); - } + return Commands.none(); + } - System.out.println("RUNNING AUTO: " + selected.getDisplayName()); + return Commands.waitSeconds(delay) + .andThen(pathBuilder.build(new Path(path.getAutoName()))); +} - return Commands.waitSeconds(delay).andThen(pathBuilder.build(new Path(selected.getAutoName()))); - } - // ========================= PATHPLANNER DIRECT ========================= // ========================= COMMANDS ========================= From b6706b7e575c2e67b3195495815bd803ebf6f738 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 26 May 2026 20:24:03 -0700 Subject: [PATCH 025/114] added auto command sequence --- src/main/deploy/autos/paths/sample1.json | 28 ++++++------------ src/main/deploy/autos/paths/sample2.json | 29 ++++++++++++------- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/auto/BLineLogic.java | 4 +++ 4 files changed, 32 insertions(+), 31 deletions(-) diff --git a/src/main/deploy/autos/paths/sample1.json b/src/main/deploy/autos/paths/sample1.json index 811fa7fd0..13b9de36b 100644 --- a/src/main/deploy/autos/paths/sample1.json +++ b/src/main/deploy/autos/paths/sample1.json @@ -5,18 +5,13 @@ "translation_target": { "x_meters": 4.013, "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.2 + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { "rotation_radians": -1.5707963267948966, "profiled_rotation": true } }, - { - "type": "event_trigger", - "t_ratio": 0.6999999999999996, - "lib_key": "intake" - }, { "type": "waypoint", "translation_target": { @@ -30,31 +25,26 @@ } }, { - "type": "waypoint", - "translation_target": { - "x_meters": 7.980123839009286, - "y_meters": 1.2696718266253875, - "intermediate_handoff_radius_meters": 0.4 - }, - "rotation_target": { - "rotation_radians": -1.5707963267948966, - "profiled_rotation": true - } + "type": "translation", + "x_meters": 7.35860681114551, + "y_meters": 1.0239752321981426, + "intermediate_handoff_radius_meters": 0.4 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.7444223890237645, - "y_meters": 3.466294139920257, + "x_meters": 7.75, + "y_meters": 3.55, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": 3.141592653589793, + "rotation_radians": -2.356194490192345, "profiled_rotation": true } } ], "constraints": { + "end_translation_tolerance_meters": 0.07, "max_velocity_meters_per_sec": [ { "value": 2.0, diff --git a/src/main/deploy/autos/paths/sample2.json b/src/main/deploy/autos/paths/sample2.json index 45639dd98..e1125e22b 100644 --- a/src/main/deploy/autos/paths/sample2.json +++ b/src/main/deploy/autos/paths/sample2.json @@ -3,29 +3,36 @@ { "type": "waypoint", "translation_target": { - "x_meters": 7.6, - "y_meters": 1.4, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 7.75, + "y_meters": 3.55, + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -2.356194490192345, "profiled_rotation": true } }, { - "type": "event_trigger", - "t_ratio": 1.0, - "lib_key": "launch" + "type": "rotation", + "rotation_radians": -1.5707963267948966, + "t_ratio": 0.7, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 7.843442724458196, + "y_meters": 1.9666006191950478, + "intermediate_handoff_radius_meters": 1.0 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.8, - "y_meters": 3.5, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 4.0238606811145505, + "y_meters": 0.472999999999999, + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -2.0943951023931953, + "rotation_radians": -1.5707963267948966, "profiled_rotation": true } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b6d5f579d..3db0deac5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -287,7 +287,7 @@ public void autonomousInit() { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(BLineLogic.getSelectedAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralAuto()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java index 2eed88b02..6b9e4a528 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLineLogic.java @@ -264,6 +264,10 @@ public static Command getSelectedAuto() { return Commands.waitSeconds(delay) .andThen(pathBuilder.build(new Path(path.getAutoName()))); } +public static Command buildRTNeutralAuto() { + return Commands.sequence(Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + pathBuilder.build(new Path("sample1")),(pathBuilder.build(new Path("sample2")))); +} From 02f781eddc0a5845028dd7bc08032081e709ae7a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 11:37:03 -0700 Subject: [PATCH 026/114] reorganized autos folder --- src/main/deploy/autos/config.json | 4 +- src/main/deploy/autos/paths/RTNeutral.json | 117 +++++++++ src/main/deploy/autos/paths/sample1.json | 56 ----- src/main/java/frc/robot/Controls.java | 2 +- src/main/java/frc/robot/Robot.java | 2 +- .../auto/{ => BLine}/BLineLogic.java | 236 +++++++++--------- .../auto/{ => BLine}/BLinePath.java | 5 +- .../auto/{ => Misc}/AutoDriveRotate.java | 2 +- .../{ => Misc}/DynamicSendableChooser.java | 2 +- .../{ => PathPlanner}/AutoBuilderConfig.java | 2 +- .../auto/{ => PathPlanner}/AutoPath.java | 4 +- .../{ => PathPlanner}/AutonomousField.java | 3 +- .../{ => PathPlanner}/PathPlannerAutos.java | 2 +- .../{ => PathPlanner}/PathPlannerLogic.java | 3 +- src/test/java/AutosTest.java | 7 +- 15 files changed, 253 insertions(+), 194 deletions(-) create mode 100644 src/main/deploy/autos/paths/RTNeutral.json delete mode 100644 src/main/deploy/autos/paths/sample1.json rename src/main/java/frc/robot/subsystems/auto/{ => BLine}/BLineLogic.java (55%) rename src/main/java/frc/robot/subsystems/auto/{ => BLine}/BLinePath.java (92%) rename src/main/java/frc/robot/subsystems/auto/{ => Misc}/AutoDriveRotate.java (99%) rename src/main/java/frc/robot/subsystems/auto/{ => Misc}/DynamicSendableChooser.java (99%) rename src/main/java/frc/robot/subsystems/auto/{ => PathPlanner}/AutoBuilderConfig.java (98%) rename src/main/java/frc/robot/subsystems/auto/{ => PathPlanner}/AutoPath.java (95%) rename src/main/java/frc/robot/subsystems/auto/{ => PathPlanner}/AutonomousField.java (98%) rename src/main/java/frc/robot/subsystems/auto/{ => PathPlanner}/PathPlannerAutos.java (98%) rename src/main/java/frc/robot/subsystems/auto/{ => PathPlanner}/PathPlannerLogic.java (98%) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 4831e7df1..174900797 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -14,8 +14,8 @@ } }, "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 5.0, - "default_max_acceleration_meters_per_sec2": 4.5, + "default_max_velocity_meters_per_sec": 4.0, + "default_max_acceleration_meters_per_sec2": 3.0, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json new file mode 100644 index 000000000..8c61415db --- /dev/null +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -0,0 +1,117 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.7799999999999998, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.451009414929385, + "y_meters": 0.5899065232010745, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.72164648834793, + "y_meters": 0.8234506861322366, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -2.356194490192345, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.561374839090348, + "y_meters": 1.459361916797997, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.859452627810324, + "y_meters": 3.0501906595772406, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 6.791302733637762, + "y_meters": 1.605102354699956, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "translation", + "x_meters": 4.9956480573787685, + "y_meters": 0.5198937216064312, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.5310636157189235, + "y_meters": 0.4167625899280516, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.05, + "max_velocity_meters_per_sec": [ + { + "value": 4.0, + "start_ordinal": 7, + "end_ordinal": 7 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 2.5, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 2.0, + "start_ordinal": 5, + "end_ordinal": 6 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample1.json b/src/main/deploy/autos/paths/sample1.json deleted file mode 100644 index 13b9de36b..000000000 --- a/src/main/deploy/autos/paths/sample1.json +++ /dev/null @@ -1,56 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.013, - "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -1.5707963267948966, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.793757559634994, - "y_meters": 0.5707461300309609, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -1.5707963267948966, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 7.35860681114551, - "y_meters": 1.0239752321981426, - "intermediate_handoff_radius_meters": 0.4 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.75, - "y_meters": 3.55, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": -2.356194490192345, - "profiled_rotation": true - } - } - ], - "constraints": { - "end_translation_tolerance_meters": 0.07, - "max_velocity_meters_per_sec": [ - { - "value": 2.0, - "start_ordinal": 2, - "end_ordinal": 3 - } - ] - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index f6efad751..1000f089a 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -30,7 +30,7 @@ import frc.robot.sensors.LEDSubsystem; import frc.robot.sensors.LEDSubsystem.LEDMode; import frc.robot.sim.SimWrapper; -import frc.robot.subsystems.auto.AutoDriveRotate; +import frc.robot.subsystems.auto.Misc.AutoDriveRotate; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.subsystems.launcher.TurretSubsystem; import frc.robot.util.AllianceUtils; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3db0deac5..a02580433 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,7 +30,7 @@ import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; -import frc.robot.subsystems.auto.BLineLogic; +import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; import frc.robot.util.DriveStateNtLogger; diff --git a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java similarity index 55% rename from src/main/java/frc/robot/subsystems/auto/BLineLogic.java rename to src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 6b9e4a528..8f156d907 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -1,6 +1,4 @@ -// ========================= BLineLogic.java ========================= - -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.BLine; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.controller.PIDController; @@ -19,6 +17,7 @@ import frc.robot.Subsystems; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; +import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.util.simulation.RobotSim; import java.util.ArrayList; @@ -30,15 +29,10 @@ public class BLineLogic { private static Subsystems s; - // ========================= START POSITIONS ========================= - public enum StartPosition { LEFT_TRENCH("Left Trench", new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90))), - CENTER("Center", new Pose2d(3.600, 4.035, Rotation2d.fromDegrees(0))), - RIGHT_TRENCH("Right Trench", new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90))), - MISC("Misc", null); public final String title; @@ -50,12 +44,8 @@ public enum StartPosition { } } - - private static final List autos = new ArrayList<>(); - - private static final SendableChooser startPositionChooser = new SendableChooser<>(); @@ -69,61 +59,48 @@ public enum StartPosition { public static final String keys = "RB=Right Bump, LB=Left Bump, LT=Left Trench, RT=Right Trench"; private static BLinePath defaultPath; - private static List rebuiltPaths = List.of(); - private static Map> commandsMap = Map.of(); - private static final Map namesToAuto = new HashMap<>(); - private static boolean pathsInitialized = false; + private static boolean pathsInitialized = false; public static FollowPath.Builder pathBuilder; - private static Command bLineLaunching; // ========================= INIT ========================= -public static void init(Subsystems subsystems) { - - s = subsystems; + public static void init(Subsystems subsystems) { - registerCommands(); + s = subsystems; - autos.clear(); + registerCommands(); - if (pathsInitialized) { - return; - } + if (pathsInitialized) return; - defaultPath = new BLinePath("test1", "test1"); + defaultPath = new BLinePath("test1", "test1"); - List physicalRebuiltPaths = - List.of( - defaultPath, - new BLinePath("Sample 1", "sample1"), - new BLinePath("Sample 2", "sample2"), - new BLinePath("Sample 5", "Sample 5")); + rebuiltPaths = + List.of( + defaultPath, + new BLinePath("RTNEUTRAL", "RTNEUTRAL"), + new BLinePath("Sample 2", "sample2"), + new BLinePath("Sample 5", "Sample 5")); - rebuiltPaths = physicalRebuiltPaths; + autos.clear(); + autos.addAll(rebuiltPaths); + commandsMap = Map.of(0, rebuiltPaths); - autos.clear(); - autos.addAll(rebuiltPaths); - - commandsMap = Map.of(0, rebuiltPaths); - - namesToAuto.clear(); - for (List list : commandsMap.values()) { - for (BLinePath auto : list) { - namesToAuto.put(auto.getDisplayName(), auto); + namesToAuto.clear(); + for (List list : commandsMap.values()) { + for (BLinePath auto : list) { + namesToAuto.put(auto.getDisplayName(), auto); + } } - } - - pathsInitialized = true; -} - + pathsInitialized = true; + } public static void configure(Subsystems s) { @@ -143,81 +120,67 @@ public static void configure(Subsystems s) { .withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); } - + // ========================= LOGGING ========================= public static void initSmartDashboard() { - // starting positions - startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); for (StartPosition pos : StartPosition.values()) { - startPositionChooser.addOption(pos.title, pos); } - - gameObjects.setDefaultOption("0", 0); - - filterAutos(0); - - - + filterAutos(0); SmartDashboard.putData("Starting Position", startPositionChooser); - SmartDashboard.putData("Auto Mode", gameObjects); - SmartDashboard.putData("Available Auto Variants", autoChooser); - SmartDashboard.putString("Auto Key", keys); autoDelayEntry.setDouble(0.0); + startPositionChooser.onChange( + v -> { + filterAutos(gameObjects.getSelected()); + updateInitialHeading(); + }); - startPositionChooser.onChange(v -> filterAutos(gameObjects.getSelected())); - - - - - + autoChooser.onChange(v -> updateInitialHeading()); } - // ========================= AUTO FILTERING ========================= - - - public static void filterAutos(int numGameObjects) { - autoChooser.clearOptions(); + // ========================= AUTOS FILTERING ========================= - StartPosition selected = startPositionChooser.getSelected(); + public static void filterAutos(int numGameObjects) { - if (selected == null) { - selected = StartPosition.MISC; - } + autoChooser.clearOptions(); - for (BLinePath auto : autos) { + StartPosition selected = startPositionChooser.getSelected(); - Pose2d autoPose = auto.getStartPose2d(); + // fallback safety + if (selected == null) { + selected = StartPosition.MISC; + } + for (BLinePath auto : autos) { + if (selected == StartPosition.MISC) { + autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); + } + Pose2d autoPose = auto.getStartPose2d(); - if (autoPose.equals(startPositionChooser.getSelected().startPose)) { - - autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); - } - else { + if (autoPose != null && selected.startPose != null && autoPose.equals(selected.startPose)) { + autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); + } } } -} - + // ========================= SELECTION METHODS ========================= public static String getSelectedAutoName() { - return autoChooser.getSelected(); } @@ -226,92 +189,121 @@ public static BLinePath getSelectedAutoPath() { String selectedName = autoChooser.getSelected(); if (selectedName == null) { - return null; + return defaultPath; } - for (BLinePath auto : autos) { - - if (auto.getDisplayName().equals(selectedName)) { - return auto; - } - } - - return null; + return namesToAuto.getOrDefault(selectedName, defaultPath); } public static Pose2d getSelectedAutoStartingPose() { BLinePath selected = getSelectedAutoPath(); - if (selected == null) { - return Pose2d.kZero; - } + if (selected == null) return Pose2d.kZero; return selected.getStartPose2d(); } + // ========================= AUTO EXECUTION ========================= + public static Command getSelectedAuto() { - double delay = autoDelayEntry.getDouble(0.0); + double delay = autoDelayEntry.getDouble(0.0); - BLinePath path = namesToAuto.get(getSelectedAutoName()); + BLinePath path = getSelectedAutoPath(); + s.drivebaseSubsystem.resetRotation(path.getPath().getInitialModuleDirection()); + return Commands.waitSeconds(delay).andThen(pathBuilder.build(path.getPath())); + } + + public static Command buildRTNeutralAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildAndSetHeading(new Path("RTNEUTRAL")), + launcherCommand(5)); + } - if (path == null) { + private static Command buildAndSetHeading(Path path) { - return Commands.none(); + return Commands.runOnce( + () -> s.drivebaseSubsystem.resetRotation(path.getInitialModuleDirection())) + .andThen(pathBuilder.build(path)); } - return Commands.waitSeconds(delay) - .andThen(pathBuilder.build(new Path(path.getAutoName()))); -} -public static Command buildRTNeutralAuto() { - return Commands.sequence(Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - pathBuilder.build(new Path("sample1")),(pathBuilder.build(new Path("sample2")))); -} + private static void updateInitialHeading() { + BLinePath selected = getSelectedAutoPath(); + if (selected == null || selected.getPath() == null) { + SmartDashboard.putNumber("Initial Heading", 0.0); + return; + } - // ========================= COMMANDS ========================= + Pose2d start = selected.getPath().getStartPose(); - public static Command intakeCommand() { + SmartDashboard.putNumber("Initial Heading", start.getRotation().getDegrees()); + } + public static Command intakeCommand() { return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) .withName("Auto Intake Command"); } - public static Command climbCommand() { + public static Command launcherCommand(double timeout) { + return Commands.parallel( + Commands.runOnce( + () -> { + s.flywheels.resetFuelCheck(); + }), + s.launcherSubsystem.launcherAimCommand(), + Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) + .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) + // .until(() -> s.flywheels.isOutOfFuel()) + .withTimeout(timeout) + .andThen(s.launcherSubsystem.rawStowCommand()) + .withName("Auto Launcher Command"); + } + + public static Command launcherCommand() { + return Commands.parallel( + Commands.runOnce( + () -> { + s.flywheels.resetFuelCheck(); + }), + s.launcherSubsystem.launcherAimCommand(), + Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) + .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) + // .until(() -> s.flywheels.isOutOfFuel()) + + .withName("Auto Launcher Command"); + } + public static Command stowCommand() { + return s.launcherSubsystem.rawStowCommand(); + } + + public static Command climbCommand() { return Commands.none().withName("Auto Climb Command"); } public static void cancelCommand() { - CommandScheduler.getInstance().cancel(bLineLaunching); } - // ========================= EVENT TRIGGERS ========================= - private static void registerCommands() { if (s.launcherSubsystem != null && s.indexerSubsystem != null) { if (Robot.isSimulation()) { - bLineLaunching = RobotSim.launch(s, 1); - FollowPath.registerEventTrigger( "launch", bLineLaunching.andThen(Commands.print("LAUNCH FINISHED"))); - } else { - - bLineLaunching = Commands.none(); - + bLineLaunching = launcherCommand(); FollowPath.registerEventTrigger("launch", bLineLaunching); } } FollowPath.registerEventTrigger("intake", intakeCommand()); - FollowPath.registerEventTrigger("climb", climbCommand()); } } diff --git a/src/main/java/frc/robot/subsystems/auto/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java similarity index 92% rename from src/main/java/frc/robot/subsystems/auto/BLinePath.java rename to src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index 48dff47bc..4d4d197be 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.BLine; import edu.wpi.first.math.geometry.Pose2d; import frc.robot.lib.BLine.Path; @@ -48,4 +48,7 @@ public Pose2d getStartPose2d() { public boolean isVision() { return vision; } + public Path getPath() { + return path; + } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java b/src/main/java/frc/robot/subsystems/auto/Misc/AutoDriveRotate.java similarity index 99% rename from src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java rename to src/main/java/frc/robot/subsystems/auto/Misc/AutoDriveRotate.java index 345fc3600..e874f2f28 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java +++ b/src/main/java/frc/robot/subsystems/auto/Misc/AutoDriveRotate.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.Misc; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; diff --git a/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java b/src/main/java/frc/robot/subsystems/auto/Misc/DynamicSendableChooser.java similarity index 99% rename from src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java rename to src/main/java/frc/robot/subsystems/auto/Misc/DynamicSendableChooser.java index 854828f63..2979f8896 100644 --- a/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java +++ b/src/main/java/frc/robot/subsystems/auto/Misc/DynamicSendableChooser.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.Misc; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoBuilderConfig.java similarity index 98% rename from src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoBuilderConfig.java index 302d56e38..6149da678 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoBuilderConfig.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoPath.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoPath.java similarity index 95% rename from src/main/java/frc/robot/subsystems/auto/AutoPath.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoPath.java index e59429fd8..becce8ded 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoPath.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutoPath.java @@ -1,11 +1,11 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.auto.PathPlannerLogic.StartPosition; +import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic.StartPosition; public class AutoPath { diff --git a/src/main/java/frc/robot/subsystems/auto/AutonomousField.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java similarity index 98% rename from src/main/java/frc/robot/subsystems/auto/AutonomousField.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java index d685134c9..ac1aa856a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import java.util.List; import java.util.Optional; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlannerAutos.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutos.java similarity index 98% rename from src/main/java/frc/robot/subsystems/auto/PathPlannerAutos.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutos.java index b01246bbd..dea4244e8 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlannerAutos.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutos.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.RobotConfig; diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java similarity index 98% rename from src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java index 5313f3691..ff9e2a0ea 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlannerLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.auto; +package frc.robot.subsystems.auto.PathPlanner; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -16,6 +16,7 @@ import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; +import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; import frc.robot.subsystems.intake.IntakeSubsystem.IntakeMode; import frc.robot.util.simulation.RobotSim; import java.io.IOException; diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index 4c1a53ce6..fb61e43ea 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -2,9 +2,10 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc.robot.generated.CompTunerConstants; -import frc.robot.subsystems.auto.AutoBuilderConfig; -import frc.robot.subsystems.auto.AutoPath; -import frc.robot.subsystems.auto.PathPlannerLogic; +import frc.robot.subsystems.auto.PathPlanner.AutoBuilderConfig; +import frc.robot.subsystems.auto.PathPlanner.AutoPath; +import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic; + import java.io.IOException; import java.util.Dictionary; import java.util.Enumeration; From ef9df68cf72a16c45fca675834edc4aa183a098a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 15:24:46 -0700 Subject: [PATCH 027/114] added another auto for testing --- src/main/deploy/autos/paths/RTNeutral.json | 12 +-- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 87 ++++++++++++++++++++ 2 files changed, 93 insertions(+), 6 deletions(-) create mode 100644 src/main/deploy/autos/paths/RTRBNEUTRAL.json diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 8c61415db..ce90f2caa 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -61,14 +61,14 @@ }, { "type": "translation", - "x_meters": 6.791302733637762, - "y_meters": 1.605102354699956, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 7.220338704860779, + "y_meters": 1.781764225203557, + "intermediate_handoff_radius_meters": 0.55 }, { "type": "translation", - "x_meters": 4.9956480573787685, - "y_meters": 0.5198937216064312, + "x_meters": 5.752770359537043, + "y_meters": 0.6208433618942024, "intermediate_handoff_radius_meters": 0.5 }, { @@ -88,7 +88,7 @@ "end_translation_tolerance_meters": 0.05, "max_velocity_meters_per_sec": [ { - "value": 4.0, + "value": 2.5, "start_ordinal": 7, "end_ordinal": 7 }, diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json new file mode 100644 index 000000000..6804d6f05 --- /dev/null +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -0,0 +1,87 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 3.226841726618708, + "y_meters": 1.2263237410071977, + "intermediate_handoff_radius_meters": 0.6 + }, + { + "type": "translation", + "x_meters": 4.3801654676259, + "y_meters": 2.261057553956829, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.684913669064759, + "y_meters": 2.9424676258992806, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -2.1236552362154617, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 6.867273381294974, + "y_meters": 3.1153309352518015, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.551501332157276, + "y_meters": 1.6301223021582745, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 13.962634015954636, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 1.0, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 4.0, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + } + ] + } +} \ No newline at end of file From fb8074a52dd8290dcb10108baadd312f1b22567f Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 17:56:07 -0700 Subject: [PATCH 028/114] sim event triggers + slowed speed for testing --- src/main/deploy/autos/paths/RTNeutral.json | 4 +- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 45 ++++++++++++------- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/auto/BLine/BLineLogic.java | 12 +++++ 4 files changed, 43 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index ce90f2caa..92fd408db 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -74,8 +74,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.5310636157189235, - "y_meters": 0.4167625899280516, + "x_meters": 3.55, + "y_meters": 0.473, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json index 6804d6f05..9131817f3 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRAL.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -3,7 +3,7 @@ { "type": "waypoint", "translation_target": { - "x_meters": 4.013, + "x_meters": 3.55, "y_meters": 0.473, "intermediate_handoff_radius_meters": 0.2 }, @@ -14,43 +14,54 @@ }, { "type": "translation", - "x_meters": 3.226841726618708, - "y_meters": 1.2263237410071977, + "x_meters": 3.0249424460431693, + "y_meters": 1.327273381294968, "intermediate_handoff_radius_meters": 0.6 }, + { + "type": "event_trigger", + "t_ratio": 0.6000000000000013, + "lib_key": "intake" + }, { "type": "translation", - "x_meters": 4.3801654676259, - "y_meters": 2.261057553956829, + "x_meters": 4.153028776978418, + "y_meters": 2.3620071942445975, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 5.684913669064759, - "y_meters": 2.9424676258992806, + "x_meters": 5.28111510791368, + "y_meters": 2.563906474820148, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -2.1236552362154617, + "rotation_radians": -2.0943951023931953, "profiled_rotation": true } }, { - "type": "translation", - "x_meters": 6.867273381294974, - "y_meters": 3.1153309352518015, - "intermediate_handoff_radius_meters": 0.5 + "type": "waypoint", + "translation_target": { + "x_meters": 6.034438848920874, + "y_meters": 3.4434172661870512, + "intermediate_handoff_radius_meters": 0.42 + }, + "rotation_target": { + "rotation_radians": -2.0943951023931953, + "profiled_rotation": true + } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.551501332157276, - "y_meters": 1.6301223021582745, + "x_meters": 7.879587663092531, + "y_meters": 2.538669064748202, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": 13.962634015954636, + "rotation_radians": 1.7453292519943295, "profiled_rotation": true } } @@ -58,7 +69,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 2.0, + "value": 2.5, "start_ordinal": 2, "end_ordinal": 2 }, @@ -78,7 +89,7 @@ "end_ordinal": 5 }, { - "value": 2.0, + "value": 2.5, "start_ordinal": 4, "end_ordinal": 4 } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a02580433..9fffd0c49 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -287,7 +287,7 @@ public void autonomousInit() { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralTestingAuto()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 8f156d907..bc2ecf919 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -222,6 +222,14 @@ public static Command buildRTNeutralAuto() { launcherCommand(5)); } + public static Command buildRTNeutralTestingAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildAndSetHeading(new Path("RTNEUTRAL")), + launcherCommand(5), + buildAndSetHeading(new Path("RTRBNEUTRAL"))); + } + private static Command buildAndSetHeading(Path path) { return Commands.runOnce( @@ -249,6 +257,9 @@ public static Command intakeCommand() { } public static Command launcherCommand(double timeout) { + if(Robot.isSimulation()) { + return RobotSim.launch(s,timeout); + } return Commands.parallel( Commands.runOnce( () -> { @@ -264,6 +275,7 @@ public static Command launcherCommand(double timeout) { } public static Command launcherCommand() { + return Commands.parallel( Commands.runOnce( () -> { From 1a86243291b995b8fd0320c4ccf9dc2e6bb619ce Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 17:56:16 -0700 Subject: [PATCH 029/114] slower speed --- src/main/deploy/autos/config.json | 2 +- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 174900797..555603298 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -14,7 +14,7 @@ } }, "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 4.0, + "default_max_velocity_meters_per_sec": 3.0, "default_max_acceleration_meters_per_sec2": 3.0, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json index 9131817f3..8fea8bcca 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRAL.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -79,7 +79,7 @@ "end_ordinal": 0 }, { - "value": 4.0, + "value": 3.0, "start_ordinal": 3, "end_ordinal": 3 }, From 5c6381b39ee78230036982db92e725658a5e354b Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 17:59:16 -0700 Subject: [PATCH 030/114] lowered speeds even more --- src/main/deploy/autos/config.json | 4 ++-- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 2 +- src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 555603298..95c2b6fe6 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -14,8 +14,8 @@ } }, "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 3.0, - "default_max_acceleration_meters_per_sec2": 3.0, + "default_max_velocity_meters_per_sec": 2.5, + "default_max_acceleration_meters_per_sec2": 2.0, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json index 8fea8bcca..7a6601f64 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRAL.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -79,7 +79,7 @@ "end_ordinal": 0 }, { - "value": 3.0, + "value": 2.5, "start_ordinal": 3, "end_ordinal": 3 }, diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index bc2ecf919..876469c61 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -223,6 +223,7 @@ public static Command buildRTNeutralAuto() { } public static Command buildRTNeutralTestingAuto() { + return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildAndSetHeading(new Path("RTNEUTRAL")), From 773580c4894e5806a19d26726889849abb0403b0 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 18:36:05 -0700 Subject: [PATCH 031/114] small tweaks --- .../deploy/autos/paths/RTNEUTRALTEST.json | 112 ++++++++++++++++++ .../deploy/autos/paths/RTRBNEUTRALTEST.json | 94 +++++++++++++++ src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/auto/BLine/BLineLogic.java | 14 +-- 4 files changed, 213 insertions(+), 9 deletions(-) create mode 100644 src/main/deploy/autos/paths/RTNEUTRALTEST.json create mode 100644 src/main/deploy/autos/paths/RTRBNEUTRALTEST.json diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json new file mode 100644 index 000000000..0804506a7 --- /dev/null +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -0,0 +1,112 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.451009414929385, + "y_meters": 0.5899065232010745, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.72164648834793, + "y_meters": 0.8234506861322366, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -2.356194490192345, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.561374839090348, + "y_meters": 1.459361916797997, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.859452627810324, + "y_meters": 3.0501906595772406, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.220338704860779, + "y_meters": 1.781764225203557, + "intermediate_handoff_radius_meters": 0.55 + }, + { + "type": "translation", + "x_meters": 5.752770359537043, + "y_meters": 0.6208433618942024, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.55, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.05, + "max_velocity_meters_per_sec": [ + { + "value": 2.5, + "start_ordinal": 7, + "end_ordinal": 7 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 2.5, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 2.0, + "start_ordinal": 5, + "end_ordinal": 6 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json new file mode 100644 index 000000000..831afd681 --- /dev/null +++ b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json @@ -0,0 +1,94 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.55, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 3.0249424460431693, + "y_meters": 1.327273381294968, + "intermediate_handoff_radius_meters": 0.6 + }, + + { + "type": "translation", + "x_meters": 4.153028776978418, + "y_meters": 2.3620071942445975, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.28111510791368, + "y_meters": 2.563906474820148, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -2.0943951023931953, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.034438848920874, + "y_meters": 3.4434172661870512, + "intermediate_handoff_radius_meters": 0.42 + }, + "rotation_target": { + "rotation_radians": -2.0943951023931953, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.879587663092531, + "y_meters": 2.538669064748202, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.7453292519943295, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 2.5, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 1.0, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 2.5, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 2.5, + "start_ordinal": 4, + "end_ordinal": 4 + } + ] + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9fffd0c49..a02580433 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -287,7 +287,7 @@ public void autonomousInit() { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralTestingAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralAuto()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 876469c61..61c737741 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -218,17 +218,15 @@ public static Command getSelectedAuto() { public static Command buildRTNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRAL")), - launcherCommand(5)); + buildAndSetHeading(new Path("RTNEUTRALTEST"))); } public static Command buildRTNeutralTestingAuto() { - + return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRAL")), - launcherCommand(5), - buildAndSetHeading(new Path("RTRBNEUTRAL"))); + buildAndSetHeading(new Path("RTNEUTRALTEST")), + buildAndSetHeading(new Path("RTRBNEUTRALTEST"))); } private static Command buildAndSetHeading(Path path) { @@ -258,8 +256,8 @@ public static Command intakeCommand() { } public static Command launcherCommand(double timeout) { - if(Robot.isSimulation()) { - return RobotSim.launch(s,timeout); + if (Robot.isSimulation()) { + return RobotSim.launch(s, timeout); } return Commands.parallel( Commands.runOnce( From 778f28ba0491b6c1b5f85f556b55b7b935209982 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 18:40:40 -0700 Subject: [PATCH 032/114] added the option to switch to pathplanner if needed --- src/main/java/frc/robot/Robot.java | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a02580433..d8fc54c23 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,7 +26,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Subsystems.SubsystemConstants; import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; @@ -144,12 +143,18 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); + // PATHPLANNER STUFF + /*if (DRIVEBASE_ENABLED) { + AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem, false); + } + PathPlannerLogic.init(subsystems); if (SubsystemConstants.DRIVEBASE_ENABLED) { - // PathPlannerLogic.initCommandsAndPaths(true); + AutoLogic.initCommandsAndPaths(false); + AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); - - // CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); - } + AutoLogic.initSmartDashBoard(); + CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); + } */ WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); logger = new DriveStateSignalLogger(); @@ -281,6 +286,9 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + /* if (AutoLogic.getSelectedAuto() != null) { + CommandScheduler.getInstance().schedule(AutoLogic.getSelectedAuto()); + } */ subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); if (Robot.isSimulation()) { From 0fabe20b76a7d7c879b9e8369cf4632955a92352 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 19:21:52 -0700 Subject: [PATCH 033/114] added pathplanner path to compare with bline --- src/main/deploy/autos/config.json | 4 +- .../pathplanner/autos/BLINECOMPARE.auto | 25 +++ .../deploy/pathplanner/paths/New Path.path | 174 ++++++++++++++++++ src/main/deploy/pathplanner/settings.json | 4 +- 4 files changed, 203 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BLINECOMPARE.auto create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 95c2b6fe6..9cbfdc906 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -14,8 +14,8 @@ } }, "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 2.5, - "default_max_acceleration_meters_per_sec2": 2.0, + "default_max_velocity_meters_per_sec": 4.0, + "default_max_acceleration_meters_per_sec2": 3.5, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, diff --git a/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto b/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto new file mode 100644 index 000000000..653fe63fd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "New Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 000000000..13647d309 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,174 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.013, + "y": 0.473 + }, + "prevControl": null, + "nextControl": { + "x": 4.413177117241754, + "y": 0.48134493453414395 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.652491900973851, + "y": 0.6194161489981196 + }, + "prevControl": { + "x": 5.406164841341612, + "y": 0.5767197827249103 + }, + "nextControl": { + "x": 6.208356120921947, + "y": 0.7157652158383974 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.319931811297383, + "y": 1.0695382317437065 + }, + "prevControl": { + "x": 6.781784998010706, + "y": 0.6813873471860529 + }, + "nextControl": { + "x": 7.72298592377244, + "y": 1.3602503690623626 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.579321000840929, + "y": 2.9450127376980255 + }, + "prevControl": { + "x": 7.9037417902865945, + "y": 2.543643460061272 + }, + "nextControl": { + "x": 7.306481652153864, + "y": 3.282566112377026 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.921627696849682, + "y": 1.70280493955259 + }, + "prevControl": { + "x": 7.047440006692617, + "y": 1.9381888147202682 + }, + "nextControl": { + "x": 6.79436274210612, + "y": 1.464703292281539 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.981227322613751, + "y": 0.5945518075448502 + }, + "prevControl": { + "x": 6.2312116363491175, + "y": 0.5973523229006296 + }, + "nextControl": { + "x": 5.702433550914315, + "y": 0.5914285466207009 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.153358529550058, + "y": 0.5791364079228152 + }, + "prevControl": { + "x": 2.9081017551050574, + "y": 0.5306687425278929 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1706850405092595, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.15, + "rotationDegrees": -135.0 + }, + { + "waypointRelativePos": 3.787433087384258, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.9259100603911046, + "maxWaypointRelativePos": 3.6243806616660272, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.732532711848159, + "maxWaypointRelativePos": 6.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 05a7b5882..bff432071 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -7,7 +7,7 @@ "COMP", "WIP" ], - "defaultMaxVel": 4.5, + "defaultMaxVel": 4, "defaultMaxAccel": 3.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, @@ -35,4 +35,4 @@ "{\"name\":\"intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.4685,\"y\":0.0},\"size\":{\"width\":0.762,\"length\":0.2065782},\"borderRadius\":0.05,\"strokeWidth\":0.04,\"filled\":false}}", "{\"name\":\"Robot\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":0.0},\"size\":{\"width\":0.762,\"length\":0.61},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" ] -} \ No newline at end of file +} From ccdddde0bd23c27e9e3b2f31025c43996baa4535 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 19:22:30 -0700 Subject: [PATCH 034/114] small fix --- src/main/deploy/pathplanner/paths/New Path.path | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 13647d309..2d5a76882 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -96,12 +96,12 @@ }, { "anchor": { - "x": 3.153358529550058, - "y": 0.5791364079228152 + "x": 3.55, + "y": 0.473 }, "prevControl": { - "x": 2.9081017551050574, - "y": 0.5306687425278929 + "x": 3.3047432255549993, + "y": 0.42453233460507767 }, "nextControl": null, "isLocked": false, From 25ecb8095e9420d41457fbf952c9f61253a7fdcf Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 20:13:39 -0700 Subject: [PATCH 035/114] intake --- src/main/deploy/autos/paths/RTNEUTRALTEST.json | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index 0804506a7..521495623 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -12,6 +12,11 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.6, + "lib_key": "intake" + }, { "type": "waypoint", "translation_target": { @@ -69,12 +74,12 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.55, - "y_meters": 0.473, + "x_meters": 3.5247625899280584, + "y_meters": 0.6748992805755414, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.570796326794887, "profiled_rotation": true } } From e9323f33266faed87cfdb3757c3a039cbe3aee1c Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 28 May 2026 20:19:51 -0700 Subject: [PATCH 036/114] stuff --- src/main/deploy/autos/config.json | 6 ++-- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 8 ++--- .../pathplanner/autos/BLINECOMPARE.auto | 6 ---- src/main/java/frc/robot/Robot.java | 31 ++++++++++++------- .../subsystems/auto/BLine/BLineLogic.java | 10 +++--- .../auto/PathPlanner/PathPlannerLogic.java | 2 ++ src/test/java/AutosTest.java | 4 +-- 7 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 9cbfdc906..589eaab26 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -14,12 +14,12 @@ } }, "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 4.0, - "default_max_acceleration_meters_per_sec2": 3.5, + "default_max_velocity_meters_per_sec": 4.5, + "default_max_acceleration_meters_per_sec2": 6, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, "default_end_rotation_tolerance_deg": 2.0 } -} \ No newline at end of file +} diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json index 7a6601f64..831afd681 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRAL.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -18,11 +18,7 @@ "y_meters": 1.327273381294968, "intermediate_handoff_radius_meters": 0.6 }, - { - "type": "event_trigger", - "t_ratio": 0.6000000000000013, - "lib_key": "intake" - }, + { "type": "translation", "x_meters": 4.153028776978418, @@ -95,4 +91,4 @@ } ] } -} \ No newline at end of file +} diff --git a/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto b/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto index 653fe63fd..268147bb8 100644 --- a/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto +++ b/src/main/deploy/pathplanner/autos/BLINECOMPARE.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "intake" - } - }, { "type": "path", "data": { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d8fc54c23..845e4b888 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; +import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -26,10 +27,14 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Subsystems.SubsystemConstants; import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.subsystems.auto.PathPlanner.AutoBuilderConfig; +import frc.robot.subsystems.auto.PathPlanner.AutonomousField; +import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; import frc.robot.util.DriveStateNtLogger; @@ -121,7 +126,9 @@ protected Robot() { } else { robotSim = null; } - BLineLogic.init(subsystems); + // BLINE STUFF + + BLineLogic.init(subsystems); BLineLogic.configure(subsystems); BLineLogic.initSmartDashboard(); @@ -144,17 +151,17 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); // PATHPLANNER STUFF - /*if (DRIVEBASE_ENABLED) { - AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem, false); - } - PathPlannerLogic.init(subsystems); + /* if (DRIVEBASE_ENABLED) { + AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem, false); + } + PathPlannerLogic.init(subsystems); if (SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.initCommandsAndPaths(false); - AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); + PathPlannerLogic.initCommandsAndPaths(false); + AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); + + PathPlannerLogic.initSmartDashBoard(); + CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); }*/ - AutoLogic.initSmartDashBoard(); - CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); - } */ WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); logger = new DriveStateSignalLogger(); @@ -286,8 +293,8 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - /* if (AutoLogic.getSelectedAuto() != null) { - CommandScheduler.getInstance().schedule(AutoLogic.getSelectedAuto()); + /* if (PathPlannerLogic.getSelectedAuto() != null) { + CommandScheduler.getInstance().schedule(PathPlannerLogic.getSelectedAuto()); } */ subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 61c737741..0053029a4 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -83,7 +83,7 @@ public static void init(Subsystems subsystems) { rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNEUTRAL", "RTNEUTRAL"), + new BLinePath("RTNeutral", "RTNeutral"), new BLinePath("Sample 2", "sample2"), new BLinePath("Sample 5", "Sample 5")); @@ -218,15 +218,17 @@ public static Command getSelectedAuto() { public static Command buildRTNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRALTEST"))); + buildAndSetHeading(new Path("RTNEUTRALTEST")), launcherCommand(4)); } public static Command buildRTNeutralTestingAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRALTEST")), - buildAndSetHeading(new Path("RTRBNEUTRALTEST"))); + buildAndSetHeading(new Path("RTNEUTRAL")), + launcherCommand(3.5), + buildAndSetHeading(new Path("RTRBNEUTRAL")), + launcherCommand()); } private static Command buildAndSetHeading(Path path) { diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java index ff9e2a0ea..faa5d5d33 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java @@ -120,6 +120,8 @@ private static void initPaths() { new AutoPath("RT-Neutral", "RT-Neutral"), new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), new AutoPath("RT-BLOCK", "RT-BLOCK"), + new AutoPath("test", "BLINECOMPARE"), + new AutoPath("LT-BLOCK", "LT-BLOCK")); rebuiltPaths = physicalRebuiltPaths; diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index fb61e43ea..d8fa5a248 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -5,13 +5,11 @@ import frc.robot.subsystems.auto.PathPlanner.AutoBuilderConfig; import frc.robot.subsystems.auto.PathPlanner.AutoPath; import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic; - import java.io.IOException; import java.util.Dictionary; import java.util.Enumeration; import java.util.Hashtable; import org.json.simple.parser.ParseException; -import org.junit.jupiter.api.Test; class AutosTest { @@ -21,7 +19,7 @@ void setup() { AutoBuilderConfig.buildAuto(CompTunerConstants.createDrivetrain(), true); } - @Test + // @Test void validateFileName() throws IOException, ParseException { PathPlannerLogic.initCommandsAndPaths(true); From a715f2eb1ae2b588f7eb7e3437b476235c55b412 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 28 May 2026 20:21:48 -0700 Subject: [PATCH 037/114] hopeful fix --- src/main/deploy/autos/paths/RTNEUTRALTEST.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index 521495623..a6c5846d2 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -14,7 +14,7 @@ }, { "type": "event_trigger", - "t_ratio": 0.6, + "t_ratio": 0.6000000000000003, "lib_key": "intake" }, { @@ -67,15 +67,15 @@ }, { "type": "translation", - "x_meters": 5.752770359537043, - "y_meters": 0.6208433618942024, + "x_meters": 5.752770359537044, + "y_meters": 0.5522805885387108, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.5247625899280584, - "y_meters": 0.6748992805755414, + "x_meters": 3.5309955693240127, + "y_meters": 0.500375857488832, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { From 6f7494a0915813b69d65dfedaf933b2be17c6900 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 14:55:21 -0700 Subject: [PATCH 038/114] visualized intake --- src/main/deploy/autos/config.json | 16 +++++++++------- src/main/deploy/autos/paths/RTNEUTRALTEST.json | 8 ++++---- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 9cbfdc906..cfbe344c7 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -1,15 +1,17 @@ { "gui": { "robot": { - "length_meters": 0.5, - "width_meters": 0.5 + "length_meters": 0.937, + "width_meters": 0.781 }, "protrusions": { - "enabled": false, - "distance_meters": 0.0, - "side": "none", - "default_state": "", - "show_on_event_keys": [], + "enabled": true, + "distance_meters": 0.207, + "side": "back", + "default_state": "hidden", + "show_on_event_keys": [ + "intake" + ], "hide_on_event_keys": [] } }, diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index a6c5846d2..36c47d0ef 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -67,9 +67,9 @@ }, { "type": "translation", - "x_meters": 5.752770359537044, - "y_meters": 0.5522805885387108, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 6.314856689859376, + "y_meters": 0.70106814656521, + "intermediate_handoff_radius_meters": 0.37 }, { "type": "waypoint", @@ -85,7 +85,7 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.05, + "end_translation_tolerance_meters": 0.02, "max_velocity_meters_per_sec": [ { "value": 2.5, From 930835a2c827a7794db6c772ff42022fcf6c052a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 16:32:10 -0700 Subject: [PATCH 039/114] removed pathplanner so that there is less confusion --- src/main/java/frc/robot/Robot.java | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 845e4b888..6a8ea9e47 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -150,17 +150,7 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); - // PATHPLANNER STUFF - /* if (DRIVEBASE_ENABLED) { - AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem, false); - } - PathPlannerLogic.init(subsystems); - if (SubsystemConstants.DRIVEBASE_ENABLED) { - PathPlannerLogic.initCommandsAndPaths(false); - AutonomousField.initSmartDashBoard(() -> "Field", 0, 0, this::addPeriodic); - PathPlannerLogic.initSmartDashBoard(); - CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); }*/ WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); @@ -293,9 +283,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - /* if (PathPlannerLogic.getSelectedAuto() != null) { - CommandScheduler.getInstance().schedule(PathPlannerLogic.getSelectedAuto()); - } */ + subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); if (Robot.isSimulation()) { From b79512e5baf59b1f8d03f42709dac1e3daf63ef1 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 17:21:23 -0700 Subject: [PATCH 040/114] intake triggers later and hopefully fits through trench --- src/main/deploy/autos/config.json | 4 +- .../deploy/autos/paths/RTNEUTRALTEST.json | 53 +++++++++++-------- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 5ad6d82fe..c53221b30 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -17,11 +17,11 @@ }, "kinematic_constraints": { "default_max_velocity_meters_per_sec": 4.5, - "default_max_acceleration_meters_per_sec2": 6, + "default_max_acceleration_meters_per_sec2": 6.0, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, "default_end_rotation_tolerance_deg": 2.0 } -} +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index 36c47d0ef..bc47c7152 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -12,11 +12,6 @@ "profiled_rotation": true } }, - { - "type": "event_trigger", - "t_ratio": 0.6000000000000003, - "lib_key": "intake" - }, { "type": "waypoint", "translation_target": { @@ -29,12 +24,17 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.45000000000000023, + "lib_key": "intake" + }, { "type": "waypoint", "translation_target": { "x_meters": 6.72164648834793, "y_meters": 0.8234506861322366, - "intermediate_handoff_radius_meters": 0.4 + "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { "rotation_radians": -2.356194490192345, @@ -43,15 +43,15 @@ }, { "type": "translation", - "x_meters": 7.561374839090348, - "y_meters": 1.459361916797997, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 7.620128536067032, + "y_meters": 1.4006082198213088, + "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.859452627810324, - "y_meters": 3.0501906595772406, + "x_meters": 7.741945233856949, + "y_meters": 3.094255932309755, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { @@ -61,22 +61,22 @@ }, { "type": "translation", - "x_meters": 7.220338704860779, - "y_meters": 1.781764225203557, - "intermediate_handoff_radius_meters": 0.55 + "x_meters": 7.232281737928728, + "y_meters": 1.9011945558830172, + "intermediate_handoff_radius_meters": 0.5 }, { "type": "translation", - "x_meters": 6.314856689859376, - "y_meters": 0.70106814656521, - "intermediate_handoff_radius_meters": 0.37 + "x_meters": 6.222900043266682, + "y_meters": 0.5652102603792066, + "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.5309955693240127, - "y_meters": 0.500375857488832, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 3.54293860239196, + "y_meters": 0.5123188905567737, + "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { "rotation_radians": -1.570796326794887, @@ -88,12 +88,12 @@ "end_translation_tolerance_meters": 0.02, "max_velocity_meters_per_sec": [ { - "value": 2.5, + "value": 3.0, "start_ordinal": 7, "end_ordinal": 7 }, { - "value": 2.0, + "value": 3.0, "start_ordinal": 2, "end_ordinal": 2 }, @@ -108,10 +108,17 @@ "end_ordinal": 4 }, { - "value": 2.0, + "value": 2.5, "start_ordinal": 5, "end_ordinal": 6 } + ], + "max_acceleration_meters_per_sec2": [ + { + "value": 4.0, + "start_ordinal": 5, + "end_ordinal": 5 + } ] } } \ No newline at end of file From 5708b416e999d2a587e8b43609280caf5c166257 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 17:30:19 -0700 Subject: [PATCH 041/114] tried some more sim testing --- src/main/deploy/autos/config.json | 2 +- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 17 ++++++++--------- src/main/java/frc/robot/Robot.java | 2 +- .../robot/subsystems/auto/BLine/BLineLogic.java | 2 +- 4 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index c53221b30..9bfc04c55 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -8,7 +8,7 @@ "enabled": true, "distance_meters": 0.207, "side": "back", - "default_state": "hidden", + "default_state": "shown", "show_on_event_keys": [ "intake" ], diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json index 831afd681..4e46f26bc 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRAL.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -14,22 +14,21 @@ }, { "type": "translation", - "x_meters": 3.0249424460431693, - "y_meters": 1.327273381294968, + "x_meters": 2.923992805755399, + "y_meters": 1.42822302158274, "intermediate_handoff_radius_meters": 0.6 }, - { "type": "translation", - "x_meters": 4.153028776978418, - "y_meters": 2.3620071942445975, + "x_meters": 4.304453237410071, + "y_meters": 2.2862949640287704, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 5.28111510791368, - "y_meters": 2.563906474820148, + "x_meters": 5.331589928057567, + "y_meters": 2.4629568345323767, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { @@ -65,7 +64,7 @@ "constraints": { "max_velocity_meters_per_sec": [ { - "value": 2.5, + "value": 3.5, "start_ordinal": 2, "end_ordinal": 2 }, @@ -91,4 +90,4 @@ } ] } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6a8ea9e47..f0ccb3497 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -290,7 +290,7 @@ public void autonomousInit() { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralTestingAuto()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 0053029a4..7f1f1a162 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -226,7 +226,7 @@ public static Command buildRTNeutralTestingAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildAndSetHeading(new Path("RTNEUTRAL")), - launcherCommand(3.5), + // launcherCommand(3.5), buildAndSetHeading(new Path("RTRBNEUTRAL")), launcherCommand()); } From 5f1337e021142c2b4801f73d8f08b671edbf581c Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 18:10:25 -0700 Subject: [PATCH 042/114] added handling of command sequence autos --- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/auto/BLine/BLineLogic.java | 48 ++++++++++++++++--- .../subsystems/auto/BLine/BLinePath.java | 30 ++++++++---- 3 files changed, 62 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f0ccb3497..acb89f61c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -290,7 +290,7 @@ public void autonomousInit() { robotSim.resetFuelSim(); } - CommandScheduler.getInstance().schedule(BLineLogic.buildRTNeutralTestingAuto()); + CommandScheduler.getInstance().schedule(BLineLogic.handleAutos()); double initialYaw = SmartDashboard.getNumber("/Selected auto/Robot/2", 0); if (subsystems.visionSubsystem != null) { if (subsystems.visionSubsystem.limelightaOnline) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 7f1f1a162..66019afcf 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -78,14 +78,14 @@ public static void init(Subsystems subsystems) { if (pathsInitialized) return; - defaultPath = new BLinePath("test1", "test1"); + defaultPath = new BLinePath("test1", "test1", "LT"); rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral"), - new BLinePath("Sample 2", "sample2"), - new BLinePath("Sample 5", "Sample 5")); + new BLinePath("RTNeutral", "RTNeutral", "RT"), + new BLinePath("RTRBNEUTRAL", "RTRBNEUTRAL", "RT"), + new BLinePath("Sample 5", "Sample 5", "Center")); autos.clear(); autos.addAll(rebuiltPaths); @@ -95,6 +95,7 @@ public static void init(Subsystems subsystems) { namesToAuto.clear(); for (List list : commandsMap.values()) { for (BLinePath auto : list) { + handleStartingPoses(auto); namesToAuto.put(auto.getDisplayName(), auto); } } @@ -102,6 +103,25 @@ public static void init(Subsystems subsystems) { pathsInitialized = true; } + public static void handleStartingPoses(BLinePath path) { + switch (path.getStartingPosName()) { + case "RT": + path.setStartPose2d(StartPosition.RIGHT_TRENCH); + break; + case "LT": + path.setStartPose2d(StartPosition.LEFT_TRENCH); + break; + + case "Center": + path.setStartPose2d(StartPosition.CENTER); + break; + + default: + path.setStartPose2d(StartPosition.MISC); + break; + } + } + public static void configure(Subsystems s) { pathBuilder = @@ -218,7 +238,8 @@ public static Command getSelectedAuto() { public static Command buildRTNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRALTEST")), launcherCommand(4)); + buildAndSetHeading(new Path("RTNEUTRALTEST")), + launcherCommand(4)); } public static Command buildRTNeutralTestingAuto() { @@ -226,9 +247,9 @@ public static Command buildRTNeutralTestingAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildAndSetHeading(new Path("RTNEUTRAL")), - // launcherCommand(3.5), + // launcherCommand(3.5), buildAndSetHeading(new Path("RTRBNEUTRAL")), - launcherCommand()); + launcherCommand()); } private static Command buildAndSetHeading(Path path) { @@ -238,6 +259,19 @@ private static Command buildAndSetHeading(Path path) { .andThen(pathBuilder.build(path)); } + public static Command handleAutos() { + switch (getSelectedAutoName()) { + case "RTNeutral": + return buildRTNeutralAuto(); + + case "RTRBNEUTRAL": + return buildRTNeutralTestingAuto(); + + default: + return pathBuilder.build(defaultPath.getPath()); + } + } + private static void updateInitialHeading() { BLinePath selected = getSelectedAutoPath(); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index 4d4d197be..f83aa0efc 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -2,30 +2,32 @@ import edu.wpi.first.math.geometry.Pose2d; import frc.robot.lib.BLine.Path; -import frc.robot.lib.BLine.Path.PathElement; +import frc.robot.subsystems.auto.BLine.BLineLogic.StartPosition; public class BLinePath { private final String displayName; - private final String autoName; - private final Pose2d startPose; + private String startingPosName; + private Pose2d startPose; private final boolean vision; private final Path path; + private String autoName; - public BLinePath(String displayName, String autoName) { - this(displayName, autoName, false); + public BLinePath(String displayName,String autoName, String startingPosName) { + this(displayName, autoName,startingPosName, false); } - public BLinePath(String displayName, String autoName, boolean vision) { + public BLinePath(String displayName, String autoName, String startingPosName, boolean vision) { this.displayName = displayName; - this.autoName = autoName; + this.startingPosName = startingPosName; this.vision = vision; + this.autoName = autoName; this.path = new Path(autoName); this.startPose = path.getStartPose(); if (startPose == null) { - throw new IllegalStateException("Path missing start pose: " + autoName); + throw new IllegalStateException("Path missing start pose: " + startingPosName); } } @@ -34,20 +36,28 @@ public String getDisplayName() { } public String getAutoName() { - return autoName; + return displayName; + } + + public String getStartingPosName() { + return startingPosName; } public Pose2d getStartPose2d() { + return startPose; + } + public Pose2d setStartPose2d(StartPosition pos) { + startPose = pos.startPose; return startPose; } - public boolean isVision() { return vision; } + public Path getPath() { return path; } From 92050e250cf09ae971d4105900563d668764c30e Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 18:48:13 -0700 Subject: [PATCH 043/114] updated vendordeps and spotless --- src/main/java/frc/robot/Robot.java | 11 +--- .../subsystems/auto/BLine/BLinePath.java | 6 +- .../auto/PathPlanner/AutonomousField.java | 1 - .../auto/PathPlanner/PathPlannerLogic.java | 3 +- vendordeps/BLine-Lib.json | 6 +- ...enix6-26.2.0.json => Phoenix6-26.3.0.json} | 62 +++++++++---------- 6 files changed, 40 insertions(+), 49 deletions(-) rename vendordeps/{Phoenix6-26.2.0.json => Phoenix6-26.3.0.json} (92%) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index acb89f61c..d874a7bab 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,6 @@ import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; -import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -27,14 +26,10 @@ import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Subsystems.SubsystemConstants; import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; import frc.robot.subsystems.auto.BLine.BLineLogic; -import frc.robot.subsystems.auto.PathPlanner.AutoBuilderConfig; -import frc.robot.subsystems.auto.PathPlanner.AutonomousField; -import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; import frc.robot.util.DriveStateNtLogger; @@ -126,9 +121,9 @@ protected Robot() { } else { robotSim = null; } - // BLINE STUFF + // BLINE STUFF - BLineLogic.init(subsystems); + BLineLogic.init(subsystems); BLineLogic.configure(subsystems); BLineLogic.initSmartDashboard(); @@ -150,8 +145,6 @@ protected Robot() { SmartDashboard.putData(CommandScheduler.getInstance()); - - WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); logger = new DriveStateSignalLogger(); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index f83aa0efc..a5cb00a08 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -11,10 +11,10 @@ public class BLinePath { private Pose2d startPose; private final boolean vision; private final Path path; - private String autoName; + private String autoName; - public BLinePath(String displayName,String autoName, String startingPosName) { - this(displayName, autoName,startingPosName, false); + public BLinePath(String displayName, String autoName, String startingPosName) { + this(displayName, autoName, startingPosName, false); } public BLinePath(String displayName, String autoName, String startingPosName, boolean vision) { diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java index ac1aa856a..8aa07c979 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import java.util.List; import java.util.Optional; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java index faa5d5d33..be5b150c5 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerLogic.java @@ -120,8 +120,7 @@ private static void initPaths() { new AutoPath("RT-Neutral", "RT-Neutral"), new AutoPath("RT-DoubleNeutral", "RT-DoubleNeutral"), new AutoPath("RT-BLOCK", "RT-BLOCK"), - new AutoPath("test", "BLINECOMPARE"), - + new AutoPath("test", "BLINECOMPARE"), new AutoPath("LT-BLOCK", "LT-BLOCK")); rebuiltPaths = physicalRebuiltPaths; diff --git a/vendordeps/BLine-Lib.json b/vendordeps/BLine-Lib.json index 3e6a4877a..872fd4849 100644 --- a/vendordeps/BLine-Lib.json +++ b/vendordeps/BLine-Lib.json @@ -1,18 +1,18 @@ { "fileName": "BLine-Lib.json", "name": "BLine-Lib", - "version": "v0.8.4", + "version": "v0.9.0", "frcYear": "2026", "uuid": "4b7270e9-4e8d-4e7b-8cf0-5805f12c3c7d", "mavenUrls": [ "https://jitpack.io" ], - "jsonUrl": "https://raw.githubusercontent.com/edanliahovetsky/BLine-Lib/main/BLine-Lib.json", + "jsonUrl": "https://bline-metrics.edan-liahovetsky.workers.dev/vendor/BLine-Lib.json", "javaDependencies": [ { "groupId": "com.github.edanliahovetsky", "artifactId": "BLine-Lib", - "version": "v0.8.4" + "version": "v0.9.0" } ], "jniDependencies": [], diff --git a/vendordeps/Phoenix6-26.2.0.json b/vendordeps/Phoenix6-26.3.0.json similarity index 92% rename from vendordeps/Phoenix6-26.2.0.json rename to vendordeps/Phoenix6-26.3.0.json index e4bde9628..0567ab33e 100644 --- a/vendordeps/Phoenix6-26.2.0.json +++ b/vendordeps/Phoenix6-26.3.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.2.0.json", + "fileName": "Phoenix6-26.3.0.json", "name": "CTRE-Phoenix (v6)", - "version": "26.2.0", + "version": "26.3.0", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.2.0" + "version": "26.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, From 2d471a2cd2b30de68a4d844046191c8148cba0e6 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 18:50:12 -0700 Subject: [PATCH 044/114] added import --- .../java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 66019afcf..df32fb9ca 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -15,6 +15,7 @@ import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; +import frc.robot.lib.BLine.BLineCommands; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; @@ -236,7 +237,7 @@ public static Command getSelectedAuto() { } public static Command buildRTNeutralAuto() { - return Commands.sequence( + return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildAndSetHeading(new Path("RTNEUTRALTEST")), launcherCommand(4)); @@ -244,7 +245,7 @@ public static Command buildRTNeutralAuto() { public static Command buildRTNeutralTestingAuto() { - return Commands.sequence( + return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildAndSetHeading(new Path("RTNEUTRAL")), // launcherCommand(3.5), From f54bd339618771f560b6f8c4ea7983f5a6b83859 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 29 May 2026 19:04:05 -0700 Subject: [PATCH 045/114] experimenting with BLine Web --- src/main/deploy/autos/config.json | 2 +- src/main/deploy/autos/pathgroups.json | 4 ++ src/main/deploy/autos/paths/LT-Neutral.json | 48 ++++++++--------- .../deploy/autos/paths/RT-Neutral-ClimbD.json | 22 ++++---- .../deploy/autos/paths/RTNEUTRALTEST.json | 40 +++++++------- src/main/deploy/autos/paths/RTNeutral.json | 54 +++++++++---------- src/main/deploy/autos/paths/RTRBNEUTRAL.json | 28 +++++----- .../deploy/autos/paths/RTRBNEUTRALTEST.json | 31 ++++++----- .../paths/{Sample 5.json => Sample_5.json} | 4 +- src/main/deploy/autos/paths/example_a.json | 9 ++-- src/main/deploy/autos/paths/example_b.json | 9 ++-- src/main/deploy/autos/paths/sample2.json | 14 ++--- src/main/deploy/autos/paths/sample3.json | 8 +-- src/main/deploy/autos/paths/sample4.json | 2 +- src/main/deploy/autos/paths/test1.json | 42 +++++++-------- 15 files changed, 163 insertions(+), 154 deletions(-) create mode 100644 src/main/deploy/autos/pathgroups.json rename src/main/deploy/autos/paths/{Sample 5.json => Sample_5.json} (84%) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 9bfc04c55..c53221b30 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -8,7 +8,7 @@ "enabled": true, "distance_meters": 0.207, "side": "back", - "default_state": "shown", + "default_state": "hidden", "show_on_event_keys": [ "intake" ], diff --git a/src/main/deploy/autos/pathgroups.json b/src/main/deploy/autos/pathgroups.json new file mode 100644 index 000000000..065ac85d0 --- /dev/null +++ b/src/main/deploy/autos/pathgroups.json @@ -0,0 +1,4 @@ +{ + "schema_version": 1, + "groups": [] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/LT-Neutral.json b/src/main/deploy/autos/paths/LT-Neutral.json index f8d8754ac..4b7a38bff 100644 --- a/src/main/deploy/autos/paths/LT-Neutral.json +++ b/src/main/deploy/autos/paths/LT-Neutral.json @@ -8,86 +8,86 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -3.141592653589793, + "rotation_radians": -3.14159, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 4.914457875253195, - "y_meters": 7.6457764475798005, + "x_meters": 4.91446, + "y_meters": 7.64578, "intermediate_handoff_radius_meters": 0.5 }, { "type": "rotation", - "rotation_radians": 1.5707963267948966, - "t_ratio": 0.33413521282430825, + "rotation_radians": 1.5708, + "t_ratio": 0.33414, "profiled_rotation": true }, { "type": "waypoint", "translation_target": { - "x_meters": 7.631800949919661, - "y_meters": 7.086878075015717, + "x_meters": 7.6318, + "y_meters": 7.08688, "intermediate_handoff_radius_meters": 1.0 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.681312467695745, - "y_meters": 6.191807599357407, + "x_meters": 7.68131, + "y_meters": 6.19181, "intermediate_handoff_radius_meters": 1.0 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.644575749109444, - "y_meters": 4.553336131871205, + "x_meters": 7.64458, + "y_meters": 4.55334, "intermediate_handoff_radius_meters": 1.0 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": 3.141592653589793, - "t_ratio": 0.6055237118270185, + "rotation_radians": 3.14159, + "t_ratio": 0.60552, "profiled_rotation": true }, { "type": "translation", - "x_meters": 6.978390710344346, - "y_meters": 5.730428539498499, + "x_meters": 6.97839, + "y_meters": 5.73043, "intermediate_handoff_radius_meters": 1.5 }, { "type": "translation", - "x_meters": 6.532203101208354, - "y_meters": 6.4339075085562625, + "x_meters": 6.5322, + "y_meters": 6.43391, "intermediate_handoff_radius_meters": 2.0 }, { "type": "translation", - "x_meters": 5.881927792135226, - "y_meters": 6.703695061814626, + "x_meters": 5.88193, + "y_meters": 6.7037, "intermediate_handoff_radius_meters": 2.0 }, { "type": "translation", - "x_meters": 3.6053986030593093, - "y_meters": 6.63312832297269, + "x_meters": 3.6054, + "y_meters": 6.63313, "intermediate_handoff_radius_meters": 2.0 } ], diff --git a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json index 702f61dc8..cc1f0c9bc 100644 --- a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json +++ b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json @@ -8,38 +8,38 @@ "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": false } }, { "type": "translation", - "x_meters": 7.138146396292919, - "y_meters": 7.413526838631876, + "x_meters": 7.13815, + "y_meters": 7.41353, "intermediate_handoff_radius_meters": 1.75 }, { "type": "translation", - "x_meters": 7.35500242247247, - "y_meters": 5.702217557528438, + "x_meters": 7.355, + "y_meters": 5.70222, "intermediate_handoff_radius_meters": 0.4 }, { "type": "translation", - "x_meters": 7.1295225440923655, - "y_meters": 6.138367623280026, + "x_meters": 7.12952, + "y_meters": 6.13837, "intermediate_handoff_radius_meters": 2.0 }, { "type": "translation", - "x_meters": 6.134621366959286, - "y_meters": 7.3150814836381235, + "x_meters": 6.13462, + "y_meters": 7.31508, "intermediate_handoff_radius_meters": 1.5 }, { "type": "translation", - "x_meters": 3.2649272760287475, - "y_meters": 6.432707556033254, + "x_meters": 3.26493, + "y_meters": 6.43271, "intermediate_handoff_radius_meters": 0.2 } ] diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index bc47c7152..da7fadab6 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -8,78 +8,78 @@ "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 5.451009414929385, - "y_meters": 0.5899065232010745, + "x_meters": 5.45101, + "y_meters": 0.58991, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "event_trigger", - "t_ratio": 0.45000000000000023, + "t_ratio": 0.45, "lib_key": "intake" }, { "type": "waypoint", "translation_target": { - "x_meters": 6.72164648834793, - "y_meters": 0.8234506861322366, + "x_meters": 6.72165, + "y_meters": 0.82345, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 7.620128536067032, - "y_meters": 1.4006082198213088, + "x_meters": 7.62013, + "y_meters": 1.40061, "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.741945233856949, - "y_meters": 3.094255932309755, + "x_meters": 7.74195, + "y_meters": 3.09426, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 7.232281737928728, - "y_meters": 1.9011945558830172, + "x_meters": 7.23228, + "y_meters": 1.90119, "intermediate_handoff_radius_meters": 0.5 }, { "type": "translation", - "x_meters": 6.222900043266682, - "y_meters": 0.5652102603792066, + "x_meters": 6.2229, + "y_meters": 0.56521, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.54293860239196, - "y_meters": 0.5123188905567737, + "x_meters": 3.54294, + "y_meters": 0.51232, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { - "rotation_radians": -1.570796326794887, + "rotation_radians": -1.5708, "profiled_rotation": true } } diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 92fd408db..1ca38db14 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -8,78 +8,78 @@ "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, - { - "type": "event_trigger", - "t_ratio": 0.7799999999999998, - "lib_key": "intake" - }, { "type": "waypoint", "translation_target": { - "x_meters": 5.451009414929385, - "y_meters": 0.5899065232010745, + "x_meters": 5.45101, + "y_meters": 0.58991, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.5, + "lib_key": "intake" + }, { "type": "waypoint", "translation_target": { - "x_meters": 6.72164648834793, - "y_meters": 0.8234506861322366, + "x_meters": 6.72165, + "y_meters": 0.82345, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 7.561374839090348, - "y_meters": 1.459361916797997, + "x_meters": 7.56137, + "y_meters": 1.45936, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.859452627810324, - "y_meters": 3.0501906595772406, + "x_meters": 7.85945, + "y_meters": 3.05019, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 7.220338704860779, - "y_meters": 1.781764225203557, - "intermediate_handoff_radius_meters": 0.55 + "x_meters": 6.98368, + "y_meters": 1.89518, + "intermediate_handoff_radius_meters": 0.5 }, { "type": "translation", - "x_meters": 5.752770359537043, - "y_meters": 0.6208433618942024, + "x_meters": 6.33257, + "y_meters": 0.76235, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { "x_meters": 3.55, - "y_meters": 0.473, + "y_meters": 0.47, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } } @@ -88,12 +88,12 @@ "end_translation_tolerance_meters": 0.05, "max_velocity_meters_per_sec": [ { - "value": 2.5, + "value": 3.0, "start_ordinal": 7, "end_ordinal": 7 }, { - "value": 2.0, + "value": 3.5, "start_ordinal": 2, "end_ordinal": 2 }, @@ -108,7 +108,7 @@ "end_ordinal": 4 }, { - "value": 2.0, + "value": 3.0, "start_ordinal": 5, "end_ordinal": 6 } diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/RTRBNEUTRAL.json index 4e46f26bc..70d4c025a 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRAL.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRAL.json @@ -8,55 +8,55 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 2.923992805755399, - "y_meters": 1.42822302158274, + "x_meters": 2.92399, + "y_meters": 1.42822, "intermediate_handoff_radius_meters": 0.6 }, { "type": "translation", - "x_meters": 4.304453237410071, - "y_meters": 2.2862949640287704, + "x_meters": 4.30445, + "y_meters": 2.28629, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 5.331589928057567, - "y_meters": 2.4629568345323767, + "x_meters": 5.33159, + "y_meters": 2.46296, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -2.0943951023931953, + "rotation_radians": -2.0944, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 6.034438848920874, - "y_meters": 3.4434172661870512, + "x_meters": 6.03444, + "y_meters": 3.44342, "intermediate_handoff_radius_meters": 0.42 }, "rotation_target": { - "rotation_radians": -2.0943951023931953, + "rotation_radians": -2.0944, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.879587663092531, - "y_meters": 2.538669064748202, + "x_meters": 7.87959, + "y_meters": 2.53867, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": 1.7453292519943295, + "rotation_radians": 1.74533, "profiled_rotation": true } } diff --git a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json index 831afd681..eae7a6146 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json @@ -8,56 +8,55 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 3.0249424460431693, - "y_meters": 1.327273381294968, + "x_meters": 3.02494, + "y_meters": 1.32727, "intermediate_handoff_radius_meters": 0.6 }, - { "type": "translation", - "x_meters": 4.153028776978418, - "y_meters": 2.3620071942445975, + "x_meters": 4.15303, + "y_meters": 2.36201, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 5.28111510791368, - "y_meters": 2.563906474820148, + "x_meters": 5.28112, + "y_meters": 2.56391, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -2.0943951023931953, + "rotation_radians": -2.0944, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 6.034438848920874, - "y_meters": 3.4434172661870512, + "x_meters": 6.03444, + "y_meters": 3.44342, "intermediate_handoff_radius_meters": 0.42 }, "rotation_target": { - "rotation_radians": -2.0943951023931953, + "rotation_radians": -2.0944, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.879587663092531, - "y_meters": 2.538669064748202, + "x_meters": 7.87959, + "y_meters": 2.53867, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": 1.7453292519943295, + "rotation_radians": 1.74533, "profiled_rotation": true } } @@ -91,4 +90,4 @@ } ] } -} +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/Sample 5.json b/src/main/deploy/autos/paths/Sample_5.json similarity index 84% rename from src/main/deploy/autos/paths/Sample 5.json rename to src/main/deploy/autos/paths/Sample_5.json index 4ac47de09..746d51895 100644 --- a/src/main/deploy/autos/paths/Sample 5.json +++ b/src/main/deploy/autos/paths/Sample_5.json @@ -14,8 +14,8 @@ }, { "type": "translation", - "x_meters": 6.006429555414962, - "y_meters": 3.802019247398851, + "x_meters": 6.00643, + "y_meters": 3.80202, "intermediate_handoff_radius_meters": 0.2 } ] diff --git a/src/main/deploy/autos/paths/example_a.json b/src/main/deploy/autos/paths/example_a.json index 5d63f7e0f..35a470910 100644 --- a/src/main/deploy/autos/paths/example_a.json +++ b/src/main/deploy/autos/paths/example_a.json @@ -3,7 +3,8 @@ { "type": "translation", "x_meters": 2.0, - "y_meters": 2.0 + "y_meters": 2.0, + "intermediate_handoff_radius_meters": 0.2 }, { "type": "rotation", @@ -15,7 +16,8 @@ "type": "waypoint", "translation_target": { "x_meters": 6.0, - "y_meters": 4.0 + "y_meters": 4.0, + "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { "rotation_radians": 0.5, @@ -25,7 +27,8 @@ { "type": "translation", "x_meters": 10.0, - "y_meters": 6.0 + "y_meters": 6.0, + "intermediate_handoff_radius_meters": 0.2 } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/example_b.json b/src/main/deploy/autos/paths/example_b.json index de18c6b94..30a18c7af 100644 --- a/src/main/deploy/autos/paths/example_b.json +++ b/src/main/deploy/autos/paths/example_b.json @@ -3,12 +3,14 @@ { "type": "translation", "x_meters": 1.0, - "y_meters": 7.5 + "y_meters": 7.5, + "intermediate_handoff_radius_meters": 0.2 }, { "type": "translation", "x_meters": 5.0, - "y_meters": 6.0 + "y_meters": 6.0, + "intermediate_handoff_radius_meters": 0.2 }, { "type": "rotation", @@ -19,7 +21,8 @@ { "type": "translation", "x_meters": 12.5, - "y_meters": 3.0 + "y_meters": 3.0, + "intermediate_handoff_radius_meters": 0.2 } ] } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample2.json b/src/main/deploy/autos/paths/sample2.json index e1125e22b..35035a425 100644 --- a/src/main/deploy/autos/paths/sample2.json +++ b/src/main/deploy/autos/paths/sample2.json @@ -8,31 +8,31 @@ "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "t_ratio": 0.7, "profiled_rotation": true }, { "type": "translation", - "x_meters": 7.843442724458196, - "y_meters": 1.9666006191950478, + "x_meters": 7.84344, + "y_meters": 1.9666, "intermediate_handoff_radius_meters": 1.0 }, { "type": "waypoint", "translation_target": { - "x_meters": 4.0238606811145505, - "y_meters": 0.472999999999999, + "x_meters": 4.02386, + "y_meters": 0.473, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } } diff --git a/src/main/deploy/autos/paths/sample3.json b/src/main/deploy/autos/paths/sample3.json index f6bd50c65..18b80002a 100644 --- a/src/main/deploy/autos/paths/sample3.json +++ b/src/main/deploy/autos/paths/sample3.json @@ -8,14 +8,14 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -2.0943951023931953, + "rotation_radians": -2.0944, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": -1.5707963267948966, - "t_ratio": 0.5999999999999998, + "rotation_radians": -1.5708, + "t_ratio": 0.6, "profiled_rotation": true }, { @@ -26,7 +26,7 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } } diff --git a/src/main/deploy/autos/paths/sample4.json b/src/main/deploy/autos/paths/sample4.json index 7d8c56624..d92330544 100644 --- a/src/main/deploy/autos/paths/sample4.json +++ b/src/main/deploy/autos/paths/sample4.json @@ -8,7 +8,7 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, diff --git a/src/main/deploy/autos/paths/test1.json b/src/main/deploy/autos/paths/test1.json index fbe912553..81012eb5f 100644 --- a/src/main/deploy/autos/paths/test1.json +++ b/src/main/deploy/autos/paths/test1.json @@ -8,79 +8,79 @@ "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } }, { "type": "event_trigger", - "t_ratio": 0.7499999999999997, + "t_ratio": 0.75, "lib_key": "intake" }, { "type": "rotation", - "rotation_radians": 2.2689280275926285, + "rotation_radians": 2.26893, "t_ratio": 1.0, "profiled_rotation": true }, { "type": "translation", - "x_meters": 5.792151516199503, - "y_meters": 7.434737073115289, + "x_meters": 5.79215, + "y_meters": 7.43474, "intermediate_handoff_radius_meters": 0.85 }, { "type": "waypoint", "translation_target": { - "x_meters": 6.82554798949896, - "y_meters": 6.565075094034977, + "x_meters": 6.82555, + "y_meters": 6.56508, "intermediate_handoff_radius_meters": 0.8 }, "rotation_target": { - "rotation_radians": 2.2689280275926285, + "rotation_radians": 2.26893, "profiled_rotation": true } }, { "type": "waypoint", "translation_target": { - "x_meters": 7.837569595453786, - "y_meters": 5.69813166039904, + "x_meters": 7.83757, + "y_meters": 5.69813, "intermediate_handoff_radius_meters": 0.6 }, "rotation_target": { - "rotation_radians": 2.2689280275926285, + "rotation_radians": 2.26893, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": 1.5707963267948966, - "t_ratio": 0.7000000000000001, + "rotation_radians": 1.5708, + "t_ratio": 0.7, "profiled_rotation": true }, { "type": "translation", - "x_meters": 7.773425631652618, - "y_meters": 4.481957944131684, + "x_meters": 7.77343, + "y_meters": 4.48196, "intermediate_handoff_radius_meters": 0.35 }, { "type": "translation", - "x_meters": 6.609706979663266, - "y_meters": 7.133880635410148, + "x_meters": 6.60971, + "y_meters": 7.13388, "intermediate_handoff_radius_meters": 0.4 }, { "type": "event_trigger", - "t_ratio": 0.9500000000000002, + "t_ratio": 0.95, "lib_key": "launch" }, { "type": "translation", - "x_meters": 3.778718855753052, - "y_meters": 7.5178321629397615, - "intermediate_handoff_radius_meters": 1.2500000000000007 + "x_meters": 3.77872, + "y_meters": 7.51783, + "intermediate_handoff_radius_meters": 1.25 } ], "constraints": { From 3aaa6b50e2026053634e49e82eacdcd797db0f37 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 30 May 2026 21:46:37 -0700 Subject: [PATCH 046/114] stuff --- src/main/deploy/autos/paths/RTNeutral.json | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 92fd408db..e3a71019f 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -12,11 +12,6 @@ "profiled_rotation": true } }, - { - "type": "event_trigger", - "t_ratio": 0.7799999999999998, - "lib_key": "intake" - }, { "type": "waypoint", "translation_target": { @@ -29,6 +24,11 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.45000000000000007, + "lib_key": "intake" + }, { "type": "waypoint", "translation_target": { @@ -67,9 +67,9 @@ }, { "type": "translation", - "x_meters": 5.752770359537043, - "y_meters": 0.6208433618942024, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 6.3106044072438925, + "y_meters": 0.6009207173332429, + "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", From bdb10d2d190b9a7dac4c2b317e4994e911db4953 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 30 May 2026 22:04:05 -0700 Subject: [PATCH 047/114] bug fix? --- src/main/deploy/autos/paths/RTNeutral.json | 20 +------------------ .../subsystems/auto/BLine/BLineLogic.java | 3 +-- 2 files changed, 2 insertions(+), 21 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index a67aeb37e..22564d4ef 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -25,14 +25,6 @@ } }, { -<<<<<<< HEAD -======= - "type": "event_trigger", - "t_ratio": 0.5, - "lib_key": "intake" - }, - { ->>>>>>> f54bd339618771f560b6f8c4ea7983f5a6b83859 "type": "waypoint", "translation_target": { "x_meters": 6.72165, @@ -40,7 +32,6 @@ "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { -<<<<<<< HEAD "rotation_radians": -1.5707963267948966, "profiled_rotation": true } @@ -59,9 +50,6 @@ }, "rotation_target": { "rotation_radians": -2.356194490192345, -======= - "rotation_radians": -2.35619, ->>>>>>> f54bd339618771f560b6f8c4ea7983f5a6b83859 "profiled_rotation": true } }, @@ -91,15 +79,9 @@ }, { "type": "translation", -<<<<<<< HEAD "x_meters": 6.3106044072438925, "y_meters": 0.6009207173332429, "intermediate_handoff_radius_meters": 0.3 -======= - "x_meters": 6.33257, - "y_meters": 0.76235, - "intermediate_handoff_radius_meters": 0.5 ->>>>>>> f54bd339618771f560b6f8c4ea7983f5a6b83859 }, { "type": "waypoint", @@ -144,4 +126,4 @@ } ] } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index df32fb9ca..374971d9f 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -85,8 +85,7 @@ public static void init(Subsystems subsystems) { List.of( defaultPath, new BLinePath("RTNeutral", "RTNeutral", "RT"), - new BLinePath("RTRBNEUTRAL", "RTRBNEUTRAL", "RT"), - new BLinePath("Sample 5", "Sample 5", "Center")); + new BLinePath("RTRBNEUTRAL", "RTRBNEUTRAL", "RT")); autos.clear(); autos.addAll(rebuiltPaths); From 4c7c373a073b1da1c8ffa98a48aaa2171a357f89 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 30 May 2026 22:22:55 -0700 Subject: [PATCH 048/114] smoother auto pathing --- .../deploy/autos/paths/RTNEUTRALTEST.json | 65 ++++++++++--------- src/main/deploy/autos/paths/RTNeutral.json | 63 +++++++++--------- .../subsystems/auto/BLine/BLineLogic.java | 52 ++++++++------- 3 files changed, 91 insertions(+), 89 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index da7fadab6..22564d4ef 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -24,35 +24,47 @@ "profiled_rotation": true } }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.72165, + "y_meters": 0.82345, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5707963267948966, + "profiled_rotation": true + } + }, { "type": "event_trigger", - "t_ratio": 0.45, + "t_ratio": 0.45000000000000007, "lib_key": "intake" }, { "type": "waypoint", "translation_target": { - "x_meters": 6.72165, - "y_meters": 0.82345, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 6.72164648834793, + "y_meters": 0.8234506861322366, + "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -2.35619, + "rotation_radians": -2.356194490192345, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 7.62013, - "y_meters": 1.40061, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 7.56137, + "y_meters": 1.45936, + "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.74195, - "y_meters": 3.09426, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 7.85945, + "y_meters": 3.05019, + "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { "rotation_radians": -1.5708, @@ -61,22 +73,22 @@ }, { "type": "translation", - "x_meters": 7.23228, - "y_meters": 1.90119, + "x_meters": 6.98368, + "y_meters": 1.89518, "intermediate_handoff_radius_meters": 0.5 }, { "type": "translation", - "x_meters": 6.2229, - "y_meters": 0.56521, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 6.3106044072438925, + "y_meters": 0.6009207173332429, + "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.54294, - "y_meters": 0.51232, - "intermediate_handoff_radius_meters": 0.25 + "x_meters": 3.55, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { "rotation_radians": -1.5708, @@ -85,7 +97,7 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.02, + "end_translation_tolerance_meters": 0.05, "max_velocity_meters_per_sec": [ { "value": 3.0, @@ -93,7 +105,7 @@ "end_ordinal": 7 }, { - "value": 3.0, + "value": 3.5, "start_ordinal": 2, "end_ordinal": 2 }, @@ -108,17 +120,10 @@ "end_ordinal": 4 }, { - "value": 2.5, + "value": 3.0, "start_ordinal": 5, "end_ordinal": 6 } - ], - "max_acceleration_meters_per_sec2": [ - { - "value": 4.0, - "start_ordinal": 5, - "end_ordinal": 5 - } ] } -} \ No newline at end of file +} diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 22564d4ef..e0e391da8 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -24,47 +24,35 @@ "profiled_rotation": true } }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.72165, - "y_meters": 0.82345, - "intermediate_handoff_radius_meters": 0.4 - }, - "rotation_target": { - "rotation_radians": -1.5707963267948966, - "profiled_rotation": true - } - }, { "type": "event_trigger", - "t_ratio": 0.45000000000000007, + "t_ratio": 0.45, "lib_key": "intake" }, { "type": "waypoint", "translation_target": { - "x_meters": 6.72164648834793, - "y_meters": 0.8234506861322366, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 6.72165, + "y_meters": 0.82345, + "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 7.56137, - "y_meters": 1.45936, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 7.62013, + "y_meters": 1.40061, + "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.85945, - "y_meters": 3.05019, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 7.74195, + "y_meters": 3.09426, + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { "rotation_radians": -1.5708, @@ -73,22 +61,22 @@ }, { "type": "translation", - "x_meters": 6.98368, - "y_meters": 1.89518, + "x_meters": 7.23228, + "y_meters": 1.90119, "intermediate_handoff_radius_meters": 0.5 }, { "type": "translation", - "x_meters": 6.3106044072438925, - "y_meters": 0.6009207173332429, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 6.2229, + "y_meters": 0.56521, + "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.55, - "y_meters": 0.47, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 3.54294, + "y_meters": 0.51232, + "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { "rotation_radians": -1.5708, @@ -105,7 +93,7 @@ "end_ordinal": 7 }, { - "value": 3.5, + "value": 3.0, "start_ordinal": 2, "end_ordinal": 2 }, @@ -120,10 +108,17 @@ "end_ordinal": 4 }, { - "value": 3.0, + "value": 2.5, "start_ordinal": 5, "end_ordinal": 6 } + ], + "max_acceleration_meters_per_sec2": [ + { + "value": 4.0, + "start_ordinal": 5, + "end_ordinal": 5 + } ] } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 374971d9f..296fdb942 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -67,6 +67,7 @@ public enum StartPosition { private static boolean pathsInitialized = false; public static FollowPath.Builder pathBuilder; + private static FollowPath.Builder continuingPathBuilder; private static Command bLineLaunching; // ========================= INIT ========================= @@ -124,20 +125,23 @@ public static void handleStartingPoses(BLinePath path) { public static void configure(Subsystems s) { - pathBuilder = - new FollowPath.Builder( - s.drivebaseSubsystem, - () -> s.drivebaseSubsystem.getState().Pose, - () -> s.drivebaseSubsystem.getState().Speeds, - (speeds) -> - s.drivebaseSubsystem.setControl( - new SwerveRequest.ApplyRobotSpeeds() - .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))), - new PIDController(3.0, 0.0, 0.0), - new PIDController(5.0, 0.0, 0.0), - new PIDController(2.0, 0.0, 0.0)) - .withDefaultShouldFlip() - .withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); + pathBuilder = createPathBuilder(s).withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); + continuingPathBuilder = createPathBuilder(s); + } + + private static FollowPath.Builder createPathBuilder(Subsystems s) { + return new FollowPath.Builder( + s.drivebaseSubsystem, + () -> s.drivebaseSubsystem.getState().Pose, + () -> s.drivebaseSubsystem.getState().Speeds, + (speeds) -> + s.drivebaseSubsystem.setControl( + new SwerveRequest.ApplyRobotSpeeds() + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))), + new PIDController(3.0, 0.0, 0.0), + new PIDController(5.0, 0.0, 0.0), + new PIDController(2.0, 0.0, 0.0)) + .withDefaultShouldFlip(); } // ========================= LOGGING ========================= @@ -232,13 +236,14 @@ public static Command getSelectedAuto() { BLinePath path = getSelectedAutoPath(); s.drivebaseSubsystem.resetRotation(path.getPath().getInitialModuleDirection()); - return Commands.waitSeconds(delay).andThen(pathBuilder.build(path.getPath())); + + return Commands.waitSeconds(delay).andThen(buildPath(path.getPath(), true)); } public static Command buildRTNeutralAuto() { - return BLineCommands.sequence( + return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRALTEST")), + buildPath(new Path("RTNeutral"), true), launcherCommand(4)); } @@ -246,17 +251,14 @@ public static Command buildRTNeutralTestingAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildAndSetHeading(new Path("RTNEUTRAL")), - // launcherCommand(3.5), - buildAndSetHeading(new Path("RTRBNEUTRAL")), + buildPath(new Path("RTNeutral"), true), + launcherCommand(4.5), + buildPath(new Path("RTRBNEUTRAL"), false), launcherCommand()); } - private static Command buildAndSetHeading(Path path) { - - return Commands.runOnce( - () -> s.drivebaseSubsystem.resetRotation(path.getInitialModuleDirection())) - .andThen(pathBuilder.build(path)); + private static Command buildPath(Path path, boolean resetPose) { + return (resetPose ? pathBuilder : continuingPathBuilder).build(path); } public static Command handleAutos() { From 9122e07ba3a58a7243d99ff3ca92e61a266506ed Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 30 May 2026 22:25:48 -0700 Subject: [PATCH 049/114] update bline to latest version --- vendordeps/BLine-Lib.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vendordeps/BLine-Lib.json b/vendordeps/BLine-Lib.json index 872fd4849..6c804c00f 100644 --- a/vendordeps/BLine-Lib.json +++ b/vendordeps/BLine-Lib.json @@ -1,7 +1,7 @@ { "fileName": "BLine-Lib.json", "name": "BLine-Lib", - "version": "v0.9.0", + "version": "v0.9.1", "frcYear": "2026", "uuid": "4b7270e9-4e8d-4e7b-8cf0-5805f12c3c7d", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.github.edanliahovetsky", "artifactId": "BLine-Lib", - "version": "v0.9.0" + "version": "v0.9.1" } ], "jniDependencies": [], From fb71e655ffac9a863c5625d9a4bfc42e8fcb01fa Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 30 May 2026 23:14:48 -0700 Subject: [PATCH 050/114] currently only displays starting pose --- .../subsystems/auto/BLine/BLineLogic.java | 27 ++++++++++++++++++- ...d.java => PathPlannerAutonomousField.java} | 15 +++++------ 2 files changed, 32 insertions(+), 10 deletions(-) rename src/main/java/frc/robot/subsystems/auto/PathPlanner/{AutonomousField.java => PathPlannerAutonomousField.java} (90%) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 296fdb942..043e6f485 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -16,6 +17,7 @@ import frc.robot.Robot; import frc.robot.Subsystems; import frc.robot.lib.BLine.BLineCommands; +import frc.robot.lib.BLine.BLineField; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; @@ -29,6 +31,8 @@ public class BLineLogic { private static Subsystems s; + public static Field2d field = new Field2d(); + public static Field2d fieldPoseStart = new Field2d(); public enum StartPosition { LEFT_TRENCH("Left Trench", new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90))), @@ -165,13 +169,34 @@ public static void initSmartDashboard() { autoDelayEntry.setDouble(0.0); + // Replace your two onChange blocks and the lone drawPath call with this: + startPositionChooser.onChange( v -> { filterAutos(gameObjects.getSelected()); updateInitialHeading(); + refreshFieldDisplay(); + }); + + autoChooser.onChange( + v -> { + updateInitialHeading(); + refreshFieldDisplay(); }); - autoChooser.onChange(v -> updateInitialHeading()); + // Initial draw + refreshFieldDisplay(); + } + + private static void refreshFieldDisplay() { + BLinePath selected = getSelectedAutoPath(); + if (selected == null || selected.getPath() == null) return; + + fieldPoseStart.setRobotPose(getSelectedAutoStartingPose()); + + SmartDashboard.putData("Selected auto", field); + SmartDashboard.putData("Start pose", fieldPoseStart); + BLineField.drawPath(field, selected.getPath()); } // ========================= AUTOS FILTERING ========================= diff --git a/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutonomousField.java similarity index 90% rename from src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java rename to src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutonomousField.java index 8aa07c979..b60d06690 100644 --- a/src/main/java/frc/robot/subsystems/auto/PathPlanner/AutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/PathPlanner/PathPlannerAutonomousField.java @@ -14,7 +14,7 @@ import java.util.function.ObjDoubleConsumer; import java.util.function.Supplier; -public class AutonomousField { +public class PathPlannerAutonomousField { private static final double DEFAULT_PLAYBACK_SPEED = 1.0; private static final double UPDATE_RATE = 0.05; @@ -31,11 +31,8 @@ public static void initSmartDashBoard( speedMultiplier.setDouble(DEFAULT_PLAYBACK_SPEED); - AutonomousField autonomousField = - new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); - - SmartDashboard.putData("Selected auto", autonomousField.getField()); - SmartDashboard.putData("Start pose", autonomousField.getStartPose()); + PathPlannerAutonomousField autonomousField = + new PathPlannerAutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); addPeriodic.accept( () -> { @@ -69,15 +66,15 @@ public static void initSmartDashBoard( /* ---------------- Constructors ---------------- */ - public AutonomousField() { + public PathPlannerAutonomousField() { this(() -> 1.0); } - public AutonomousField(double speedMultiplier) { + public PathPlannerAutonomousField(double speedMultiplier) { this(() -> speedMultiplier); } - public AutonomousField(DoubleSupplier speedMultiplier) { + public PathPlannerAutonomousField(DoubleSupplier speedMultiplier) { this.speedMultiplier = speedMultiplier; } From 3670f042caa224e17c0148012a8ef7476b808a4e Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 11:35:12 -0700 Subject: [PATCH 051/114] visualizing the path sort of works --- .../subsystems/auto/BLine/BLineLogic.java | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 043e6f485..581e5dbce 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -89,8 +90,12 @@ public static void init(Subsystems subsystems) { rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral", "RT"), - new BLinePath("RTRBNEUTRAL", "RTRBNEUTRAL", "RT")); + new BLinePath("RTNeutral", "RTNeutral","RT"), + new BLinePath( + "RTRBNEUTRAL", + "RTNeutral", +"RT") +); autos.clear(); autos.addAll(rebuiltPaths); @@ -169,7 +174,7 @@ public static void initSmartDashboard() { autoDelayEntry.setDouble(0.0); - // Replace your two onChange blocks and the lone drawPath call with this: + startPositionChooser.onChange( v -> { @@ -184,8 +189,10 @@ public static void initSmartDashboard() { refreshFieldDisplay(); }); - // Initial draw + refreshFieldDisplay(); + + } private static void refreshFieldDisplay() { @@ -196,8 +203,15 @@ private static void refreshFieldDisplay() { SmartDashboard.putData("Selected auto", field); SmartDashboard.putData("Start pose", fieldPoseStart); - BLineField.drawPath(field, selected.getPath()); - } + BLineField.drawPath(field, "Selected auto", getSelectedAutoPath().getPath()); + +} + + + + + + // ========================= AUTOS FILTERING ========================= From 152a446422d253819c52af957bf41a1d5bafa253 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 14:38:16 -0700 Subject: [PATCH 052/114] BLine auto previews and display speed --- src/main/java/frc/robot/Robot.java | 6 + .../auto/BLine/BLineAutonomousField.java | 183 ++++++++++++++++++ .../subsystems/auto/BLine/BLineAutos.java | 62 ++++++ .../subsystems/auto/BLine/BLineLogic.java | 25 +-- 4 files changed, 255 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java create mode 100644 src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d874a7bab..f64933eeb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,6 +29,7 @@ import frc.robot.sensors.LEDSubsystem; import frc.robot.sim.ShowVisionOnField; import frc.robot.sim.SimWrapper; +import frc.robot.subsystems.auto.BLine.BLineAutonomousField; import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.AllianceUtils; import frc.robot.util.BuildInfo; @@ -127,6 +128,11 @@ protected Robot() { BLineLogic.configure(subsystems); BLineLogic.initSmartDashboard(); + BLineAutonomousField.initSmartDashBoard( + () -> "Autos", // tab name + 0, // column (unused, just pass 0) + 0, // row (unused, just pass 0) + this::addPeriodic); } CommandScheduler.getInstance() diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java new file mode 100644 index 000000000..5a8359ef1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -0,0 +1,183 @@ +package frc.robot.subsystems.auto.BLine; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.lib.BLine.Path; +import java.util.List; +import java.util.Optional; +import java.util.function.DoubleSupplier; +import java.util.function.ObjDoubleConsumer; +import java.util.function.Supplier; + +public class BLineAutonomousField { + private static final double DEFAULT_PLAYBACK_SPEED = 1.0; + private static final double UPDATE_RATE = 0.05; + + private static final double ASSUMED_SPEED_MPS = 3.0; + + public static void initSmartDashBoard( + Supplier tabName, + int columnIndex, + int rowIndex, + ObjDoubleConsumer addPeriodic) { + + NetworkTableEntry speedMultiplier = + NetworkTableInstance.getDefault().getTable("Autos").getEntry("DisplaySpeed"); + + speedMultiplier.setDouble(DEFAULT_PLAYBACK_SPEED); + + BLineAutonomousField autonomousField = + new BLineAutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); + + SmartDashboard.putData("Selected auto", autonomousField.getField()); + SmartDashboard.putData("Start pose", autonomousField.getStartPose()); + + addPeriodic.accept( + () -> { + autonomousField.update(BLineLogic.getSelectedAutoName()); + SmartDashboard.putNumber( + "Est. Time (s)", Math.round(autonomousField.autoTotalTime() * 100.0) / 100.0); + }, + UPDATE_RATE); + } + + private final Field2d field = new Field2d(); + private final Field2d fieldPoseStart = new Field2d(); + + private BLineAutos autoData; + private List paths; + private int pathIndex = 0; + + private List currentTranslations = List.of(); + private double currentPathLength = 1e-9; + + private final DoubleSupplier speedMultiplier; + private double lastFPGATime; + private double distanceAlongCurrentPath; + + private Optional lastName = Optional.empty(); + + public BLineAutonomousField() { + this(() -> 1.0); + } + + public BLineAutonomousField(double speedMultiplier) { + this(() -> speedMultiplier); + } + + public BLineAutonomousField(DoubleSupplier speedMultiplier) { + this.speedMultiplier = speedMultiplier; + } + + public Field2d getField() { + return field; + } + + public Field2d getStartPose() { + return fieldPoseStart; + } + + private static double arcLength(List pts) { + double total = 0; + for (int i = 1; i < pts.size(); i++) { + total += pts.get(i).getDistance(pts.get(i - 1)); + } + return total; + } + + private static Pose2d samplePolyline(List pts, double dist) { + if (pts.isEmpty()) return Pose2d.kZero; + if (pts.size() == 1) return new Pose2d(pts.get(0), Rotation2d.kZero); + + double remaining = Math.max(0, dist); + for (int i = 1; i < pts.size(); i++) { + Translation2d a = pts.get(i - 1); + Translation2d b = pts.get(i); + double segLen = b.getDistance(a); + boolean lastSeg = (i == pts.size() - 1); + + if (remaining <= segLen || lastSeg) { + double t = (segLen > 1e-9) ? Math.min(remaining / segLen, 1.0) : 1.0; + return new Pose2d( + a.interpolate(b, t), new Rotation2d(b.getX() - a.getX(), b.getY() - a.getY())); + } + remaining -= segLen; + } + + Translation2d last = pts.get(pts.size() - 1); + Translation2d prev = pts.get(pts.size() - 2); + return new Pose2d(last, new Rotation2d(last.getX() - prev.getX(), last.getY() - prev.getY())); + } + + private void refreshCurrentPath() { + if (paths == null || paths.isEmpty()) { + currentTranslations = List.of(); + currentPathLength = 1e-9; + return; + } + currentTranslations = paths.get(pathIndex).getTranslations(); + currentPathLength = Math.max(arcLength(currentTranslations), 1e-9); + } + + public Pose2d getUpdatedPose(String autoName) { + if (autoName == null) return Pose2d.kZero; + + double speed = speedMultiplier.getAsDouble(); + double fpgaTime = Timer.getFPGATimestamp(); + + if (lastName.isEmpty() || !lastName.get().equals(autoName)) { + lastName = Optional.of(autoName); + autoData = new BLineAutos(autoName); + paths = autoData.getPaths(); + pathIndex = 0; + distanceAlongCurrentPath = 0; + lastFPGATime = fpgaTime; + refreshCurrentPath(); + } + + if (paths.isEmpty()) { + return autoData.getStartingPose(); + } + + distanceAlongCurrentPath += (fpgaTime - lastFPGATime) * speed * ASSUMED_SPEED_MPS; + lastFPGATime = fpgaTime; + + while (distanceAlongCurrentPath > currentPathLength) { + distanceAlongCurrentPath -= currentPathLength; + pathIndex++; + if (pathIndex >= paths.size()) { + pathIndex = 0; + } + refreshCurrentPath(); + } + + return samplePolyline(currentTranslations, distanceAlongCurrentPath); + } + + public void update(String autoName) { + if (DriverStation.isEnabled()) { + lastName = Optional.empty(); + return; + } + if (autoName == null) return; + + field.setRobotPose(getUpdatedPose(autoName)); + if (autoData != null) { + fieldPoseStart.setRobotPose(autoData.getStartingPose()); + } + } + + public double autoTotalTime() { + if (autoData == null) { + return 0; + } + return autoData.getRunTime(); + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java new file mode 100644 index 000000000..9f1ab2877 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.auto.BLine; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.lib.BLine.Path; +import java.util.ArrayList; +import java.util.List; + +public class BLineAutos { + + private static final double ASSUMED_SPEED_MPS = 3.0; + + private final List paths; + private final Pose2d startingPose; + private final double estimatedDuration; + + public BLineAutos(String... pathNames) { + List loaded = new ArrayList<>(); + for (String name : pathNames) { + try { + loaded.add(new Path(name)); + } catch (Exception e) { + + System.err.println("[BLineAutos] Could not load path: " + name); + } + } + this.paths = List.copyOf(loaded); + + if (!this.paths.isEmpty()) { + Pose2d start = this.paths.get(0).getStartPose(); + this.startingPose = (start != null) ? start : Pose2d.kZero; + } else { + this.startingPose = Pose2d.kZero; + } + + double totalMeters = 0; + for (Path path : this.paths) { + totalMeters += arcLength(path.getTranslations()); + } + this.estimatedDuration = totalMeters / ASSUMED_SPEED_MPS; + } + + private static double arcLength(List pts) { + double total = 0; + for (int i = 1; i < pts.size(); i++) { + total += pts.get(i).getDistance(pts.get(i - 1)); + } + return total; + } + + public List getPaths() { + return paths; + } + + public Pose2d getStartingPose() { + return startingPose; + } + + public double getRunTime() { + return estimatedDuration; + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 581e5dbce..17c743ee7 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -39,7 +38,7 @@ public enum StartPosition { LEFT_TRENCH("Left Trench", new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90))), CENTER("Center", new Pose2d(3.600, 4.035, Rotation2d.fromDegrees(0))), RIGHT_TRENCH("Right Trench", new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90))), - MISC("Misc", null); + MISC("Misc", new Pose2d()); public final String title; public final Pose2d startPose; @@ -90,12 +89,8 @@ public static void init(Subsystems subsystems) { rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral","RT"), - new BLinePath( - "RTRBNEUTRAL", - "RTNeutral", -"RT") -); + new BLinePath("RTNeutral", "RTNeutral", "RT"), + new BLinePath("RTRBNEUTRAL", "RTNeutral", "RT")); autos.clear(); autos.addAll(rebuiltPaths); @@ -174,8 +169,6 @@ public static void initSmartDashboard() { autoDelayEntry.setDouble(0.0); - - startPositionChooser.onChange( v -> { filterAutos(gameObjects.getSelected()); @@ -189,10 +182,7 @@ public static void initSmartDashboard() { refreshFieldDisplay(); }); - refreshFieldDisplay(); - - } private static void refreshFieldDisplay() { @@ -204,14 +194,7 @@ private static void refreshFieldDisplay() { SmartDashboard.putData("Selected auto", field); SmartDashboard.putData("Start pose", fieldPoseStart); BLineField.drawPath(field, "Selected auto", getSelectedAutoPath().getPath()); - -} - - - - - - + } // ========================= AUTOS FILTERING ========================= From c6826369ddb6a111e4ca3875c9bd26d10e3fee83 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 16:44:00 -0700 Subject: [PATCH 053/114] added "estimated duration"(manually entered) --- src/main/deploy/autos/config.json | 5 ++++- .../deploy/autos/paths/RTNEUTRALTEST.json | 16 ++++++++-------- .../auto/BLine/BLineAutonomousField.java | 12 ++++-------- .../subsystems/auto/BLine/BLineAutos.java | 6 ------ .../subsystems/auto/BLine/BLineLogic.java | 6 +++--- .../subsystems/auto/BLine/BLinePath.java | 19 +++++++++++++++---- 6 files changed, 34 insertions(+), 30 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index c53221b30..4861e9149 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -22,6 +22,9 @@ "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0 + "default_end_rotation_tolerance_deg": 2.0, + "default_auto_velocity_velocity_safety_factor": 0.9, + "default_auto_velocity_acceleration_safety_factor": 0.8, + "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index 22564d4ef..0f238bf27 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -32,24 +32,24 @@ "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "event_trigger", - "t_ratio": 0.45000000000000007, + "t_ratio": 0.45, "lib_key": "intake" }, { "type": "waypoint", "translation_target": { - "x_meters": 6.72164648834793, - "y_meters": 0.8234506861322366, + "x_meters": 6.72165, + "y_meters": 0.82345, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, @@ -79,8 +79,8 @@ }, { "type": "translation", - "x_meters": 6.3106044072438925, - "y_meters": 0.6009207173332429, + "x_meters": 6.3106, + "y_meters": 0.60092, "intermediate_handoff_radius_meters": 0.3 }, { @@ -126,4 +126,4 @@ } ] } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java index 5a8359ef1..c983a68ea 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -33,6 +33,7 @@ public static void initSmartDashBoard( speedMultiplier.setDouble(DEFAULT_PLAYBACK_SPEED); + @SuppressWarnings("static-access") BLineAutonomousField autonomousField = new BLineAutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); @@ -42,8 +43,10 @@ public static void initSmartDashBoard( addPeriodic.accept( () -> { autonomousField.update(BLineLogic.getSelectedAutoName()); + BLineLogic.getSelectedAutoPath(); SmartDashboard.putNumber( - "Est. Time (s)", Math.round(autonomousField.autoTotalTime() * 100.0) / 100.0); + "Est. Time (s)", + Math.round(BLineLogic.getSelectedAutoPath().autoTotalTime() * 100.0) / 100.0); }, UPDATE_RATE); } @@ -173,11 +176,4 @@ public void update(String autoName) { fieldPoseStart.setRobotPose(autoData.getStartingPose()); } } - - public double autoTotalTime() { - if (autoData == null) { - return 0; - } - return autoData.getRunTime(); - } } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java index 9f1ab2877..a6641edc9 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java @@ -12,7 +12,6 @@ public class BLineAutos { private final List paths; private final Pose2d startingPose; - private final double estimatedDuration; public BLineAutos(String... pathNames) { List loaded = new ArrayList<>(); @@ -37,7 +36,6 @@ public BLineAutos(String... pathNames) { for (Path path : this.paths) { totalMeters += arcLength(path.getTranslations()); } - this.estimatedDuration = totalMeters / ASSUMED_SPEED_MPS; } private static double arcLength(List pts) { @@ -55,8 +53,4 @@ public List getPaths() { public Pose2d getStartingPose() { return startingPose; } - - public double getRunTime() { - return estimatedDuration; - } } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 17c743ee7..dabefe9d2 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -84,13 +84,13 @@ public static void init(Subsystems subsystems) { if (pathsInitialized) return; - defaultPath = new BLinePath("test1", "test1", "LT"); + defaultPath = new BLinePath("test1", "test1", "LT", 5.72); rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral", "RT"), - new BLinePath("RTRBNEUTRAL", "RTNeutral", "RT")); + new BLinePath("RTNeutral", "RTNeutral", "RT", 5.22), + new BLinePath("RTRBNEUTRAL", "RTNeutral", "RT", 8.62)); autos.clear(); autos.addAll(rebuiltPaths); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index a5cb00a08..ecc62fe2e 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -11,18 +11,25 @@ public class BLinePath { private Pose2d startPose; private final boolean vision; private final Path path; + private double estimatedTime; private String autoName; - public BLinePath(String displayName, String autoName, String startingPosName) { - this(displayName, autoName, startingPosName, false); + public BLinePath( + String displayName, String autoName, String startingPosName, double estimatedTime) { + this(displayName, autoName, startingPosName, false, estimatedTime); } - public BLinePath(String displayName, String autoName, String startingPosName, boolean vision) { + public BLinePath( + String displayName, + String autoName, + String startingPosName, + boolean vision, + double estimate) { this.displayName = displayName; this.startingPosName = startingPosName; this.vision = vision; this.autoName = autoName; - + this.estimatedTime = estimate; this.path = new Path(autoName); this.startPose = path.getStartPose(); @@ -35,6 +42,10 @@ public String getDisplayName() { return displayName; } + public double autoTotalTime() { + return estimatedTime; + } + public String getAutoName() { return displayName; } From 91a1f0e64c7258c783c54e0b9bd427fbfc9f8466 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 17:21:46 -0700 Subject: [PATCH 054/114] pretty much finished autoselector? --- .../auto/BLine/BLineAutonomousField.java | 57 +++++++++---------- .../subsystems/auto/BLine/BLineLogic.java | 29 +++++++--- .../subsystems/auto/BLine/BLinePath.java | 54 ++++++++++-------- 3 files changed, 77 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java index c983a68ea..fcc1e798b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -7,8 +7,6 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.lib.BLine.Path; import java.util.List; import java.util.Optional; @@ -17,11 +15,13 @@ import java.util.function.Supplier; public class BLineAutonomousField { - private static final double DEFAULT_PLAYBACK_SPEED = 1.0; - private static final double UPDATE_RATE = 0.05; + private static final double DEFAULT_PLAYBACK_SPEED = 1.0; + private static final double UPDATE_RATE = 0.02; private static final double ASSUMED_SPEED_MPS = 3.0; + /* ---------------- NetworkTables init ---------------- */ + public static void initSmartDashBoard( Supplier tabName, int columnIndex, @@ -33,40 +33,36 @@ public static void initSmartDashBoard( speedMultiplier.setDouble(DEFAULT_PLAYBACK_SPEED); - @SuppressWarnings("static-access") BLineAutonomousField autonomousField = new BLineAutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); - SmartDashboard.putData("Selected auto", autonomousField.getField()); - SmartDashboard.putData("Start pose", autonomousField.getStartPose()); - addPeriodic.accept( () -> { autonomousField.update(BLineLogic.getSelectedAutoName()); - BLineLogic.getSelectedAutoPath(); - SmartDashboard.putNumber( - "Est. Time (s)", - Math.round(BLineLogic.getSelectedAutoPath().autoTotalTime() * 100.0) / 100.0); }, UPDATE_RATE); } - private final Field2d field = new Field2d(); - private final Field2d fieldPoseStart = new Field2d(); + /* ---------------- Path data ---------------- */ - private BLineAutos autoData; - private List paths; + private List paths = List.of(); private int pathIndex = 0; + private Pose2d startingPose = Pose2d.kZero; + + /* ---------------- Polylines ---------------- */ private List currentTranslations = List.of(); private double currentPathLength = 1e-9; + /* ---------------- Time / distance ---------------- */ + private final DoubleSupplier speedMultiplier; private double lastFPGATime; private double distanceAlongCurrentPath; private Optional lastName = Optional.empty(); + /* ---------------- Constructors ---------------- */ public BLineAutonomousField() { this(() -> 1.0); } @@ -79,13 +75,7 @@ public BLineAutonomousField(DoubleSupplier speedMultiplier) { this.speedMultiplier = speedMultiplier; } - public Field2d getField() { - return field; - } - - public Field2d getStartPose() { - return fieldPoseStart; - } + /* ---------------- Geometry helpers ---------------- */ private static double arcLength(List pts) { double total = 0; @@ -120,7 +110,7 @@ private static Pose2d samplePolyline(List pts, double dist) { } private void refreshCurrentPath() { - if (paths == null || paths.isEmpty()) { + if (paths.isEmpty()) { currentTranslations = List.of(); currentPathLength = 1e-9; return; @@ -129,6 +119,8 @@ private void refreshCurrentPath() { currentPathLength = Math.max(arcLength(currentTranslations), 1e-9); } + /* ---------------- Pose update ---------------- */ + public Pose2d getUpdatedPose(String autoName) { if (autoName == null) return Pose2d.kZero; @@ -137,8 +129,11 @@ public Pose2d getUpdatedPose(String autoName) { if (lastName.isEmpty() || !lastName.get().equals(autoName)) { lastName = Optional.of(autoName); - autoData = new BLineAutos(autoName); - paths = autoData.getPaths(); + + BLinePath selected = BLineLogic.getSelectedAutoPath(); + paths = (selected != null) ? selected.getAllPaths() : List.of(); + startingPose = BLineLogic.getSelectedAutoStartingPose(); + pathIndex = 0; distanceAlongCurrentPath = 0; lastFPGATime = fpgaTime; @@ -146,7 +141,7 @@ public Pose2d getUpdatedPose(String autoName) { } if (paths.isEmpty()) { - return autoData.getStartingPose(); + return startingPose; } distanceAlongCurrentPath += (fpgaTime - lastFPGATime) * speed * ASSUMED_SPEED_MPS; @@ -164,6 +159,8 @@ public Pose2d getUpdatedPose(String autoName) { return samplePolyline(currentTranslations, distanceAlongCurrentPath); } + /* ---------------- Periodic update ---------------- */ + public void update(String autoName) { if (DriverStation.isEnabled()) { lastName = Optional.empty(); @@ -171,9 +168,7 @@ public void update(String autoName) { } if (autoName == null) return; - field.setRobotPose(getUpdatedPose(autoName)); - if (autoData != null) { - fieldPoseStart.setRobotPose(autoData.getStartingPose()); - } + BLineLogic.field.setRobotPose(getUpdatedPose(autoName)); + BLineLogic.fieldPoseStart.setRobotPose(startingPose); } } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index dabefe9d2..1322c5450 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -83,15 +83,13 @@ public static void init(Subsystems subsystems) { registerCommands(); if (pathsInitialized) return; - - defaultPath = new BLinePath("test1", "test1", "LT", 5.72); + defaultPath = new BLinePath("test1", "LT", 5.72, "test1"); rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral", "RT", 5.22), - new BLinePath("RTRBNEUTRAL", "RTNeutral", "RT", 8.62)); - + new BLinePath("RTNeutral", "RT", 5.22, "RTNeutral"), + new BLinePath("RTRBNEUTRAL", "RT", 8.62, "RTNeutral", "RTRBNEUTRAL")); autos.clear(); autos.addAll(rebuiltPaths); @@ -185,15 +183,28 @@ public static void initSmartDashboard() { refreshFieldDisplay(); } - private static void refreshFieldDisplay() { + private static int lastPathCount = 0; + + public static void refreshFieldDisplay() { BLinePath selected = getSelectedAutoPath(); - if (selected == null || selected.getPath() == null) return; + if (selected == null) return; fieldPoseStart.setRobotPose(getSelectedAutoStartingPose()); - SmartDashboard.putData("Selected auto", field); SmartDashboard.putData("Start pose", fieldPoseStart); - BLineField.drawPath(field, "Selected auto", getSelectedAutoPath().getPath()); + + List allPaths = selected.getAllPaths(); + + for (int i = 0; i < allPaths.size(); i++) { + BLineField.drawPath(field, "AutoPart" + i, allPaths.get(i)); + } + + // overwrite stale slots from a previously longer auto + for (int i = allPaths.size(); i < lastPathCount; i++) { + BLineField.drawPath(field, "AutoPart" + i, allPaths.get(0)); + } + + lastPathCount = allPaths.size(); } // ========================= AUTOS FILTERING ========================= diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index ecc62fe2e..b4c6f78a1 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -3,64 +3,68 @@ import edu.wpi.first.math.geometry.Pose2d; import frc.robot.lib.BLine.Path; import frc.robot.subsystems.auto.BLine.BLineLogic.StartPosition; +import java.util.ArrayList; +import java.util.List; public class BLinePath { private final String displayName; - private String startingPosName; - private Pose2d startPose; + private final String startingPosName; private final boolean vision; + private final double estimatedTime; private final Path path; - private double estimatedTime; - private String autoName; + private final List allPaths; + private Pose2d startPose; + // Single path constructor public BLinePath( String displayName, String autoName, String startingPosName, double estimatedTime) { - this(displayName, autoName, startingPosName, false, estimatedTime); + this(displayName, startingPosName, estimatedTime, false, autoName); + } + + // Multi-path constructor + public BLinePath( + String displayName, String startingPosName, double estimatedTime, String... pathNames) { + this(displayName, startingPosName, estimatedTime, false, pathNames); } public BLinePath( String displayName, - String autoName, String startingPosName, + double estimatedTime, boolean vision, - double estimate) { + String... pathNames) { this.displayName = displayName; this.startingPosName = startingPosName; + this.estimatedTime = estimatedTime; this.vision = vision; - this.autoName = autoName; - this.estimatedTime = estimate; - this.path = new Path(autoName); - this.startPose = path.getStartPose(); - if (startPose == null) { - throw new IllegalStateException("Path missing start pose: " + startingPosName); + List loaded = new ArrayList<>(); + for (String name : pathNames) { + loaded.add(new Path(name)); } + this.allPaths = List.copyOf(loaded); + this.path = this.allPaths.get(0); + this.startPose = this.path.getStartPose(); } public String getDisplayName() { return displayName; } - public double autoTotalTime() { - return estimatedTime; - } - - public String getAutoName() { - return displayName; - } - public String getStartingPosName() { return startingPosName; } - public Pose2d getStartPose2d() { + public double autoTotalTime() { + return estimatedTime; + } + public Pose2d getStartPose2d() { return startPose; } public Pose2d setStartPose2d(StartPosition pos) { - startPose = pos.startPose; return startPose; } @@ -72,4 +76,8 @@ public boolean isVision() { public Path getPath() { return path; } + + public List getAllPaths() { + return allPaths; + } } From c52ee10fcbac610e0cd1403e901f1fb548ac654a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 17:25:57 -0700 Subject: [PATCH 055/114] added back time estimates + spotless --- .../frc/robot/subsystems/auto/BLine/BLineAutonomousField.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java index fcc1e798b..4744d58a9 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -7,6 +7,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.lib.BLine.Path; import java.util.List; import java.util.Optional; @@ -39,6 +40,9 @@ public static void initSmartDashBoard( addPeriodic.accept( () -> { autonomousField.update(BLineLogic.getSelectedAutoName()); + SmartDashboard.putNumber( + "Est. Time (s)", + Math.round(BLineLogic.getSelectedAutoPath().autoTotalTime() * 100.0) / 100.0); }, UPDATE_RATE); } From 8acf6a32eaad4b1d9b9fd2bfa699cdb1e63efc19 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 17:44:22 -0700 Subject: [PATCH 056/114] added new elastic layout --- src/main/deploy/bline-layout.json | 185 ++++++++++++++++++++++++++++++ 1 file changed, 185 insertions(+) create mode 100644 src/main/deploy/bline-layout.json diff --git a/src/main/deploy/bline-layout.json b/src/main/deploy/bline-layout.json new file mode 100644 index 000000000..d03d84477 --- /dev/null +++ b/src/main/deploy/bline-layout.json @@ -0,0 +1,185 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "offset", + "x": 0.0, + "y": 256.0, + "width": 1152.0, + "height": 128.0, + "type": "Number Slider", + "properties": { + "topic": "/SmartDashboard/offset", + "period": 0.06, + "data_type": "double", + "min_value": 1.0, + "max_value": 10.0, + "divisions": 5, + "update_continuously": true + } + } + ] + } + }, + { + "name": "Autos", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Selected auto", + "x": 0.0, + "y": 0.0, + "width": 1024.0, + "height": 896.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Selected auto", + "period": 0.06, + "field_game": "Rebuilt", + "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": "Start pose", + "x": 1024.0, + "y": 384.0, + "width": 768.0, + "height": 512.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Start pose", + "period": 0.06, + "field_game": "Rebuilt", + "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": "Starting Position", + "x": 1408.0, + "y": 128.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Starting Position", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Auto Key", + "x": 1408.0, + "y": 0.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Auto Key", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Est. Time (s)", + "x": 1152.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/Est. Time (s)", + "period": 0.06, + "data_type": "double" + } + }, + { + "title": "Available Auto Variants", + "x": 1024.0, + "y": 256.0, + "width": 768.0, + "height": 128.0, + "type": "Split Button Chooser", + "properties": { + "topic": "/SmartDashboard/Available Auto Variants", + "period": 0.06 + } + }, + { + "title": "Auto Delay", + "x": 1280.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Autos/Auto Delay", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "DisplaySpeed", + "x": 1024.0, + "y": 0.0, + "width": 384.0, + "height": 128.0, + "type": "Number Slider", + "properties": { + "topic": "/Autos/DisplaySpeed", + "period": 0.06, + "data_type": "double", + "min_value": -4.0, + "max_value": 4.0, + "divisions": 5, + "update_continuously": true + } + }, + { + "title": "Initial Heading", + "x": 1024.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Initial Heading", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + } + ] +} \ No newline at end of file From 026735011526e94b6c8dc730e4cdf95d91670cde Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 18:00:47 -0700 Subject: [PATCH 057/114] rounded module heading --- src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 1322c5450..a2dc6e529 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -318,7 +318,7 @@ private static void updateInitialHeading() { Pose2d start = selected.getPath().getStartPose(); - SmartDashboard.putNumber("Initial Heading", start.getRotation().getDegrees()); + SmartDashboard.putNumber("Initial Heading", Math.round(start.getRotation().getDegrees())); } public static Command intakeCommand() { From 4bc8980a8855ab0003ba40afadf9c5bec5a5d944 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 31 May 2026 18:20:20 -0700 Subject: [PATCH 058/114] removed est time --- .../auto/BLine/BLineAutonomousField.java | 4 +--- .../subsystems/auto/BLine/BLineLogic.java | 10 +++++----- .../robot/subsystems/auto/BLine/BLinePath.java | 18 ++++++++---------- 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java index 4744d58a9..e9c8147ec 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -40,9 +40,7 @@ public static void initSmartDashBoard( addPeriodic.accept( () -> { autonomousField.update(BLineLogic.getSelectedAutoName()); - SmartDashboard.putNumber( - "Est. Time (s)", - Math.round(BLineLogic.getSelectedAutoPath().autoTotalTime() * 100.0) / 100.0); + }, UPDATE_RATE); } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index a2dc6e529..87242f7a7 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -83,13 +83,13 @@ public static void init(Subsystems subsystems) { registerCommands(); if (pathsInitialized) return; - defaultPath = new BLinePath("test1", "LT", 5.72, "test1"); + defaultPath = new BLinePath("test1", "test1", "LT"); rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RT", 5.22, "RTNeutral"), - new BLinePath("RTRBNEUTRAL", "RT", 8.62, "RTNeutral", "RTRBNEUTRAL")); + new BLinePath("RTNeutral", "RTNeutral","RT"), + new BLinePath("RTRBNEUTRAL", "RT", "RTNeutral","RTRBNEUTRAL")); autos.clear(); autos.addAll(rebuiltPaths); @@ -312,13 +312,13 @@ private static void updateInitialHeading() { BLinePath selected = getSelectedAutoPath(); if (selected == null || selected.getPath() == null) { - SmartDashboard.putNumber("Initial Heading", 0.0); + SmartDashboard.putNumber("Initial Heading(Deg)", 0.0); return; } Pose2d start = selected.getPath().getStartPose(); - SmartDashboard.putNumber("Initial Heading", Math.round(start.getRotation().getDegrees())); + SmartDashboard.putNumber("Initial Heading(Deg)", Math.round(start.getRotation().getDegrees())); } public static Command intakeCommand() { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index b4c6f78a1..4c4f68b72 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -11,32 +11,32 @@ public class BLinePath { private final String displayName; private final String startingPosName; private final boolean vision; - private final double estimatedTime; + private final Path path; private final List allPaths; private Pose2d startPose; // Single path constructor public BLinePath( - String displayName, String autoName, String startingPosName, double estimatedTime) { - this(displayName, startingPosName, estimatedTime, false, autoName); + String displayName, String autoName, String startingPosName) { + this(displayName, startingPosName, false, autoName); } + // Multi-path constructor public BLinePath( - String displayName, String startingPosName, double estimatedTime, String... pathNames) { - this(displayName, startingPosName, estimatedTime, false, pathNames); + String displayName, String startingPosName, String... pathNames) { + this(displayName, startingPosName, false, pathNames); } public BLinePath( String displayName, String startingPosName, - double estimatedTime, boolean vision, String... pathNames) { this.displayName = displayName; this.startingPosName = startingPosName; - this.estimatedTime = estimatedTime; + this.vision = vision; List loaded = new ArrayList<>(); @@ -56,9 +56,7 @@ public String getStartingPosName() { return startingPosName; } - public double autoTotalTime() { - return estimatedTime; - } + public Pose2d getStartPose2d() { return startPose; From d85304a1ac23badb1adf38e3be08b081b2f7c245 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 1 Jun 2026 09:07:17 -0700 Subject: [PATCH 059/114] simplified layout --- .../robot/subsystems/auto/BLine/BLineLogic.java | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 87242f7a7..80b63410f 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -192,21 +192,9 @@ public static void refreshFieldDisplay() { fieldPoseStart.setRobotPose(getSelectedAutoStartingPose()); SmartDashboard.putData("Selected auto", field); SmartDashboard.putData("Start pose", fieldPoseStart); - - List allPaths = selected.getAllPaths(); - - for (int i = 0; i < allPaths.size(); i++) { - BLineField.drawPath(field, "AutoPart" + i, allPaths.get(i)); - } - - // overwrite stale slots from a previously longer auto - for (int i = allPaths.size(); i < lastPathCount; i++) { - BLineField.drawPath(field, "AutoPart" + i, allPaths.get(0)); - } - - lastPathCount = allPaths.size(); } + // ========================= AUTOS FILTERING ========================= public static void filterAutos(int numGameObjects) { From 3a35147899115efe10b16fd86bcf57961b3987a3 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 1 Jun 2026 16:03:47 -0700 Subject: [PATCH 060/114] some cleanup and spotless --- .../auto/BLine/BLineAutonomousField.java | 2 -- .../subsystems/auto/BLine/BLineLogic.java | 21 +++---------------- .../subsystems/auto/BLine/BLinePath.java | 14 +++---------- 3 files changed, 6 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java index e9c8147ec..fcc1e798b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutonomousField.java @@ -7,7 +7,6 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.lib.BLine.Path; import java.util.List; import java.util.Optional; @@ -40,7 +39,6 @@ public static void initSmartDashBoard( addPeriodic.accept( () -> { autonomousField.update(BLineLogic.getSelectedAutoName()); - }, UPDATE_RATE); } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 80b63410f..a37f0f1a3 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -11,13 +11,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Controls; import frc.robot.Robot; import frc.robot.Subsystems; import frc.robot.lib.BLine.BLineCommands; -import frc.robot.lib.BLine.BLineField; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; import frc.robot.subsystems.auto.Misc.DynamicSendableChooser; @@ -72,7 +70,6 @@ public enum StartPosition { public static FollowPath.Builder pathBuilder; private static FollowPath.Builder continuingPathBuilder; - private static Command bLineLaunching; // ========================= INIT ========================= @@ -88,8 +85,8 @@ public static void init(Subsystems subsystems) { rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral","RT"), - new BLinePath("RTRBNEUTRAL", "RT", "RTNeutral","RTRBNEUTRAL")); + new BLinePath("RTNeutral", "RTNeutral", "RT"), + new BLinePath("RTRBNEUTRAL", "RT", "RTNeutral", "RTRBNEUTRAL")); autos.clear(); autos.addAll(rebuiltPaths); @@ -194,7 +191,6 @@ public static void refreshFieldDisplay() { SmartDashboard.putData("Start pose", fieldPoseStart); } - // ========================= AUTOS FILTERING ========================= public static void filterAutos(int numGameObjects) { @@ -355,22 +351,11 @@ public static Command climbCommand() { return Commands.none().withName("Auto Climb Command"); } - public static void cancelCommand() { - CommandScheduler.getInstance().cancel(bLineLaunching); - } - private static void registerCommands() { if (s.launcherSubsystem != null && s.indexerSubsystem != null) { - if (Robot.isSimulation()) { - bLineLaunching = RobotSim.launch(s, 1); - FollowPath.registerEventTrigger( - "launch", bLineLaunching.andThen(Commands.print("LAUNCH FINISHED"))); - } else { - bLineLaunching = launcherCommand(); - FollowPath.registerEventTrigger("launch", bLineLaunching); - } + // TODO ADD LAUNCHER STUFF FOR SOTM? } FollowPath.registerEventTrigger("intake", intakeCommand()); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index 4c4f68b72..f5ef98891 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -17,23 +17,17 @@ public class BLinePath { private Pose2d startPose; // Single path constructor - public BLinePath( - String displayName, String autoName, String startingPosName) { + public BLinePath(String displayName, String autoName, String startingPosName) { this(displayName, startingPosName, false, autoName); } - // Multi-path constructor - public BLinePath( - String displayName, String startingPosName, String... pathNames) { + public BLinePath(String displayName, String startingPosName, String... pathNames) { this(displayName, startingPosName, false, pathNames); } public BLinePath( - String displayName, - String startingPosName, - boolean vision, - String... pathNames) { + String displayName, String startingPosName, boolean vision, String... pathNames) { this.displayName = displayName; this.startingPosName = startingPosName; @@ -56,8 +50,6 @@ public String getStartingPosName() { return startingPosName; } - - public Pose2d getStartPose2d() { return startPose; } From 34eacf58d1e399a9391fbe386d35f0ac61a89212 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 1 Jun 2026 16:44:03 -0700 Subject: [PATCH 061/114] trying to do SOTM --- .../deploy/autos/paths/RTNEUTRALTEST.json | 5 ++++ src/main/deploy/autos/paths/RTNeutral.json | 2 +- .../deploy/autos/paths/RTRBNEUTRALTEST.json | 16 ++++++++-- .../subsystems/auto/BLine/BLineLogic.java | 30 +++++++++++++++---- 4 files changed, 43 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index 0f238bf27..ead883ace 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -83,6 +83,11 @@ "y_meters": 0.60092, "intermediate_handoff_radius_meters": 0.3 }, + { + "type": "event_trigger", + "t_ratio": 0.8, + "lib_key": "launch" + }, { "type": "waypoint", "translation_target": { diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index e0e391da8..6077d0686 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -26,7 +26,7 @@ }, { "type": "event_trigger", - "t_ratio": 0.45, + "t_ratio": 0.4, "lib_key": "intake" }, { diff --git a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json index eae7a6146..f0abeaa90 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json @@ -12,11 +12,16 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.8, + "lib_key": "cancel" + }, { "type": "translation", - "x_meters": 3.02494, - "y_meters": 1.32727, - "intermediate_handoff_radius_meters": 0.6 + "x_meters": 3.19373, + "y_meters": 2.05349, + "intermediate_handoff_radius_meters": 0.28 }, { "type": "translation", @@ -87,6 +92,11 @@ "value": 2.5, "start_ordinal": 4, "end_ordinal": 4 + }, + { + "value": 1.0, + "start_ordinal": 1, + "end_ordinal": 1 } ] } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index a37f0f1a3..c0c7c2391 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -25,6 +25,7 @@ import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.concurrent.atomic.AtomicBoolean; public class BLineLogic { @@ -67,7 +68,7 @@ public enum StartPosition { private static final Map namesToAuto = new HashMap<>(); private static boolean pathsInitialized = false; - +private static Command bLineLaunching; public static FollowPath.Builder pathBuilder; private static FollowPath.Builder continuingPathBuilder; @@ -268,9 +269,9 @@ public static Command buildRTNeutralTestingAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildPath(new Path("RTNeutral"), true), - launcherCommand(4.5), - buildPath(new Path("RTRBNEUTRAL"), false), + buildPath(new Path("RTNEUTRALTEST"), true), + + buildPath(new Path("RTRBNEUTRALTEST"), false), launcherCommand()); } @@ -352,13 +353,30 @@ public static Command climbCommand() { } private static void registerCommands() { - + AtomicBoolean launchAllowed = new AtomicBoolean(true); if (s.launcherSubsystem != null && s.indexerSubsystem != null) { - // TODO ADD LAUNCHER STUFF FOR SOTM? + if (Robot.isSimulation()) { + bLineLaunching = RobotSim.launch(s, 30); + FollowPath.registerEventTrigger( + "launch", + Commands.runOnce(() -> launchAllowed.set(true)) + .andThen( + RobotSim.launch(s, 30) + .until( + () -> { + return launchAllowed.get(); + })) + .andThen(Commands.print("LAUNCH FINISHED"))); + } else { + bLineLaunching = launcherCommand(); + FollowPath.registerEventTrigger("launch", bLineLaunching); + } } FollowPath.registerEventTrigger("intake", intakeCommand()); FollowPath.registerEventTrigger("climb", climbCommand()); + FollowPath.registerEventTrigger( + "cancel", Commands.runOnce(() -> launchAllowed.set(false))); } } From 2b13e4dc144247171cb8207becee538410bea56b Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 1 Jun 2026 16:52:45 -0700 Subject: [PATCH 062/114] better pathing --- src/main/deploy/autos/paths/RTNeutral.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index e0e391da8..ae9f8ab79 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -26,7 +26,7 @@ }, { "type": "event_trigger", - "t_ratio": 0.45, + "t_ratio": 0.0, "lib_key": "intake" }, { @@ -67,8 +67,8 @@ }, { "type": "translation", - "x_meters": 6.2229, - "y_meters": 0.56521, + "x_meters": 6.41431, + "y_meters": 0.64523, "intermediate_handoff_radius_meters": 0.5 }, { From cf0e74db7cccb2e31c7cd36cbacc43199f416482 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 1 Jun 2026 16:55:34 -0700 Subject: [PATCH 063/114] fixed dimensions --- src/main/deploy/autos/config.json | 4 ++-- src/main/deploy/autos/paths/RTNeutral.json | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 4861e9149..362f5d147 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -1,8 +1,8 @@ { "gui": { "robot": { - "length_meters": 0.937, - "width_meters": 0.781 + "length_meters": 0.78, + "width_meters": 0.94 }, "protrusions": { "enabled": true, diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index ae9f8ab79..628ac22b0 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -67,8 +67,8 @@ }, { "type": "translation", - "x_meters": 6.41431, - "y_meters": 0.64523, + "x_meters": 6.57682, + "y_meters": 0.62778, "intermediate_handoff_radius_meters": 0.5 }, { From b6d594505acf39a887de36d45e5bd02e7c7f3b62 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 1 Jun 2026 17:02:01 -0700 Subject: [PATCH 064/114] more fix --- src/main/deploy/autos/paths/RTNeutral.json | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 628ac22b0..078768552 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -52,7 +52,7 @@ "translation_target": { "x_meters": 7.74195, "y_meters": 3.09426, - "intermediate_handoff_radius_meters": 0.5 + "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { "rotation_radians": -1.5708, @@ -63,19 +63,19 @@ "type": "translation", "x_meters": 7.23228, "y_meters": 1.90119, - "intermediate_handoff_radius_meters": 0.5 + "intermediate_handoff_radius_meters": 0.3 }, { "type": "translation", - "x_meters": 6.57682, - "y_meters": 0.62778, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 6.95578, + "y_meters": 1.04364, + "intermediate_handoff_radius_meters": 0.2 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.54294, - "y_meters": 0.51232, + "x_meters": 4.11942, + "y_meters": 0.47062, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { From 18a38b1c9b31f3b814b57988d1bb339f7af6d36d Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 1 Jun 2026 17:37:20 -0700 Subject: [PATCH 065/114] updated wheel radius --- src/main/deploy/autos/paths/RTNeutral.json | 28 +++++++++++++------ .../robot/generated/CompTunerConstants.java | 2 +- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 078768552..70c9ad18a 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -61,15 +61,15 @@ }, { "type": "translation", - "x_meters": 7.23228, - "y_meters": 1.90119, + "x_meters": 7.10746, + "y_meters": 2.04773, "intermediate_handoff_radius_meters": 0.3 }, { "type": "translation", - "x_meters": 6.95578, - "y_meters": 1.04364, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 6.69956, + "y_meters": 1.2829, + "intermediate_handoff_radius_meters": 0.35 }, { "type": "waypoint", @@ -85,10 +85,10 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.05, + "end_translation_tolerance_meters": 0.03, "max_velocity_meters_per_sec": [ { - "value": 3.0, + "value": 2.5, "start_ordinal": 7, "end_ordinal": 7 }, @@ -115,10 +115,20 @@ ], "max_acceleration_meters_per_sec2": [ { - "value": 4.0, + "value": 2.5, "start_ordinal": 5, "end_ordinal": 5 + }, + { + "value": 2.5, + "start_ordinal": 6, + "end_ordinal": 6 + }, + { + "value": 3.0, + "start_ordinal": 7, + "end_ordinal": 7 } ] } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 80f6fa662..23040bc19 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -104,7 +104,7 @@ public class CompTunerConstants { private static final double kDriveGearRatio = 5.357142857142857; private static final double kSteerGearRatio = 18.75; - public static final Distance kWheelRadius = Inches.of(1.924); + public static final Distance kWheelRadius = Inches.of(1.983); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 5d50bd358afc621b7490ad6f340fcf5b5844376c Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 1 Jun 2026 18:00:08 -0700 Subject: [PATCH 066/114] stuff okay --- src/main/deploy/autos/paths/RTNeutral.json | 4 ++-- src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/RTNeutral.json index 70c9ad18a..c93a94a96 100644 --- a/src/main/deploy/autos/paths/RTNeutral.json +++ b/src/main/deploy/autos/paths/RTNeutral.json @@ -85,7 +85,7 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.03, + "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { "value": 2.5, @@ -93,7 +93,7 @@ "end_ordinal": 7 }, { - "value": 3.0, + "value": 2.5, "start_ordinal": 2, "end_ordinal": 2 }, diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index a37f0f1a3..e3368e27f 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -261,7 +261,8 @@ public static Command buildRTNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("RTNeutral"), true), - launcherCommand(4)); + launcherCommand(1), + buildPath(new Path("RTNeutral"), false)); } public static Command buildRTNeutralTestingAuto() { From 86545fef0ebabb15517aba7688c7245cf1a5a47b Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 1 Jun 2026 18:29:30 -0700 Subject: [PATCH 067/114] tried speeding up autos --- .../deploy/autos/paths/RTNeutralFAST.json | 138 ++++++++++++++++++ 1 file changed, 138 insertions(+) create mode 100644 src/main/deploy/autos/paths/RTNeutralFAST.json diff --git a/src/main/deploy/autos/paths/RTNeutralFAST.json b/src/main/deploy/autos/paths/RTNeutralFAST.json new file mode 100644 index 000000000..8a62f600d --- /dev/null +++ b/src/main/deploy/autos/paths/RTNeutralFAST.json @@ -0,0 +1,138 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.45101, + "y_meters": 0.58991, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.72165, + "y_meters": 0.82345, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.421343333333331, + "y_meters": 1.4356900000000028, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.800416666666669, + "y_meters": 3.386593333333334, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -2.356194490192345, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": -1.5707963267948966, + "t_ratio": 0.8, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 7.19068, + "y_meters": 2.311913333333335, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "translation", + "x_meters": 6.182533333333334, + "y_meters": 0.999740000000001, + "intermediate_handoff_radius_meters": 0.5 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.11942, + "y_meters": 0.47062, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 3.0, + "start_ordinal": 7, + "end_ordinal": 7 + }, + { + "value": 3.0, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 3.0, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 4.5, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 3.0, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 3.0, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 4.5, + "start_ordinal": 6, + "end_ordinal": 6 + } + ] + } +} \ No newline at end of file From 9ef4989cebcaa9b5fce0f318379fd909f7b393a2 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 2 Jun 2026 18:51:32 -0700 Subject: [PATCH 068/114] SOTM works? --- src/main/deploy/autos/paths/RTNEUTRALTEST.json | 11 +++-------- src/main/deploy/autos/paths/RTRBNEUTRALTEST.json | 5 +++++ .../frc/robot/subsystems/auto/BLine/BLineLogic.java | 8 +++----- src/main/java/frc/robot/util/simulation/FuelSim.java | 2 +- 4 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json index ead883ace..8864e7b53 100644 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTNEUTRALTEST.json @@ -57,14 +57,14 @@ "type": "translation", "x_meters": 7.56137, "y_meters": 1.45936, - "intermediate_handoff_radius_meters": 0.5 + "intermediate_handoff_radius_meters": 1.0 }, { "type": "waypoint", "translation_target": { "x_meters": 7.85945, "y_meters": 3.05019, - "intermediate_handoff_radius_meters": 0.2 + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { "rotation_radians": -1.5708, @@ -81,12 +81,7 @@ "type": "translation", "x_meters": 6.3106, "y_meters": 0.60092, - "intermediate_handoff_radius_meters": 0.3 - }, - { - "type": "event_trigger", - "t_ratio": 0.8, - "lib_key": "launch" + "intermediate_handoff_radius_meters": 1.0 }, { "type": "waypoint", diff --git a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json index f0abeaa90..37e4c4f92 100644 --- a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json +++ b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json @@ -12,6 +12,11 @@ "profiled_rotation": true } }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "launch" + }, { "type": "event_trigger", "t_ratio": 0.8, diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index c0c7c2391..5263c8073 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -68,7 +68,7 @@ public enum StartPosition { private static final Map namesToAuto = new HashMap<>(); private static boolean pathsInitialized = false; -private static Command bLineLaunching; + private static Command bLineLaunching; public static FollowPath.Builder pathBuilder; private static FollowPath.Builder continuingPathBuilder; @@ -270,7 +270,6 @@ public static Command buildRTNeutralTestingAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("RTNEUTRALTEST"), true), - buildPath(new Path("RTRBNEUTRALTEST"), false), launcherCommand()); } @@ -365,7 +364,7 @@ private static void registerCommands() { RobotSim.launch(s, 30) .until( () -> { - return launchAllowed.get(); + return launchAllowed.get() == false; })) .andThen(Commands.print("LAUNCH FINISHED"))); } else { @@ -376,7 +375,6 @@ private static void registerCommands() { FollowPath.registerEventTrigger("intake", intakeCommand()); FollowPath.registerEventTrigger("climb", climbCommand()); - FollowPath.registerEventTrigger( - "cancel", Commands.runOnce(() -> launchAllowed.set(false))); + FollowPath.registerEventTrigger("cancel", Commands.runOnce(() -> launchAllowed.set(false))); } } diff --git a/src/main/java/frc/robot/util/simulation/FuelSim.java b/src/main/java/frc/robot/util/simulation/FuelSim.java index 73338d6b3..54e420687 100644 --- a/src/main/java/frc/robot/util/simulation/FuelSim.java +++ b/src/main/java/frc/robot/util/simulation/FuelSim.java @@ -577,7 +577,7 @@ public void launchFuel( spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); } - spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + // spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); } protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { From bb078b325f84e5fb54f104cb8e397a12ce3e6b80 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 2 Jun 2026 18:54:39 -0700 Subject: [PATCH 069/114] inverted instead of == --- src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 5263c8073..e6abf3970 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -364,7 +364,7 @@ private static void registerCommands() { RobotSim.launch(s, 30) .until( () -> { - return launchAllowed.get() == false; + return !launchAllowed.get(); })) .andThen(Commands.print("LAUNCH FINISHED"))); } else { From 110987c4f7731c6716d777a11874b4a21d87f0e3 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 2 Jun 2026 18:58:06 -0700 Subject: [PATCH 070/114] even better shorthand --- src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index e6abf3970..3ce3fc943 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -362,9 +362,9 @@ private static void registerCommands() { Commands.runOnce(() -> launchAllowed.set(true)) .andThen( RobotSim.launch(s, 30) - .until( + .onlyWhile( () -> { - return !launchAllowed.get(); + return launchAllowed.get(); })) .andThen(Commands.print("LAUNCH FINISHED"))); } else { From e7e84e5f7d2566d000baa31d2aed9b2fc0fc3f24 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 2 Jun 2026 19:04:20 -0700 Subject: [PATCH 071/114] cleanup --- .../robot/subsystems/auto/BLine/BLineAutos.java | 16 ---------------- .../robot/subsystems/auto/BLine/BLineLogic.java | 2 -- 2 files changed, 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java index a6641edc9..416ba9b16 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineAutos.java @@ -1,15 +1,12 @@ package frc.robot.subsystems.auto.BLine; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import frc.robot.lib.BLine.Path; import java.util.ArrayList; import java.util.List; public class BLineAutos { - private static final double ASSUMED_SPEED_MPS = 3.0; - private final List paths; private final Pose2d startingPose; @@ -31,19 +28,6 @@ public BLineAutos(String... pathNames) { } else { this.startingPose = Pose2d.kZero; } - - double totalMeters = 0; - for (Path path : this.paths) { - totalMeters += arcLength(path.getTranslations()); - } - } - - private static double arcLength(List pts) { - double total = 0; - for (int i = 1; i < pts.size(); i++) { - total += pts.get(i).getDistance(pts.get(i - 1)); - } - return total; } public List getPaths() { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index aa8992192..b4ae0eeb5 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -181,8 +181,6 @@ public static void initSmartDashboard() { refreshFieldDisplay(); } - private static int lastPathCount = 0; - public static void refreshFieldDisplay() { BLinePath selected = getSelectedAutoPath(); if (selected == null) return; From f64ac2e7c460346b5f11e92edc1f067b28a9725a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 2 Jun 2026 19:06:18 -0700 Subject: [PATCH 072/114] small cleanup --- .../frc/robot/subsystems/auto/BLine/BLineLogic.java | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index b4ae0eeb5..aba15e2ef 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -260,7 +260,7 @@ public static Command buildRTNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("RTNeutral"), true), - launcherCommand(1), + launcherCommand(4), buildPath(new Path("RTNeutral"), false)); } @@ -268,8 +268,9 @@ public static Command buildRTNeutralTestingAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildPath(new Path("RTNEUTRALTEST"), true), - buildPath(new Path("RTRBNEUTRALTEST"), false), + buildPath(new Path("RTNeutral"), true), + launcherCommand(4), + buildPath(new Path("RTRBNEUTRAL"), false), launcherCommand()); } @@ -294,10 +295,7 @@ private static void updateInitialHeading() { BLinePath selected = getSelectedAutoPath(); - if (selected == null || selected.getPath() == null) { - SmartDashboard.putNumber("Initial Heading(Deg)", 0.0); - return; - } + Pose2d start = selected.getPath().getStartPose(); From c0c364669a1e526a6046b49b2be33de9ebc082c3 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Wed, 3 Jun 2026 17:05:18 -0700 Subject: [PATCH 073/114] cleanup and path mirroring --- .../{RTRBNEUTRAL.json => BUMPNEUTRAL.json} | 0 src/main/deploy/autos/paths/LT-Neutral.json | 103 ---------- .../paths/{RTNeutral.json => Neutral.json} | 0 .../deploy/autos/paths/RT-Neutral-ClimbD.json | 46 ----- .../deploy/autos/paths/RTNEUTRALTEST.json | 129 ------------ .../deploy/autos/paths/RTRBNEUTRALTEST.json | 108 ---------- src/main/deploy/autos/paths/Sample_5.json | 22 --- .../paths/{sample3.json => default.json} | 18 +- src/main/deploy/autos/paths/example_a.json | 34 ---- src/main/deploy/autos/paths/example_b.json | 28 --- src/main/deploy/autos/paths/sample2.json | 40 ---- src/main/deploy/autos/paths/sample4.json | 22 --- src/main/deploy/autos/paths/test1.json | 112 ----------- .../subsystems/auto/BLine/BLineLogic.java | 187 +++++++++--------- .../subsystems/auto/BLine/BLinePath.java | 22 ++- 15 files changed, 112 insertions(+), 759 deletions(-) rename src/main/deploy/autos/paths/{RTRBNEUTRAL.json => BUMPNEUTRAL.json} (100%) delete mode 100644 src/main/deploy/autos/paths/LT-Neutral.json rename src/main/deploy/autos/paths/{RTNeutral.json => Neutral.json} (100%) delete mode 100644 src/main/deploy/autos/paths/RT-Neutral-ClimbD.json delete mode 100644 src/main/deploy/autos/paths/RTNEUTRALTEST.json delete mode 100644 src/main/deploy/autos/paths/RTRBNEUTRALTEST.json delete mode 100644 src/main/deploy/autos/paths/Sample_5.json rename src/main/deploy/autos/paths/{sample3.json => default.json} (58%) delete mode 100644 src/main/deploy/autos/paths/example_a.json delete mode 100644 src/main/deploy/autos/paths/example_b.json delete mode 100644 src/main/deploy/autos/paths/sample2.json delete mode 100644 src/main/deploy/autos/paths/sample4.json delete mode 100644 src/main/deploy/autos/paths/test1.json diff --git a/src/main/deploy/autos/paths/RTRBNEUTRAL.json b/src/main/deploy/autos/paths/BUMPNEUTRAL.json similarity index 100% rename from src/main/deploy/autos/paths/RTRBNEUTRAL.json rename to src/main/deploy/autos/paths/BUMPNEUTRAL.json diff --git a/src/main/deploy/autos/paths/LT-Neutral.json b/src/main/deploy/autos/paths/LT-Neutral.json deleted file mode 100644 index 4b7a38bff..000000000 --- a/src/main/deploy/autos/paths/LT-Neutral.json +++ /dev/null @@ -1,103 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.013, - "y_meters": 7.597, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": -3.14159, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 4.91446, - "y_meters": 7.64578, - "intermediate_handoff_radius_meters": 0.5 - }, - { - "type": "rotation", - "rotation_radians": 1.5708, - "t_ratio": 0.33414, - "profiled_rotation": true - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.6318, - "y_meters": 7.08688, - "intermediate_handoff_radius_meters": 1.0 - }, - "rotation_target": { - "rotation_radians": 1.5708, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.68131, - "y_meters": 6.19181, - "intermediate_handoff_radius_meters": 1.0 - }, - "rotation_target": { - "rotation_radians": 1.5708, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.64458, - "y_meters": 4.55334, - "intermediate_handoff_radius_meters": 1.0 - }, - "rotation_target": { - "rotation_radians": 1.5708, - "profiled_rotation": true - } - }, - { - "type": "rotation", - "rotation_radians": 3.14159, - "t_ratio": 0.60552, - "profiled_rotation": true - }, - { - "type": "translation", - "x_meters": 6.97839, - "y_meters": 5.73043, - "intermediate_handoff_radius_meters": 1.5 - }, - { - "type": "translation", - "x_meters": 6.5322, - "y_meters": 6.43391, - "intermediate_handoff_radius_meters": 2.0 - }, - { - "type": "translation", - "x_meters": 5.88193, - "y_meters": 6.7037, - "intermediate_handoff_radius_meters": 2.0 - }, - { - "type": "translation", - "x_meters": 3.6054, - "y_meters": 6.63313, - "intermediate_handoff_radius_meters": 2.0 - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 2.5, - "start_ordinal": 2, - "end_ordinal": 4 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTNeutral.json b/src/main/deploy/autos/paths/Neutral.json similarity index 100% rename from src/main/deploy/autos/paths/RTNeutral.json rename to src/main/deploy/autos/paths/Neutral.json diff --git a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json b/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json deleted file mode 100644 index cc1f0c9bc..000000000 --- a/src/main/deploy/autos/paths/RT-Neutral-ClimbD.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.013, - "y_meters": 7.597, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": 1.5708, - "profiled_rotation": false - } - }, - { - "type": "translation", - "x_meters": 7.13815, - "y_meters": 7.41353, - "intermediate_handoff_radius_meters": 1.75 - }, - { - "type": "translation", - "x_meters": 7.355, - "y_meters": 5.70222, - "intermediate_handoff_radius_meters": 0.4 - }, - { - "type": "translation", - "x_meters": 7.12952, - "y_meters": 6.13837, - "intermediate_handoff_radius_meters": 2.0 - }, - { - "type": "translation", - "x_meters": 6.13462, - "y_meters": 7.31508, - "intermediate_handoff_radius_meters": 1.5 - }, - { - "type": "translation", - "x_meters": 3.26493, - "y_meters": 6.43271, - "intermediate_handoff_radius_meters": 0.2 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTNEUTRALTEST.json b/src/main/deploy/autos/paths/RTNEUTRALTEST.json deleted file mode 100644 index 8864e7b53..000000000 --- a/src/main/deploy/autos/paths/RTNEUTRALTEST.json +++ /dev/null @@ -1,129 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.013, - "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.45101, - "y_meters": 0.58991, - "intermediate_handoff_radius_meters": 0.4 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.72165, - "y_meters": 0.82345, - "intermediate_handoff_radius_meters": 0.4 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "event_trigger", - "t_ratio": 0.45, - "lib_key": "intake" - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.72165, - "y_meters": 0.82345, - "intermediate_handoff_radius_meters": 0.4 - }, - "rotation_target": { - "rotation_radians": -2.35619, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 7.56137, - "y_meters": 1.45936, - "intermediate_handoff_radius_meters": 1.0 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.85945, - "y_meters": 3.05019, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 6.98368, - "y_meters": 1.89518, - "intermediate_handoff_radius_meters": 0.5 - }, - { - "type": "translation", - "x_meters": 6.3106, - "y_meters": 0.60092, - "intermediate_handoff_radius_meters": 1.0 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.55, - "y_meters": 0.47, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - } - ], - "constraints": { - "end_translation_tolerance_meters": 0.05, - "max_velocity_meters_per_sec": [ - { - "value": 3.0, - "start_ordinal": 7, - "end_ordinal": 7 - }, - { - "value": 3.5, - "start_ordinal": 2, - "end_ordinal": 2 - }, - { - "value": 2.5, - "start_ordinal": 3, - "end_ordinal": 3 - }, - { - "value": 2.0, - "start_ordinal": 4, - "end_ordinal": 4 - }, - { - "value": 3.0, - "start_ordinal": 5, - "end_ordinal": 6 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json b/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json deleted file mode 100644 index 37e4c4f92..000000000 --- a/src/main/deploy/autos/paths/RTRBNEUTRALTEST.json +++ /dev/null @@ -1,108 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.55, - "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "event_trigger", - "t_ratio": 0.0, - "lib_key": "launch" - }, - { - "type": "event_trigger", - "t_ratio": 0.8, - "lib_key": "cancel" - }, - { - "type": "translation", - "x_meters": 3.19373, - "y_meters": 2.05349, - "intermediate_handoff_radius_meters": 0.28 - }, - { - "type": "translation", - "x_meters": 4.15303, - "y_meters": 2.36201, - "intermediate_handoff_radius_meters": 0.5 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.28112, - "y_meters": 2.56391, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -2.0944, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.03444, - "y_meters": 3.44342, - "intermediate_handoff_radius_meters": 0.42 - }, - "rotation_target": { - "rotation_radians": -2.0944, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.87959, - "y_meters": 2.53867, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 1.74533, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 2.5, - "start_ordinal": 2, - "end_ordinal": 2 - }, - { - "value": 1.0, - "start_ordinal": 0, - "end_ordinal": 0 - }, - { - "value": 2.5, - "start_ordinal": 3, - "end_ordinal": 3 - }, - { - "value": 2.0, - "start_ordinal": 5, - "end_ordinal": 5 - }, - { - "value": 2.5, - "start_ordinal": 4, - "end_ordinal": 4 - }, - { - "value": 1.0, - "start_ordinal": 1, - "end_ordinal": 1 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/Sample_5.json b/src/main/deploy/autos/paths/Sample_5.json deleted file mode 100644 index 746d51895..000000000 --- a/src/main/deploy/autos/paths/Sample_5.json +++ /dev/null @@ -1,22 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.6, - "y_meters": 4.035, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 0.0, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 6.00643, - "y_meters": 3.80202, - "intermediate_handoff_radius_meters": 0.2 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample3.json b/src/main/deploy/autos/paths/default.json similarity index 58% rename from src/main/deploy/autos/paths/sample3.json rename to src/main/deploy/autos/paths/default.json index 18b80002a..ba18adb09 100644 --- a/src/main/deploy/autos/paths/sample3.json +++ b/src/main/deploy/autos/paths/default.json @@ -3,30 +3,24 @@ { "type": "waypoint", "translation_target": { - "x_meters": 7.8, - "y_meters": 3.5, + "x_meters": 3.6, + "y_meters": 4.035, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -2.0944, + "rotation_radians": 0.0, "profiled_rotation": true } }, - { - "type": "rotation", - "rotation_radians": -1.5708, - "t_ratio": 0.6, - "profiled_rotation": true - }, { "type": "waypoint", "translation_target": { - "x_meters": 6.0, - "y_meters": 0.6, + "x_meters": 2.5, + "y_meters": 4.035, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5708, + "rotation_radians": 0.0, "profiled_rotation": true } } diff --git a/src/main/deploy/autos/paths/example_a.json b/src/main/deploy/autos/paths/example_a.json deleted file mode 100644 index 35a470910..000000000 --- a/src/main/deploy/autos/paths/example_a.json +++ /dev/null @@ -1,34 +0,0 @@ -{ - "path_elements": [ - { - "type": "translation", - "x_meters": 2.0, - "y_meters": 2.0, - "intermediate_handoff_radius_meters": 0.2 - }, - { - "type": "rotation", - "rotation_radians": 0.0, - "t_ratio": 0.5, - "profiled_rotation": true - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.0, - "y_meters": 4.0, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 0.5, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 10.0, - "y_meters": 6.0, - "intermediate_handoff_radius_meters": 0.2 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/example_b.json b/src/main/deploy/autos/paths/example_b.json deleted file mode 100644 index 30a18c7af..000000000 --- a/src/main/deploy/autos/paths/example_b.json +++ /dev/null @@ -1,28 +0,0 @@ -{ - "path_elements": [ - { - "type": "translation", - "x_meters": 1.0, - "y_meters": 7.5, - "intermediate_handoff_radius_meters": 0.2 - }, - { - "type": "translation", - "x_meters": 5.0, - "y_meters": 6.0, - "intermediate_handoff_radius_meters": 0.2 - }, - { - "type": "rotation", - "rotation_radians": 1.2, - "t_ratio": 0.5, - "profiled_rotation": true - }, - { - "type": "translation", - "x_meters": 12.5, - "y_meters": 3.0, - "intermediate_handoff_radius_meters": 0.2 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample2.json b/src/main/deploy/autos/paths/sample2.json deleted file mode 100644 index 35035a425..000000000 --- a/src/main/deploy/autos/paths/sample2.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.75, - "y_meters": 3.55, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -2.35619, - "profiled_rotation": true - } - }, - { - "type": "rotation", - "rotation_radians": -1.5708, - "t_ratio": 0.7, - "profiled_rotation": true - }, - { - "type": "translation", - "x_meters": 7.84344, - "y_meters": 1.9666, - "intermediate_handoff_radius_meters": 1.0 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.02386, - "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/sample4.json b/src/main/deploy/autos/paths/sample4.json deleted file mode 100644 index d92330544..000000000 --- a/src/main/deploy/autos/paths/sample4.json +++ /dev/null @@ -1,22 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.0, - "y_meters": 0.6, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 3.3, - "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.2 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/test1.json b/src/main/deploy/autos/paths/test1.json deleted file mode 100644 index 81012eb5f..000000000 --- a/src/main/deploy/autos/paths/test1.json +++ /dev/null @@ -1,112 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 4.013, - "y_meters": 7.597, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 1.5708, - "profiled_rotation": true - } - }, - { - "type": "event_trigger", - "t_ratio": 0.75, - "lib_key": "intake" - }, - { - "type": "rotation", - "rotation_radians": 2.26893, - "t_ratio": 1.0, - "profiled_rotation": true - }, - { - "type": "translation", - "x_meters": 5.79215, - "y_meters": 7.43474, - "intermediate_handoff_radius_meters": 0.85 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.82555, - "y_meters": 6.56508, - "intermediate_handoff_radius_meters": 0.8 - }, - "rotation_target": { - "rotation_radians": 2.26893, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.83757, - "y_meters": 5.69813, - "intermediate_handoff_radius_meters": 0.6 - }, - "rotation_target": { - "rotation_radians": 2.26893, - "profiled_rotation": true - } - }, - { - "type": "rotation", - "rotation_radians": 1.5708, - "t_ratio": 0.7, - "profiled_rotation": true - }, - { - "type": "translation", - "x_meters": 7.77343, - "y_meters": 4.48196, - "intermediate_handoff_radius_meters": 0.35 - }, - { - "type": "translation", - "x_meters": 6.60971, - "y_meters": 7.13388, - "intermediate_handoff_radius_meters": 0.4 - }, - { - "type": "event_trigger", - "t_ratio": 0.95, - "lib_key": "launch" - }, - { - "type": "translation", - "x_meters": 3.77872, - "y_meters": 7.51783, - "intermediate_handoff_radius_meters": 1.25 - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 2.0, - "start_ordinal": 3, - "end_ordinal": 3 - }, - { - "value": 2.0, - "start_ordinal": 4, - "end_ordinal": 4 - }, - { - "value": 2.0, - "start_ordinal": 5, - "end_ordinal": 6 - } - ], - "max_acceleration_meters_per_sec2": [ - { - "value": 1.75, - "start_ordinal": 4, - "end_ordinal": 4 - } - ] - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index aba15e2ef..5a8fdb995 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Controls; import frc.robot.Robot; @@ -33,10 +34,14 @@ public class BLineLogic { public static Field2d field = new Field2d(); public static Field2d fieldPoseStart = new Field2d(); + private static final Pose2d RIGHT_TRENCH_POSE = + new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90)); + private static final Pose2d LEFT_TRENCH_POSE = + new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90)); + public enum StartPosition { - LEFT_TRENCH("Left Trench", new Pose2d(4.013, 7.597, Rotation2d.fromDegrees(90))), + TRENCH("Trench", new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90))), CENTER("Center", new Pose2d(3.600, 4.035, Rotation2d.fromDegrees(0))), - RIGHT_TRENCH("Right Trench", new Pose2d(4.013, 0.473, Rotation2d.fromDegrees(-90))), MISC("Misc", new Pose2d()); public final String title; @@ -48,13 +53,22 @@ public enum StartPosition { } } - private static final List autos = new ArrayList<>(); + public enum TrenchSide { + RIGHT("Right"), + LEFT("Left"); + + public final String title; + + TrenchSide(String title) { + this.title = title; + } + } + private static final List autos = new ArrayList<>(); private static final SendableChooser startPositionChooser = new SendableChooser<>(); - + private static final SendableChooser trenchSideChooser = new SendableChooser<>(); private static final DynamicSendableChooser autoChooser = new DynamicSendableChooser<>(); - private static final SendableChooser gameObjects = new SendableChooser<>(); private static final NetworkTableEntry autoDelayEntry = @@ -69,25 +83,35 @@ public enum StartPosition { private static boolean pathsInitialized = false; private static Command bLineLaunching; + private static Command bLineSimLaunching; + public static FollowPath.Builder pathBuilder; private static FollowPath.Builder continuingPathBuilder; + // ========================= MIRRORING ========================= + + public static boolean isMirrored() { + return startPositionChooser.getSelected() == StartPosition.TRENCH + && trenchSideChooser.getSelected() == TrenchSide.LEFT; + } + // ========================= INIT ========================= public static void init(Subsystems subsystems) { - s = subsystems; registerCommands(); if (pathsInitialized) return; - defaultPath = new BLinePath("test1", "test1", "LT"); + + defaultPath = new BLinePath("default", "Center", "default"); rebuiltPaths = List.of( defaultPath, - new BLinePath("RTNeutral", "RTNeutral", "RT"), - new BLinePath("RTRBNEUTRAL", "RT", "RTNeutral", "RTRBNEUTRAL")); + new BLinePath("Neutral", "RT", "Neutral"), + new BLinePath("TESTING", "RT", "Neutral", "BUMPNEUTRAL")); + autos.clear(); autos.addAll(rebuiltPaths); @@ -107,16 +131,12 @@ public static void init(Subsystems subsystems) { public static void handleStartingPoses(BLinePath path) { switch (path.getStartingPosName()) { case "RT": - path.setStartPose2d(StartPosition.RIGHT_TRENCH); - break; case "LT": - path.setStartPose2d(StartPosition.LEFT_TRENCH); + path.setStartPose2d(StartPosition.TRENCH); break; - case "Center": path.setStartPose2d(StartPosition.CENTER); break; - default: path.setStartPose2d(StartPosition.MISC); break; @@ -124,7 +144,6 @@ public static void handleStartingPoses(BLinePath path) { } public static void configure(Subsystems s) { - pathBuilder = createPathBuilder(s).withPoseReset(pose -> s.drivebaseSubsystem.resetPose(pose)); continuingPathBuilder = createPathBuilder(s); } @@ -141,24 +160,28 @@ private static FollowPath.Builder createPathBuilder(Subsystems s) { new PIDController(3.0, 0.0, 0.0), new PIDController(5.0, 0.0, 0.0), new PIDController(2.0, 0.0, 0.0)) - .withDefaultShouldFlip(); + .withDefaultShouldFlip() + .withShouldMirror(BLineLogic::isMirrored); } // ========================= LOGGING ========================= public static void initSmartDashboard() { - startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); - for (StartPosition pos : StartPosition.values()) { startPositionChooser.addOption(pos.title, pos); } - gameObjects.setDefaultOption("0", 0); + trenchSideChooser.setDefaultOption(TrenchSide.RIGHT.title, TrenchSide.RIGHT); + trenchSideChooser.addOption(TrenchSide.LEFT.title, TrenchSide.LEFT); + gameObjects.setDefaultOption("0", 0); filterAutos(0); + SmartDashboard.putData("Selected auto", field); + SmartDashboard.putData("Start pose", fieldPoseStart); SmartDashboard.putData("Starting Position", startPositionChooser); + SmartDashboard.putData("Trench Side", trenchSideChooser); SmartDashboard.putData("Auto Mode", gameObjects); SmartDashboard.putData("Available Auto Variants", autoChooser); SmartDashboard.putString("Auto Key", keys); @@ -169,50 +192,45 @@ public static void initSmartDashboard() { v -> { filterAutos(gameObjects.getSelected()); updateInitialHeading(); - refreshFieldDisplay(); + updateFieldDisplay(); + }); + + trenchSideChooser.onChange( + v -> { + updateInitialHeading(); + updateFieldDisplay(); }); autoChooser.onChange( v -> { updateInitialHeading(); - refreshFieldDisplay(); + updateFieldDisplay(); }); - refreshFieldDisplay(); + updateFieldDisplay(); } - public static void refreshFieldDisplay() { - BLinePath selected = getSelectedAutoPath(); - if (selected == null) return; - + public static void updateFieldDisplay() { fieldPoseStart.setRobotPose(getSelectedAutoStartingPose()); - SmartDashboard.putData("Selected auto", field); - SmartDashboard.putData("Start pose", fieldPoseStart); } - // ========================= AUTOS FILTERING ========================= + static Pose2d getTrenchPose() { + return isMirrored() ? LEFT_TRENCH_POSE : RIGHT_TRENCH_POSE; + } public static void filterAutos(int numGameObjects) { - autoChooser.clearOptions(); StartPosition selected = startPositionChooser.getSelected(); - - // fallback safety - if (selected == null) { - selected = StartPosition.MISC; - } + if (selected == null) selected = StartPosition.MISC; for (BLinePath auto : autos) { - if (selected == StartPosition.MISC) { autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); + continue; } - Pose2d autoPose = auto.getStartPose2d(); - - if (autoPose != null && selected.startPose != null && autoPose.equals(selected.startPose)) { - + if (auto.getStartPositionType() == selected) { autoChooser.addOption(auto.getDisplayName(), auto.getDisplayName()); } } @@ -225,52 +243,45 @@ public static String getSelectedAutoName() { } public static BLinePath getSelectedAutoPath() { - String selectedName = autoChooser.getSelected(); - - if (selectedName == null) { - return defaultPath; - } - + if (selectedName == null) return defaultPath; return namesToAuto.getOrDefault(selectedName, defaultPath); } public static Pose2d getSelectedAutoStartingPose() { - BLinePath selected = getSelectedAutoPath(); - if (selected == null) return Pose2d.kZero; + if (startPositionChooser.getSelected() == StartPosition.TRENCH) { + return isMirrored() ? LEFT_TRENCH_POSE : RIGHT_TRENCH_POSE; + } + return selected.getStartPose2d(); } // ========================= AUTO EXECUTION ========================= public static Command getSelectedAuto() { - double delay = autoDelayEntry.getDouble(0.0); - BLinePath path = getSelectedAutoPath(); s.drivebaseSubsystem.resetRotation(path.getPath().getInitialModuleDirection()); - return Commands.waitSeconds(delay).andThen(buildPath(path.getPath(), true)); } - public static Command buildRTNeutralAuto() { + public static Command buildNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildPath(new Path("RTNeutral"), true), + buildPath(new Path("Neutral"), true), launcherCommand(4), - buildPath(new Path("RTNeutral"), false)); + buildPath(new Path("Neutral"), false)); } - public static Command buildRTNeutralTestingAuto() { - + public static Command buildNeutralTestingAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildPath(new Path("RTNeutral"), true), - launcherCommand(4), - buildPath(new Path("RTRBNEUTRAL"), false), + buildPath(new Path("Neutral"), true), + launcherCommand(4), + buildPath(new Path("BUMPNEUTRAL"), false), launcherCommand()); } @@ -280,63 +291,50 @@ private static Command buildPath(Path path, boolean resetPose) { public static Command handleAutos() { switch (getSelectedAutoName()) { - case "RTNeutral": - return buildRTNeutralAuto(); - - case "RTRBNEUTRAL": - return buildRTNeutralTestingAuto(); - + case "Neutral": + return buildNeutralAuto(); + case "BUMPNEUTRAL": + return buildNeutralTestingAuto(); default: return pathBuilder.build(defaultPath.getPath()); } } private static void updateInitialHeading() { - BLinePath selected = getSelectedAutoPath(); - - - + if (selected == null || selected.getPath() == null) { + SmartDashboard.putNumber("Initial Heading(Deg)", 0.0); + return; + } Pose2d start = selected.getPath().getStartPose(); - SmartDashboard.putNumber("Initial Heading(Deg)", Math.round(start.getRotation().getDegrees())); } + // ========================= COMMANDS ========================= + public static Command intakeCommand() { return Commands.runOnce(() -> Controls.intakeMode = IntakeMode.INTAKE) .withName("Auto Intake Command"); } public static Command launcherCommand(double timeout) { - if (Robot.isSimulation()) { - return RobotSim.launch(s, timeout); - } + if (Robot.isSimulation()) return RobotSim.launch(s, timeout); return Commands.parallel( - Commands.runOnce( - () -> { - s.flywheels.resetFuelCheck(); - }), + Commands.runOnce(() -> s.flywheels.resetFuelCheck()), s.launcherSubsystem.launcherAimCommand(), Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) - // .until(() -> s.flywheels.isOutOfFuel()) .withTimeout(timeout) .andThen(s.launcherSubsystem.rawStowCommand()) .withName("Auto Launcher Command"); } public static Command launcherCommand() { - return Commands.parallel( - Commands.runOnce( - () -> { - s.flywheels.resetFuelCheck(); - }), + Commands.runOnce(() -> s.flywheels.resetFuelCheck()), s.launcherSubsystem.launcherAimCommand(), Commands.waitUntil(() -> s.launcherSubsystem.isAtTarget()) .andThen(s.indexerSubsystem.runIndexer(() -> s.flywheels.getTargetSpeed()))) - // .until(() -> s.flywheels.isOutOfFuel()) - .withName("Auto Launcher Command"); } @@ -348,21 +346,24 @@ public static Command climbCommand() { return Commands.none().withName("Auto Climb Command"); } + public static void cancelCommand() { + if (Robot.isSimulation()) { + CommandScheduler.getInstance().cancel(bLineSimLaunching); + } else { + CommandScheduler.getInstance().cancel(bLineLaunching); + } + } + private static void registerCommands() { AtomicBoolean launchAllowed = new AtomicBoolean(true); - if (s.launcherSubsystem != null && s.indexerSubsystem != null) { + if (s.launcherSubsystem != null && s.indexerSubsystem != null) { if (Robot.isSimulation()) { - bLineLaunching = RobotSim.launch(s, 30); + bLineSimLaunching = RobotSim.launch(s, 30); FollowPath.registerEventTrigger( "launch", Commands.runOnce(() -> launchAllowed.set(true)) - .andThen( - RobotSim.launch(s, 30) - .onlyWhile( - () -> { - return launchAllowed.get(); - })) + .andThen(bLineSimLaunching.onlyWhile(launchAllowed::get)) .andThen(Commands.print("LAUNCH FINISHED"))); } else { bLineLaunching = launcherCommand(); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index f5ef98891..354de1645 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -11,17 +11,12 @@ public class BLinePath { private final String displayName; private final String startingPosName; private final boolean vision; - private final Path path; private final List allPaths; - private Pose2d startPose; - // Single path constructor - public BLinePath(String displayName, String autoName, String startingPosName) { - this(displayName, startingPosName, false, autoName); - } + private StartPosition positionType; + private Pose2d startPose; - // Multi-path constructor public BLinePath(String displayName, String startingPosName, String... pathNames) { this(displayName, startingPosName, false, pathNames); } @@ -30,7 +25,6 @@ public BLinePath( String displayName, String startingPosName, boolean vision, String... pathNames) { this.displayName = displayName; this.startingPosName = startingPosName; - this.vision = vision; List loaded = new ArrayList<>(); @@ -51,12 +45,16 @@ public String getStartingPosName() { } public Pose2d getStartPose2d() { + if (positionType == StartPosition.TRENCH) { + return BLineLogic.getTrenchPose(); + } return startPose; } public Pose2d setStartPose2d(StartPosition pos) { - startPose = pos.startPose; - return startPose; + this.positionType = pos; + this.startPose = pos.startPose; + return getStartPose2d(); } public boolean isVision() { @@ -70,4 +68,8 @@ public Path getPath() { public List getAllPaths() { return allPaths; } + + public StartPosition getStartPositionType() { + return positionType; + } } From a820424889f771f1d93dae44f4e191ff5fcf72c7 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 5 Jun 2026 10:41:54 -0700 Subject: [PATCH 074/114] new auto --- C2026-Public | 1 + .../autos/paths/TrenchDoubleNeutral.json | 130 ++++++++++++++++++ 2 files changed, 131 insertions(+) create mode 160000 C2026-Public create mode 100644 src/main/deploy/autos/paths/TrenchDoubleNeutral.json diff --git a/C2026-Public b/C2026-Public new file mode 160000 index 000000000..0f403dd8f --- /dev/null +++ b/C2026-Public @@ -0,0 +1 @@ +Subproject commit 0f403dd8f3ad1f7d0dbe0c1bd23b24c210f41677 diff --git a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json new file mode 100644 index 000000000..39b932aa0 --- /dev/null +++ b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json @@ -0,0 +1,130 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 7.59, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.67447, + "y_meters": 7.50044, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 5.73632, + "y_meters": 6.0623, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.92377, + "y_meters": 5.02285, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": 1.74533, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": -2.35619, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.43559, + "y_meters": 4.42625, + "intermediate_handoff_radius_meters": 0.38 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.81706, + "y_meters": 5.88785, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.68959, + "y_meters": 6.81078, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01264, + "y_meters": 7.2893, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.09, + "max_velocity_meters_per_sec": [ + { + "value": 2.7, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 2.25, + "start_ordinal": 5, + "end_ordinal": 5 + } + ] + } +} \ No newline at end of file From b9edff1990fea2676576862fa728f44e93ab27de Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 5 Jun 2026 10:42:25 -0700 Subject: [PATCH 075/114] new auto --- .../autos/paths/TrenchDoubleNeutral.json | 130 ++++++++++++++++++ 1 file changed, 130 insertions(+) create mode 100644 src/main/deploy/autos/paths/TrenchDoubleNeutral.json diff --git a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json new file mode 100644 index 000000000..39b932aa0 --- /dev/null +++ b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json @@ -0,0 +1,130 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 7.59, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.67447, + "y_meters": 7.50044, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 5.73632, + "y_meters": 6.0623, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.92377, + "y_meters": 5.02285, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": 1.74533, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": -2.35619, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.43559, + "y_meters": 4.42625, + "intermediate_handoff_radius_meters": 0.38 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.81706, + "y_meters": 5.88785, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.68959, + "y_meters": 6.81078, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01264, + "y_meters": 7.2893, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.09, + "max_velocity_meters_per_sec": [ + { + "value": 2.7, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 2.25, + "start_ordinal": 5, + "end_ordinal": 5 + } + ] + } +} \ No newline at end of file From c50ccd432665830bc7135ca1fe1d89a6bcb57c14 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Fri, 5 Jun 2026 13:03:50 -0700 Subject: [PATCH 076/114] removed citrus repo --- C2026-Public | 1 - 1 file changed, 1 deletion(-) delete mode 160000 C2026-Public diff --git a/C2026-Public b/C2026-Public deleted file mode 160000 index 0f403dd8f..000000000 --- a/C2026-Public +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0f403dd8f3ad1f7d0dbe0c1bd23b24c210f41677 From 860dacf16bba5592b2520db6d6833ab0cb0188df Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 6 Jun 2026 17:07:33 -0700 Subject: [PATCH 077/114] succesfully fixed double dip path --- src/main/deploy/autos/config.json | 5 +- .../autos/paths/TrenchDoubleNeutral.json | 50 ++++++++----------- 2 files changed, 21 insertions(+), 34 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 362f5d147..1aaddd378 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -22,9 +22,6 @@ "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0, - "default_auto_velocity_velocity_safety_factor": 0.9, - "default_auto_velocity_acceleration_safety_factor": 0.8, - "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 + "default_end_rotation_tolerance_deg": 2.0 } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json index 39b932aa0..e531d6181 100644 --- a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json +++ b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json @@ -4,11 +4,11 @@ "type": "waypoint", "translation_target": { "x_meters": 4.01, - "y_meters": 7.59, + "y_meters": 0.473, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": 1.5708, + "rotation_radians": -1.5707963267948966, "profiled_rotation": true } }, @@ -20,36 +20,36 @@ { "type": "waypoint", "translation_target": { - "x_meters": 5.67447, - "y_meters": 7.50044, + "x_meters": 5.721925696594428, + "y_meters": 0.5055820433436562, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": 1.5708, + "rotation_radians": -1.5707963267948966, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 5.73632, - "y_meters": 6.0623, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 5.725459318885447, + "y_meters": 2.2979504643962834, + "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 5.92377, - "y_meters": 5.02285, + "x_meters": 6.075194460431656, + "y_meters": 3.2018992805755397, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": 1.74533, + "rotation_radians": -1.7453292519943295, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": -2.35619, + "rotation_radians": 2.356194490192345, "t_ratio": 1.0, "profiled_rotation": true }, @@ -57,11 +57,11 @@ "type": "waypoint", "translation_target": { "x_meters": 7.43559, - "y_meters": 4.42625, + "y_meters": 3.8, "intermediate_handoff_radius_meters": 0.38 }, "rotation_target": { - "rotation_radians": -2.35619, + "rotation_radians": 2.356194490192345, "profiled_rotation": true } }, @@ -69,11 +69,11 @@ "type": "waypoint", "translation_target": { "x_meters": 7.81706, - "y_meters": 5.88785, + "y_meters": 2.8, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -1.5708, + "rotation_radians": 1.5707963267948966, "profiled_rotation": true } }, @@ -81,11 +81,11 @@ "type": "waypoint", "translation_target": { "x_meters": 6.68959, - "y_meters": 6.81078, + "y_meters": 0.8, "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": -1.5708, + "rotation_radians": 1.5707963267948966, "profiled_rotation": true } }, @@ -93,11 +93,11 @@ "type": "waypoint", "translation_target": { "x_meters": 4.01264, - "y_meters": 7.2893, + "y_meters": 0.8, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": -1.5708, + "rotation_radians": 1.5707963267948966, "profiled_rotation": true } } @@ -113,16 +113,6 @@ { "value": 2.0, "start_ordinal": 2, - "end_ordinal": 3 - }, - { - "value": 2.0, - "start_ordinal": 4, - "end_ordinal": 4 - }, - { - "value": 2.25, - "start_ordinal": 5, "end_ordinal": 5 } ] From a1d358f860a9db7368584c8bc8c011aff470dbe7 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sat, 6 Jun 2026 17:13:00 -0700 Subject: [PATCH 078/114] fixed for real --- .../autos/paths/TrenchDoubleNeutral.json | 49 ++++++++++++------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json index e531d6181..17d463191 100644 --- a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json +++ b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json @@ -8,7 +8,7 @@ "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, @@ -20,36 +20,36 @@ { "type": "waypoint", "translation_target": { - "x_meters": 5.721925696594428, - "y_meters": 0.5055820433436562, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 5.76429, + "y_meters": 0.6251, + "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 5.725459318885447, - "y_meters": 2.2979504643962834, + "x_meters": 5.72546, + "y_meters": 2.29795, "intermediate_handoff_radius_meters": 0.5 }, { "type": "waypoint", "translation_target": { - "x_meters": 6.075194460431656, - "y_meters": 3.2018992805755397, + "x_meters": 6.07519, + "y_meters": 3.2019, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": -1.7453292519943295, + "rotation_radians": -1.74533, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": 2.356194490192345, + "rotation_radians": 2.35619, "t_ratio": 1.0, "profiled_rotation": true }, @@ -61,7 +61,7 @@ "intermediate_handoff_radius_meters": 0.38 }, "rotation_target": { - "rotation_radians": 2.356194490192345, + "rotation_radians": 2.35619, "profiled_rotation": true } }, @@ -73,7 +73,7 @@ "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } }, @@ -85,7 +85,7 @@ "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } }, @@ -93,11 +93,11 @@ "type": "waypoint", "translation_target": { "x_meters": 4.01264, - "y_meters": 0.8, + "y_meters": 0.68, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { - "rotation_radians": 1.5707963267948966, + "rotation_radians": 1.5708, "profiled_rotation": true } } @@ -105,6 +105,11 @@ "constraints": { "end_translation_tolerance_meters": 0.09, "max_velocity_meters_per_sec": [ + { + "value": 2.25, + "start_ordinal": 0, + "end_ordinal": 0 + }, { "value": 2.7, "start_ordinal": 1, @@ -114,7 +119,17 @@ "value": 2.0, "start_ordinal": 2, "end_ordinal": 5 + }, + { + "value": 3.36, + "start_ordinal": 6, + "end_ordinal": 6 + }, + { + "value": 4.05, + "start_ordinal": 7, + "end_ordinal": 7 } ] } -} \ No newline at end of file +} From 5a9304206b620e06123cebc2de8067d30dce4b27 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 15:31:57 -0700 Subject: [PATCH 079/114] cleanup --- src/main/deploy/autos/config.json | 7 ++- .../deploy/autos/paths/RTNeutralFAST.json | 18 +++--- .../autos/paths/TrenchDoubleNeutral.json | 63 +++++++------------ .../subsystems/auto/BLine/BLinePath.java | 8 +-- 4 files changed, 41 insertions(+), 55 deletions(-) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 1aaddd378..d0b33f5d6 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -8,7 +8,7 @@ "enabled": true, "distance_meters": 0.207, "side": "back", - "default_state": "hidden", + "default_state": "shown", "show_on_event_keys": [ "intake" ], @@ -22,6 +22,9 @@ "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0 + "default_end_rotation_tolerance_deg": 2.0, + "default_auto_velocity_velocity_safety_factor": 0.9, + "default_auto_velocity_acceleration_safety_factor": 0.8, + "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTNeutralFAST.json b/src/main/deploy/autos/paths/RTNeutralFAST.json index 8a62f600d..5c622b7a1 100644 --- a/src/main/deploy/autos/paths/RTNeutralFAST.json +++ b/src/main/deploy/autos/paths/RTNeutralFAST.json @@ -43,38 +43,38 @@ }, { "type": "translation", - "x_meters": 7.421343333333331, - "y_meters": 1.4356900000000028, + "x_meters": 7.42134, + "y_meters": 1.43569, "intermediate_handoff_radius_meters": 0.4 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.800416666666669, - "y_meters": 3.386593333333334, + "x_meters": 7.80042, + "y_meters": 3.38659, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, { "type": "rotation", - "rotation_radians": -1.5707963267948966, + "rotation_radians": -1.5708, "t_ratio": 0.8, "profiled_rotation": true }, { "type": "translation", "x_meters": 7.19068, - "y_meters": 2.311913333333335, + "y_meters": 2.31191, "intermediate_handoff_radius_meters": 0.4 }, { "type": "translation", - "x_meters": 6.182533333333334, - "y_meters": 0.999740000000001, + "x_meters": 6.18253, + "y_meters": 0.99974, "intermediate_handoff_radius_meters": 0.5 }, { diff --git a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json index 17d463191..3cf07f737 100644 --- a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json +++ b/src/main/deploy/autos/paths/TrenchDoubleNeutral.json @@ -20,9 +20,9 @@ { "type": "waypoint", "translation_target": { - "x_meters": 5.76429, - "y_meters": 0.6251, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 5.5, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { "rotation_radians": -1.5708, @@ -31,15 +31,15 @@ }, { "type": "translation", - "x_meters": 5.72546, - "y_meters": 2.29795, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 5.80138, + "y_meters": 2.24561, + "intermediate_handoff_radius_meters": 0.44 }, { "type": "waypoint", "translation_target": { - "x_meters": 6.07519, - "y_meters": 3.2019, + "x_meters": 6.1368, + "y_meters": 3.10806, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { @@ -54,23 +54,17 @@ "profiled_rotation": true }, { - "type": "waypoint", - "translation_target": { - "x_meters": 7.43559, - "y_meters": 3.8, - "intermediate_handoff_radius_meters": 0.38 - }, - "rotation_target": { - "rotation_radians": 2.35619, - "profiled_rotation": true - } + "type": "translation", + "x_meters": 7.453, + "y_meters": 3.57527, + "intermediate_handoff_radius_meters": 0.38 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.81706, - "y_meters": 2.8, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 7.80082, + "y_meters": 2.54808, + "intermediate_handoff_radius_meters": 0.38 }, "rotation_target": { "rotation_radians": 1.5708, @@ -78,22 +72,16 @@ } }, { - "type": "waypoint", - "translation_target": { - "x_meters": 6.68959, - "y_meters": 0.8, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": 1.5708, - "profiled_rotation": true - } + "type": "translation", + "x_meters": 7.13219, + "y_meters": 1.58945, + "intermediate_handoff_radius_meters": 0.4 }, { "type": "waypoint", "translation_target": { "x_meters": 4.01264, - "y_meters": 0.68, + "y_meters": 0.71, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { @@ -111,7 +99,7 @@ "end_ordinal": 0 }, { - "value": 2.7, + "value": 2.8, "start_ordinal": 1, "end_ordinal": 1 }, @@ -121,15 +109,10 @@ "end_ordinal": 5 }, { - "value": 3.36, - "start_ordinal": 6, - "end_ordinal": 6 - }, - { - "value": 4.05, + "value": 4.0, "start_ordinal": 7, "end_ordinal": 7 } ] } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index 354de1645..d728d8899 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -17,18 +17,18 @@ public class BLinePath { private StartPosition positionType; private Pose2d startPose; - public BLinePath(String displayName, String startingPosName, String... pathNames) { - this(displayName, startingPosName, false, pathNames); + public BLinePath(String displayName, String startingPosName, String... displayedPathNames) { + this(displayName, startingPosName, false, displayedPathNames); } public BLinePath( - String displayName, String startingPosName, boolean vision, String... pathNames) { + String displayName, String startingPosName, boolean vision, String... displayedPathNames) { this.displayName = displayName; this.startingPosName = startingPosName; this.vision = vision; List loaded = new ArrayList<>(); - for (String name : pathNames) { + for (String name : displayedPathNames) { loaded.add(new Path(name)); } this.allPaths = List.copyOf(loaded); From 3509225f35cb6db81ee3bce9aa18bd8fa763a194 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 16:08:29 -0700 Subject: [PATCH 080/114] better naming conventions --- .../{Neutral.json => FirstNeutralTrench.json} | 2 +- ...lFAST.json => FirstNeutralTrenchFAST.json} | 0 .../deploy/autos/paths/SecondNeutralBump.json | 118 ++++++++++++++++++ ...eNeutral.json => SecondNeutralTrench.json} | 8 +- .../subsystems/auto/BLine/BLineLogic.java | 25 ++-- 5 files changed, 135 insertions(+), 18 deletions(-) rename src/main/deploy/autos/paths/{Neutral.json => FirstNeutralTrench.json} (99%) rename src/main/deploy/autos/paths/{RTNeutralFAST.json => FirstNeutralTrenchFAST.json} (100%) create mode 100644 src/main/deploy/autos/paths/SecondNeutralBump.json rename src/main/deploy/autos/paths/{TrenchDoubleNeutral.json => SecondNeutralTrench.json} (95%) diff --git a/src/main/deploy/autos/paths/Neutral.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json similarity index 99% rename from src/main/deploy/autos/paths/Neutral.json rename to src/main/deploy/autos/paths/FirstNeutralTrench.json index c93a94a96..900676ab4 100644 --- a/src/main/deploy/autos/paths/Neutral.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -131,4 +131,4 @@ } ] } -} +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/RTNeutralFAST.json b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json similarity index 100% rename from src/main/deploy/autos/paths/RTNeutralFAST.json rename to src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json new file mode 100644 index 000000000..379c8c5c7 --- /dev/null +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -0,0 +1,118 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 1.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.5, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 5.75212, + "y_meters": 2.00744, + "intermediate_handoff_radius_meters": 0.39 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.9775, + "y_meters": 3.17868, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.74533, + "profiled_rotation": true + } + }, + { + "type": "rotation", + "rotation_radians": 2.35619, + "t_ratio": 1.0, + "profiled_rotation": true + }, + { + "type": "translation", + "x_meters": 7.54972, + "y_meters": 3.3878, + "intermediate_handoff_radius_meters": 0.36 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.74965, + "y_meters": 2.41665, + "intermediate_handoff_radius_meters": 0.48 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 6.49127, + "y_meters": 2.73181, + "intermediate_handoff_radius_meters": 0.7 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.37991, + "y_meters": 2.48256, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.09, + "max_velocity_meters_per_sec": [ + { + "value": 2.25, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 2.8, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 2.0, + "start_ordinal": 2, + "end_ordinal": 5 + }, + { + "value": 3.5, + "start_ordinal": 6, + "end_ordinal": 6 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json b/src/main/deploy/autos/paths/SecondNeutralTrench.json similarity index 95% rename from src/main/deploy/autos/paths/TrenchDoubleNeutral.json rename to src/main/deploy/autos/paths/SecondNeutralTrench.json index 3cf07f737..bde593654 100644 --- a/src/main/deploy/autos/paths/TrenchDoubleNeutral.json +++ b/src/main/deploy/autos/paths/SecondNeutralTrench.json @@ -73,9 +73,9 @@ }, { "type": "translation", - "x_meters": 7.13219, - "y_meters": 1.58945, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 7.03813, + "y_meters": 1.44552, + "intermediate_handoff_radius_meters": 0.2 }, { "type": "waypoint", @@ -110,7 +110,7 @@ }, { "value": 4.0, - "start_ordinal": 7, + "start_ordinal": 6, "end_ordinal": 7 } ] diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 5a8fdb995..15551166d 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -109,8 +109,8 @@ public static void init(Subsystems subsystems) { rebuiltPaths = List.of( defaultPath, - new BLinePath("Neutral", "RT", "Neutral"), - new BLinePath("TESTING", "RT", "Neutral", "BUMPNEUTRAL")); + new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), + new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench")); autos.clear(); autos.addAll(rebuiltPaths); @@ -268,20 +268,19 @@ public static Command getSelectedAuto() { return Commands.waitSeconds(delay).andThen(buildPath(path.getPath(), true)); } - public static Command buildNeutralAuto() { + public static Command buildSingleNeutralAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildPath(new Path("Neutral"), true), - launcherCommand(4), - buildPath(new Path("Neutral"), false)); + buildPath(new Path("FirstNeutralTrench"), true), + launcherCommand()); } - public static Command buildNeutralTestingAuto() { + public static Command buildDoubleNeutralAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), - buildPath(new Path("Neutral"), true), + buildPath(new Path("FirstNeutralTrench"), true), launcherCommand(4), - buildPath(new Path("BUMPNEUTRAL"), false), + buildPath(new Path("SecondNeutralTrench"), false), launcherCommand()); } @@ -291,10 +290,10 @@ private static Command buildPath(Path path, boolean resetPose) { public static Command handleAutos() { switch (getSelectedAutoName()) { - case "Neutral": - return buildNeutralAuto(); - case "BUMPNEUTRAL": - return buildNeutralTestingAuto(); + case "TrenchNeutral": + return buildSingleNeutralAuto(); + case "DoubleTrenchNeutral": + return buildDoubleNeutralAuto(); default: return pathBuilder.build(defaultPath.getPath()); } From 1d3a33a2d74d498b08c4f4811b0e28a7d1e7f754 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 16:14:29 -0700 Subject: [PATCH 081/114] added OP paths --- .../deploy/autos/paths/FirstNeutralBump.json | 134 ++++++++++++++++++ src/main/deploy/autos/paths/bumptotrench.json | 70 +++++++++ 2 files changed, 204 insertions(+) create mode 100644 src/main/deploy/autos/paths/FirstNeutralBump.json create mode 100644 src/main/deploy/autos/paths/bumptotrench.json diff --git a/src/main/deploy/autos/paths/FirstNeutralBump.json b/src/main/deploy/autos/paths/FirstNeutralBump.json new file mode 100644 index 000000000..c93a94a96 --- /dev/null +++ b/src/main/deploy/autos/paths/FirstNeutralBump.json @@ -0,0 +1,134 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.013, + "y_meters": 0.473, + "intermediate_handoff_radius_meters": 0.5 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 5.45101, + "y_meters": 0.58991, + "intermediate_handoff_radius_meters": 0.4 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "intake" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 6.72165, + "y_meters": 0.82345, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -2.35619, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.62013, + "y_meters": 1.40061, + "intermediate_handoff_radius_meters": 0.3 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 7.74195, + "y_meters": 3.09426, + "intermediate_handoff_radius_meters": 0.3 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 7.10746, + "y_meters": 2.04773, + "intermediate_handoff_radius_meters": 0.3 + }, + { + "type": "translation", + "x_meters": 6.69956, + "y_meters": 1.2829, + "intermediate_handoff_radius_meters": 0.35 + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.11942, + "y_meters": 0.47062, + "intermediate_handoff_radius_meters": 0.25 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 2.5, + "start_ordinal": 7, + "end_ordinal": 7 + }, + { + "value": 2.5, + "start_ordinal": 2, + "end_ordinal": 2 + }, + { + "value": 2.5, + "start_ordinal": 3, + "end_ordinal": 3 + }, + { + "value": 2.0, + "start_ordinal": 4, + "end_ordinal": 4 + }, + { + "value": 2.5, + "start_ordinal": 5, + "end_ordinal": 6 + } + ], + "max_acceleration_meters_per_sec2": [ + { + "value": 2.5, + "start_ordinal": 5, + "end_ordinal": 5 + }, + { + "value": 2.5, + "start_ordinal": 6, + "end_ordinal": 6 + }, + { + "value": 3.0, + "start_ordinal": 7, + "end_ordinal": 7 + } + ] + } +} diff --git a/src/main/deploy/autos/paths/bumptotrench.json b/src/main/deploy/autos/paths/bumptotrench.json new file mode 100644 index 000000000..c088897ff --- /dev/null +++ b/src/main/deploy/autos/paths/bumptotrench.json @@ -0,0 +1,70 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.38, + "y_meters": 2.48, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.0, + "lib_key": "launch" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.96984, + "y_meters": 1.43785, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + }, + { + "type": "translation", + "x_meters": 2.9767, + "y_meters": 0.76884, + "intermediate_handoff_radius_meters": 0.4 + }, + { + "type": "event_trigger", + "t_ratio": 0.48, + "lib_key": "cancel" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 4.01, + "y_meters": 0.47, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": -1.5708, + "profiled_rotation": true + } + } + ], + "constraints": { + "max_velocity_meters_per_sec": [ + { + "value": 1.6, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 1.6, + "start_ordinal": 1, + "end_ordinal": 1 + } + ] + } +} \ No newline at end of file From 4450f0890dc6c3445ec63368e80a80128b604c83 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 16:24:22 -0700 Subject: [PATCH 082/114] more bump paths --- .../autos/.bline-web/path-metadata.json | 81 +++++++++++++++++++ src/main/deploy/autos/config.json | 2 +- .../deploy/autos/paths/FirstNeutralBump.json | 57 ++++++------- .../deploy/autos/paths/SecondNeutralBump.json | 30 ++++--- 4 files changed, 126 insertions(+), 44 deletions(-) create mode 100644 src/main/deploy/autos/.bline-web/path-metadata.json diff --git a/src/main/deploy/autos/.bline-web/path-metadata.json b/src/main/deploy/autos/.bline-web/path-metadata.json new file mode 100644 index 000000000..e26a179c1 --- /dev/null +++ b/src/main/deploy/autos/.bline-web/path-metadata.json @@ -0,0 +1,81 @@ +{ + "paths": { + "FirstNeutralBump.json": { + "editor_metadata": { + "ranged_constraints": [ + { + "key": "max_velocity_meters_per_sec", + "value": 4.05, + "start_ordinal": 2, + "end_ordinal": 2, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 1.37, + "start_ordinal": 6, + "end_ordinal": 6, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 3.27, + "start_ordinal": 7, + "end_ordinal": 7, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 4.05, + "start_ordinal": 8, + "end_ordinal": 8, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + } + ] + } + }, + "SecondNeutralBump.json": { + "editor_metadata": { + "ranged_constraints": [ + { + "key": "max_velocity_meters_per_sec", + "value": 1.82, + "start_ordinal": 6, + "end_ordinal": 6, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.5,\"yMeters\":0.47,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.458898792535486,\"yMeters\":3.704650345923196,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.640235632238253,\"yMeters\":2.5258424606681427,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5707963267948966,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.466055517062335,\"yMeters\":2.529046591458746,\"handoffRadiusMeters\":0.7},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + } + ] + } + } + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index d0b33f5d6..362f5d147 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -8,7 +8,7 @@ "enabled": true, "distance_meters": 0.207, "side": "back", - "default_state": "shown", + "default_state": "hidden", "show_on_event_keys": [ "intake" ], diff --git a/src/main/deploy/autos/paths/FirstNeutralBump.json b/src/main/deploy/autos/paths/FirstNeutralBump.json index c93a94a96..ccee7c969 100644 --- a/src/main/deploy/autos/paths/FirstNeutralBump.json +++ b/src/main/deploy/autos/paths/FirstNeutralBump.json @@ -16,7 +16,7 @@ "type": "waypoint", "translation_target": { "x_meters": 5.45101, - "y_meters": 0.58991, + "y_meters": 0.47, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { @@ -32,8 +32,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 6.72165, - "y_meters": 0.82345, + "x_meters": 6.57376, + "y_meters": 0.82149, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { @@ -50,9 +50,9 @@ { "type": "waypoint", "translation_target": { - "x_meters": 7.74195, - "y_meters": 3.09426, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 7.77981, + "y_meters": 2.76138, + "intermediate_handoff_radius_meters": 0.6 }, "rotation_target": { "rotation_radians": -1.5708, @@ -61,21 +61,21 @@ }, { "type": "translation", - "x_meters": 7.10746, - "y_meters": 2.04773, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 6.95844, + "y_meters": 2.62345, + "intermediate_handoff_radius_meters": 0.6 }, { "type": "translation", - "x_meters": 6.69956, - "y_meters": 1.2829, + "x_meters": 5.61403, + "y_meters": 2.50023, "intermediate_handoff_radius_meters": 0.35 }, { "type": "waypoint", "translation_target": { - "x_meters": 4.11942, - "y_meters": 0.47062, + "x_meters": 3.38, + "y_meters": 2.28, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { @@ -88,18 +88,13 @@ "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { - "value": 2.5, - "start_ordinal": 7, - "end_ordinal": 7 + "value": 4.05, + "start_ordinal": 1, + "end_ordinal": 1 }, { "value": 2.5, "start_ordinal": 2, - "end_ordinal": 2 - }, - { - "value": 2.5, - "start_ordinal": 3, "end_ordinal": 3 }, { @@ -108,22 +103,22 @@ "end_ordinal": 4 }, { - "value": 2.5, - "start_ordinal": 5, - "end_ordinal": 6 - } - ], - "max_acceleration_meters_per_sec2": [ - { - "value": 2.5, + "value": 1.37, "start_ordinal": 5, "end_ordinal": 5 }, { - "value": 2.5, + "value": 3.27, "start_ordinal": 6, "end_ordinal": 6 }, + { + "value": 4.05, + "start_ordinal": 7, + "end_ordinal": 7 + } + ], + "max_acceleration_meters_per_sec2": [ { "value": 3.0, "start_ordinal": 7, @@ -131,4 +126,4 @@ } ] } -} +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json index 379c8c5c7..bf9d15d62 100644 --- a/src/main/deploy/autos/paths/SecondNeutralBump.json +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -55,16 +55,16 @@ }, { "type": "translation", - "x_meters": 7.54972, - "y_meters": 3.3878, + "x_meters": 7.4589, + "y_meters": 3.70465, "intermediate_handoff_radius_meters": 0.36 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.74965, - "y_meters": 2.41665, - "intermediate_handoff_radius_meters": 0.48 + "x_meters": 7.64024, + "y_meters": 2.52584, + "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { "rotation_radians": 1.5708, @@ -73,10 +73,16 @@ }, { "type": "translation", - "x_meters": 6.49127, - "y_meters": 2.73181, + "x_meters": 6.46606, + "y_meters": 2.52905, "intermediate_handoff_radius_meters": 0.7 }, + { + "type": "translation", + "x_meters": 5.4365, + "y_meters": 2.61687, + "intermediate_handoff_radius_meters": 0.2 + }, { "type": "waypoint", "translation_target": { @@ -93,11 +99,6 @@ "constraints": { "end_translation_tolerance_meters": 0.09, "max_velocity_meters_per_sec": [ - { - "value": 2.25, - "start_ordinal": 0, - "end_ordinal": 0 - }, { "value": 2.8, "start_ordinal": 1, @@ -106,6 +107,11 @@ { "value": 2.0, "start_ordinal": 2, + "end_ordinal": 4 + }, + { + "value": 1.82, + "start_ordinal": 5, "end_ordinal": 5 }, { From 7b8296b34606d56626856520648f11c22fe80dce Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 16:33:12 -0700 Subject: [PATCH 083/114] expanded autos --- src/main/deploy/autos/config.json | 23 ++--- src/main/deploy/autos/paths/BUMPNEUTRAL.json | 93 ------------------- src/main/deploy/autos/paths/config.json | 30 ++++++ .../deploy/autos/{ => paths}/pathgroups.json | 2 +- .../subsystems/auto/BLine/BLineLogic.java | 34 +++++-- 5 files changed, 68 insertions(+), 114 deletions(-) delete mode 100644 src/main/deploy/autos/paths/BUMPNEUTRAL.json create mode 100644 src/main/deploy/autos/paths/config.json rename src/main/deploy/autos/{ => paths}/pathgroups.json (95%) diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 362f5d147..5f53bb000 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -1,30 +1,25 @@ { "gui": { "robot": { - "length_meters": 0.78, - "width_meters": 0.94 + "length_meters": 0.8, + "width_meters": 0.8 }, "protrusions": { - "enabled": true, - "distance_meters": 0.207, - "side": "back", - "default_state": "hidden", - "show_on_event_keys": [ - "intake" - ], + "enabled": false, + "distance_meters": 0.0, + "side": "none", + "default_state": "", + "show_on_event_keys": [], "hide_on_event_keys": [] } }, "kinematic_constraints": { "default_max_velocity_meters_per_sec": 4.5, - "default_max_acceleration_meters_per_sec2": 6.0, + "default_max_acceleration_meters_per_sec2": 12.0, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0, - "default_auto_velocity_velocity_safety_factor": 0.9, - "default_auto_velocity_acceleration_safety_factor": 0.8, - "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 + "default_end_rotation_tolerance_deg": 2.0 } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/BUMPNEUTRAL.json b/src/main/deploy/autos/paths/BUMPNEUTRAL.json deleted file mode 100644 index 70d4c025a..000000000 --- a/src/main/deploy/autos/paths/BUMPNEUTRAL.json +++ /dev/null @@ -1,93 +0,0 @@ -{ - "path_elements": [ - { - "type": "waypoint", - "translation_target": { - "x_meters": 3.55, - "y_meters": 0.473, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": -1.5708, - "profiled_rotation": true - } - }, - { - "type": "translation", - "x_meters": 2.92399, - "y_meters": 1.42822, - "intermediate_handoff_radius_meters": 0.6 - }, - { - "type": "translation", - "x_meters": 4.30445, - "y_meters": 2.28629, - "intermediate_handoff_radius_meters": 0.5 - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 5.33159, - "y_meters": 2.46296, - "intermediate_handoff_radius_meters": 0.5 - }, - "rotation_target": { - "rotation_radians": -2.0944, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 6.03444, - "y_meters": 3.44342, - "intermediate_handoff_radius_meters": 0.42 - }, - "rotation_target": { - "rotation_radians": -2.0944, - "profiled_rotation": true - } - }, - { - "type": "waypoint", - "translation_target": { - "x_meters": 7.87959, - "y_meters": 2.53867, - "intermediate_handoff_radius_meters": 0.2 - }, - "rotation_target": { - "rotation_radians": 1.74533, - "profiled_rotation": true - } - } - ], - "constraints": { - "max_velocity_meters_per_sec": [ - { - "value": 3.5, - "start_ordinal": 2, - "end_ordinal": 2 - }, - { - "value": 1.0, - "start_ordinal": 0, - "end_ordinal": 0 - }, - { - "value": 2.5, - "start_ordinal": 3, - "end_ordinal": 3 - }, - { - "value": 2.0, - "start_ordinal": 5, - "end_ordinal": 5 - }, - { - "value": 2.5, - "start_ordinal": 4, - "end_ordinal": 4 - } - ] - } -} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/config.json b/src/main/deploy/autos/paths/config.json new file mode 100644 index 000000000..362f5d147 --- /dev/null +++ b/src/main/deploy/autos/paths/config.json @@ -0,0 +1,30 @@ +{ + "gui": { + "robot": { + "length_meters": 0.78, + "width_meters": 0.94 + }, + "protrusions": { + "enabled": true, + "distance_meters": 0.207, + "side": "back", + "default_state": "hidden", + "show_on_event_keys": [ + "intake" + ], + "hide_on_event_keys": [] + } + }, + "kinematic_constraints": { + "default_max_velocity_meters_per_sec": 4.5, + "default_max_acceleration_meters_per_sec2": 6.0, + "default_intermediate_handoff_radius_meters": 0.2, + "default_max_velocity_deg_per_sec": 720.0, + "default_max_acceleration_deg_per_sec2": 1500.0, + "default_end_translation_tolerance_meters": 0.03, + "default_end_rotation_tolerance_deg": 2.0, + "default_auto_velocity_velocity_safety_factor": 0.9, + "default_auto_velocity_acceleration_safety_factor": 0.8, + "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/pathgroups.json b/src/main/deploy/autos/paths/pathgroups.json similarity index 95% rename from src/main/deploy/autos/pathgroups.json rename to src/main/deploy/autos/paths/pathgroups.json index 065ac85d0..f7fc60a41 100644 --- a/src/main/deploy/autos/pathgroups.json +++ b/src/main/deploy/autos/paths/pathgroups.json @@ -1,4 +1,4 @@ { "schema_version": 1, "groups": [] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 15551166d..3835cdcb0 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -110,7 +110,10 @@ public static void init(Subsystems subsystems) { List.of( defaultPath, new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), - new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench")); + new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), + new BLinePath("BumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench"), + new BLinePath("DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump")); + autos.clear(); autos.addAll(rebuiltPaths); @@ -268,21 +271,36 @@ public static Command getSelectedAuto() { return Commands.waitSeconds(delay).andThen(buildPath(path.getPath(), true)); } - public static Command buildSingleNeutralAuto() { + public static Command buildSingleNeutralTrenchAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("FirstNeutralTrench"), true), launcherCommand()); } - public static Command buildDoubleNeutralAuto() { + public static Command buildDoubleNeutralTrenchAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("FirstNeutralTrench"), true), - launcherCommand(4), + launcherCommand(4.5), buildPath(new Path("SecondNeutralTrench"), false), launcherCommand()); } + public static Command buildSingleNeutralBumpAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralBump"), true), + launcherCommand()); + } + + public static Command buildDoubleNeutralBumpAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralBump"), true), + buildPath(new Path("BumpToTrench"), false), + buildPath(new Path("SecondNeutralBump"), false), + launcherCommand()); + } private static Command buildPath(Path path, boolean resetPose) { return (resetPose ? pathBuilder : continuingPathBuilder).build(path); @@ -291,9 +309,13 @@ private static Command buildPath(Path path, boolean resetPose) { public static Command handleAutos() { switch (getSelectedAutoName()) { case "TrenchNeutral": - return buildSingleNeutralAuto(); + return buildSingleNeutralTrenchAuto(); case "DoubleTrenchNeutral": - return buildDoubleNeutralAuto(); + return buildDoubleNeutralTrenchAuto(); + case "BumpNeutral": + return buildSingleNeutralBumpAuto(); + case "DoubleBumpNeutral": + return buildDoubleNeutralBumpAuto(); default: return pathBuilder.build(defaultPath.getPath()); } From 78a0e725b2adf13d222ab451d5eaa486845a69ef Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 16:34:15 -0700 Subject: [PATCH 084/114] added folders for organization --- src/main/deploy/autos/config.json | 5 +++- src/main/deploy/autos/pathgroups.json | 22 ++++++++++++++++ src/main/deploy/autos/paths/config.json | 29 +-------------------- src/main/deploy/autos/paths/pathgroups.json | 5 ++-- 4 files changed, 29 insertions(+), 32 deletions(-) create mode 100644 src/main/deploy/autos/pathgroups.json diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 5f53bb000..2d9955283 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -20,6 +20,9 @@ "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0 + "default_end_rotation_tolerance_deg": 2.0, + "default_auto_velocity_velocity_safety_factor": 0.9, + "default_auto_velocity_acceleration_safety_factor": 0.8, + "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 } } \ No newline at end of file diff --git a/src/main/deploy/autos/pathgroups.json b/src/main/deploy/autos/pathgroups.json new file mode 100644 index 000000000..cfdc77d16 --- /dev/null +++ b/src/main/deploy/autos/pathgroups.json @@ -0,0 +1,22 @@ +{ + "schema_version": 1, + "groups": [ + { + "group_id": "group-be48ead2-6d4e-4a08-9787-7ec4fcb2553e", + "display_name": "Trench Paths", + "path_file_names": [ + "FirstNeutralTrench.json", + "FirstNeutralTrenchFAST.json", + "SecondNeutralTrench.json" + ] + }, + { + "group_id": "group-24f6ec6b-f994-4a13-86c1-eaa4d5454b32", + "display_name": "Bump Paths", + "path_file_names": [ + "FirstNeutralBump.json", + "SecondNeutralBump.json" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/config.json b/src/main/deploy/autos/paths/config.json index 362f5d147..f0b6f1bf5 100644 --- a/src/main/deploy/autos/paths/config.json +++ b/src/main/deploy/autos/paths/config.json @@ -1,30 +1,3 @@ { - "gui": { - "robot": { - "length_meters": 0.78, - "width_meters": 0.94 - }, - "protrusions": { - "enabled": true, - "distance_meters": 0.207, - "side": "back", - "default_state": "hidden", - "show_on_event_keys": [ - "intake" - ], - "hide_on_event_keys": [] - } - }, - "kinematic_constraints": { - "default_max_velocity_meters_per_sec": 4.5, - "default_max_acceleration_meters_per_sec2": 6.0, - "default_intermediate_handoff_radius_meters": 0.2, - "default_max_velocity_deg_per_sec": 720.0, - "default_max_acceleration_deg_per_sec2": 1500.0, - "default_end_translation_tolerance_meters": 0.03, - "default_end_rotation_tolerance_deg": 2.0, - "default_auto_velocity_velocity_safety_factor": 0.9, - "default_auto_velocity_acceleration_safety_factor": 0.8, - "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 - } + "path_elements": [] } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/pathgroups.json b/src/main/deploy/autos/paths/pathgroups.json index f7fc60a41..f0b6f1bf5 100644 --- a/src/main/deploy/autos/paths/pathgroups.json +++ b/src/main/deploy/autos/paths/pathgroups.json @@ -1,4 +1,3 @@ { - "schema_version": 1, - "groups": [] -} + "path_elements": [] +} \ No newline at end of file From cf97ddc4158744336f7b12cc930bfb37c78bac0d Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 16:44:53 -0700 Subject: [PATCH 085/114] fixed path previews --- src/main/deploy/bline-layout.json | 76 ++++++------------- .../subsystems/auto/BLine/BLineLogic.java | 2 +- 2 files changed, 24 insertions(+), 54 deletions(-) diff --git a/src/main/deploy/bline-layout.json b/src/main/deploy/bline-layout.json index d03d84477..6ddeeb295 100644 --- a/src/main/deploy/bline-layout.json +++ b/src/main/deploy/bline-layout.json @@ -44,7 +44,7 @@ "x": 0.0, "y": 0.0, "width": 1024.0, - "height": 896.0, + "height": 512.0, "type": "Field", "properties": { "topic": "/SmartDashboard/Selected auto", @@ -63,9 +63,9 @@ { "title": "Start pose", "x": 1024.0, - "y": 384.0, - "width": 768.0, - "height": 512.0, + "y": 0.0, + "width": 640.0, + "height": 384.0, "type": "Field", "properties": { "topic": "/SmartDashboard/Start pose", @@ -83,48 +83,20 @@ }, { "title": "Starting Position", - "x": 1408.0, - "y": 128.0, - "width": 384.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Starting Position", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Auto Key", - "x": 1408.0, - "y": 0.0, - "width": 512.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Auto Key", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Est. Time (s)", "x": 1152.0, - "y": 128.0, - "width": 128.0, + "y": 384.0, + "width": 256.0, "height": 128.0, - "type": "Large Text Display", + "type": "Split Button Chooser", "properties": { - "topic": "/SmartDashboard/Est. Time (s)", - "period": 0.06, - "data_type": "double" + "topic": "/SmartDashboard/Starting Position", + "period": 0.06 } }, { "title": "Available Auto Variants", - "x": 1024.0, - "y": 256.0, + "x": 256.0, + "y": 512.0, "width": 768.0, "height": 128.0, "type": "Split Button Chooser", @@ -135,8 +107,8 @@ }, { "title": "Auto Delay", - "x": 1280.0, - "y": 128.0, + "x": 1024.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -149,9 +121,9 @@ }, { "title": "DisplaySpeed", - "x": 1024.0, - "y": 0.0, - "width": 384.0, + "x": 0.0, + "y": 512.0, + "width": 256.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -165,17 +137,15 @@ } }, { - "title": "Initial Heading", - "x": 1024.0, - "y": 128.0, - "width": 128.0, + "title": "Trench Side", + "x": 1408.0, + "y": 384.0, + "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "Split Button Chooser", "properties": { - "topic": "/SmartDashboard/Initial Heading", - "period": 0.06, - "data_type": "double", - "show_submit_button": true + "topic": "/SmartDashboard/Trench Side", + "period": 0.06 } } ] diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 3835cdcb0..f725050be 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -111,7 +111,7 @@ public static void init(Subsystems subsystems) { defaultPath, new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), - new BLinePath("BumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench"), + new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), new BLinePath("DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump")); From 5b7d7f619c7ed6e57a7d85c07daa1bcdf2bf9919 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 17:00:12 -0700 Subject: [PATCH 086/114] fixed paths --- .../autos/.bline-web/path-metadata.json | 13 -------- src/main/deploy/autos/config.json | 16 +++++---- .../deploy/autos/paths/FirstNeutralBump.json | 23 +++++-------- .../autos/paths/FirstNeutralTrenchFAST.json | 33 ++++--------------- .../deploy/autos/paths/SecondNeutralBump.json | 2 +- .../autos/paths/SecondNeutralTrench.json | 12 +++---- 6 files changed, 31 insertions(+), 68 deletions(-) diff --git a/src/main/deploy/autos/.bline-web/path-metadata.json b/src/main/deploy/autos/.bline-web/path-metadata.json index e26a179c1..775d4b236 100644 --- a/src/main/deploy/autos/.bline-web/path-metadata.json +++ b/src/main/deploy/autos/.bline-web/path-metadata.json @@ -29,19 +29,6 @@ "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" } }, - { - "key": "max_velocity_meters_per_sec", - "value": 3.27, - "start_ordinal": 7, - "end_ordinal": 7, - "source": "auto_velocity", - "auto_velocity": { - "velocity_safety_factor": 0.9, - "acceleration_safety_factor": 0.8, - "merge_tolerance_meters_per_sec": 0.3, - "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.013,\"yMeters\":0.473,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"waypoint\",\"xMeters\":5.45101,\"yMeters\":0.58991,\"handoffRadiusMeters\":0.4,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":0},{\"type\":\"waypoint\",\"xMeters\":6.573755148587652,\"yMeters\":0.8214867605289937,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-2.35619,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.62013,\"yMeters\":1.40061,\"handoffRadiusMeters\":0.3},{\"type\":\"waypoint\",\"xMeters\":7.779809454259738,\"yMeters\":2.76137685856349,\"handoffRadiusMeters\":0.6,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.958438065219933,\"yMeters\":2.6234522215077574,\"handoffRadiusMeters\":0.6},{\"type\":\"translation\",\"xMeters\":5.614032533314306,\"yMeters\":2.5002326599980744,\"handoffRadiusMeters\":0.35},{\"type\":\"waypoint\",\"xMeters\":3.38,\"yMeters\":2.28,\"handoffRadiusMeters\":0.25,\"rotationRadians\":-1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" - } - }, { "key": "max_velocity_meters_per_sec", "value": 4.05, diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index 2d9955283..fffa75030 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -1,15 +1,17 @@ { "gui": { "robot": { - "length_meters": 0.8, - "width_meters": 0.8 + "length_meters": 0.78, + "width_meters": 0.93 }, "protrusions": { - "enabled": false, - "distance_meters": 0.0, - "side": "none", - "default_state": "", - "show_on_event_keys": [], + "enabled": true, + "distance_meters": 0.21, + "side": "back", + "default_state": "shown", + "show_on_event_keys": [ + "intake" + ], "hide_on_event_keys": [] } }, diff --git a/src/main/deploy/autos/paths/FirstNeutralBump.json b/src/main/deploy/autos/paths/FirstNeutralBump.json index ccee7c969..1d60cdc60 100644 --- a/src/main/deploy/autos/paths/FirstNeutralBump.json +++ b/src/main/deploy/autos/paths/FirstNeutralBump.json @@ -50,9 +50,9 @@ { "type": "waypoint", "translation_target": { - "x_meters": 7.77981, - "y_meters": 2.76138, - "intermediate_handoff_radius_meters": 0.6 + "x_meters": 7.80486, + "y_meters": 3.31046, + "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { "rotation_radians": -1.5708, @@ -61,21 +61,21 @@ }, { "type": "translation", - "x_meters": 6.95844, - "y_meters": 2.62345, + "x_meters": 6.93776, + "y_meters": 2.87846, "intermediate_handoff_radius_meters": 0.6 }, { "type": "translation", - "x_meters": 5.61403, - "y_meters": 2.50023, + "x_meters": 5.64549, + "y_meters": 2.65731, "intermediate_handoff_radius_meters": 0.35 }, { "type": "waypoint", "translation_target": { - "x_meters": 3.38, - "y_meters": 2.28, + "x_meters": 3.39036, + "y_meters": 2.47649, "intermediate_handoff_radius_meters": 0.25 }, "rotation_target": { @@ -107,11 +107,6 @@ "start_ordinal": 5, "end_ordinal": 5 }, - { - "value": 3.27, - "start_ordinal": 6, - "end_ordinal": 6 - }, { "value": 4.05, "start_ordinal": 7, diff --git a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json index 5c622b7a1..26ebad9c1 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json @@ -16,7 +16,7 @@ "type": "waypoint", "translation_target": { "x_meters": 5.45101, - "y_meters": 0.58991, + "y_meters": 0.47, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { @@ -43,8 +43,8 @@ }, { "type": "translation", - "x_meters": 7.42134, - "y_meters": 1.43569, + "x_meters": 7.46186, + "y_meters": 1.4098, "intermediate_handoff_radius_meters": 0.4 }, { @@ -55,7 +55,7 @@ "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { - "rotation_radians": -2.35619, + "rotation_radians": -1.5708, "profiled_rotation": true } }, @@ -73,8 +73,8 @@ }, { "type": "translation", - "x_meters": 6.18253, - "y_meters": 0.99974, + "x_meters": 6.85896, + "y_meters": 0.93211, "intermediate_handoff_radius_meters": 0.5 }, { @@ -91,33 +91,12 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { "value": 3.0, "start_ordinal": 7, "end_ordinal": 7 }, - { - "value": 3.0, - "start_ordinal": 0, - "end_ordinal": 0 - }, - { - "value": 3.0, - "start_ordinal": 1, - "end_ordinal": 1 - }, - { - "value": 4.5, - "start_ordinal": 2, - "end_ordinal": 2 - }, - { - "value": 3.0, - "start_ordinal": 3, - "end_ordinal": 3 - }, { "value": 2.0, "start_ordinal": 4, diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json index bf9d15d62..9996937e9 100644 --- a/src/main/deploy/autos/paths/SecondNeutralBump.json +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -20,7 +20,7 @@ { "type": "waypoint", "translation_target": { - "x_meters": 5.5, + "x_meters": 5.76, "y_meters": 0.47, "intermediate_handoff_radius_meters": 0.3 }, diff --git a/src/main/deploy/autos/paths/SecondNeutralTrench.json b/src/main/deploy/autos/paths/SecondNeutralTrench.json index bde593654..37c26b5be 100644 --- a/src/main/deploy/autos/paths/SecondNeutralTrench.json +++ b/src/main/deploy/autos/paths/SecondNeutralTrench.json @@ -20,8 +20,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 5.5, - "y_meters": 0.47, + "x_meters": 5.72169, + "y_meters": 0.51571, "intermediate_handoff_radius_meters": 0.3 }, "rotation_target": { @@ -73,9 +73,9 @@ }, { "type": "translation", - "x_meters": 7.03813, - "y_meters": 1.44552, - "intermediate_handoff_radius_meters": 0.2 + "x_meters": 7.03744, + "y_meters": 1.07133, + "intermediate_handoff_radius_meters": 0.4 }, { "type": "waypoint", @@ -111,7 +111,7 @@ { "value": 4.0, "start_ordinal": 6, - "end_ordinal": 7 + "end_ordinal": 6 } ] } From 6008c512c5dc0ba9aa1789ac45a1420280b41c6e Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 17:03:42 -0700 Subject: [PATCH 087/114] fixed incorrect accerlation values --- .../autos/.bline-web/path-metadata.json | 43 ++++++++++++++++++- src/main/deploy/autos/config.json | 6 +-- .../deploy/autos/paths/FirstNeutralBump.json | 6 +-- .../deploy/autos/paths/SecondNeutralBump.json | 25 ++++++++--- .../autos/paths/SecondNeutralTrench.json | 8 ++-- 5 files changed, 71 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/autos/.bline-web/path-metadata.json b/src/main/deploy/autos/.bline-web/path-metadata.json index 775d4b236..b0c467427 100644 --- a/src/main/deploy/autos/.bline-web/path-metadata.json +++ b/src/main/deploy/autos/.bline-web/path-metadata.json @@ -50,7 +50,20 @@ "ranged_constraints": [ { "key": "max_velocity_meters_per_sec", - "value": 1.82, + "value": 2.25, + "start_ordinal": 1, + "end_ordinal": 1, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 1.41, "start_ordinal": 6, "end_ordinal": 6, "source": "auto_velocity", @@ -58,7 +71,33 @@ "velocity_safety_factor": 0.9, "acceleration_safety_factor": 0.8, "merge_tolerance_meters_per_sec": 0.3, - "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.5,\"yMeters\":0.47,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.458898792535486,\"yMeters\":3.704650345923196,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.640235632238253,\"yMeters\":2.5258424606681427,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5707963267948966,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.466055517062335,\"yMeters\":2.529046591458746,\"handoffRadiusMeters\":0.7},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":0.9,\"autoVelocityAccelerationSafetyFactor\":0.8},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 3.34, + "start_ordinal": 8, + "end_ordinal": 8, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" + } + }, + { + "key": "max_velocity_meters_per_sec", + "value": 4.05, + "start_ordinal": 9, + "end_ordinal": 9, + "source": "auto_velocity", + "auto_velocity": { + "velocity_safety_factor": 0.9, + "acceleration_safety_factor": 0.8, + "merge_tolerance_meters_per_sec": 0.3, + "input_signature": "{\"pathElements\":[{\"type\":\"waypoint\",\"xMeters\":4.01,\"yMeters\":0.473,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"event_trigger\",\"tRatio\":1},{\"type\":\"waypoint\",\"xMeters\":5.691302974503924,\"yMeters\":0.6304414045306999,\"handoffRadiusMeters\":0.5,\"rotationRadians\":-1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":5.75212,\"yMeters\":2.00744,\"handoffRadiusMeters\":0.39},{\"type\":\"waypoint\",\"xMeters\":5.9775,\"yMeters\":3.17868,\"handoffRadiusMeters\":0.3,\"rotationRadians\":-1.74533,\"profiledRotation\":true},{\"type\":\"rotation\",\"rotationRadians\":2.35619,\"tRatio\":1,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":7.4589,\"yMeters\":3.70465,\"handoffRadiusMeters\":0.36},{\"type\":\"waypoint\",\"xMeters\":7.64024,\"yMeters\":2.52584,\"handoffRadiusMeters\":0.4,\"rotationRadians\":1.5708,\"profiledRotation\":true},{\"type\":\"translation\",\"xMeters\":6.46606,\"yMeters\":2.52905,\"handoffRadiusMeters\":0.7},{\"type\":\"translation\",\"xMeters\":5.4365,\"yMeters\":2.61687,\"handoffRadiusMeters\":0.4},{\"type\":\"waypoint\",\"xMeters\":3.37991,\"yMeters\":2.48256,\"handoffRadiusMeters\":0.2,\"rotationRadians\":1.5708,\"profiledRotation\":true}],\"scalarConstraints\":{\"maxVelocityMps\":null,\"maxAccelerationMps2\":null,\"maxVelocityDegPerSec\":null,\"maxAccelerationDegPerSec\":null},\"rotationRangedConstraints\":[],\"config\":{\"maxVelocityMps\":4.5,\"maxAccelerationMps2\":6,\"handoffRadiusMeters\":0.2,\"maxVelocityDegPerSec\":720,\"maxAccelerationDegPerSec\":1500,\"autoVelocityVelocitySafetyFactor\":1,\"autoVelocityAccelerationSafetyFactor\":1},\"options\":{\"velocitySafetyFactor\":0.9,\"accelerationSafetyFactor\":0.8,\"sampleStepMeters\":null}}" } } ] diff --git a/src/main/deploy/autos/config.json b/src/main/deploy/autos/config.json index fffa75030..f3a856625 100644 --- a/src/main/deploy/autos/config.json +++ b/src/main/deploy/autos/config.json @@ -17,14 +17,14 @@ }, "kinematic_constraints": { "default_max_velocity_meters_per_sec": 4.5, - "default_max_acceleration_meters_per_sec2": 12.0, + "default_max_acceleration_meters_per_sec2": 6.0, "default_intermediate_handoff_radius_meters": 0.2, "default_max_velocity_deg_per_sec": 720.0, "default_max_acceleration_deg_per_sec2": 1500.0, "default_end_translation_tolerance_meters": 0.03, "default_end_rotation_tolerance_deg": 2.0, - "default_auto_velocity_velocity_safety_factor": 0.9, - "default_auto_velocity_acceleration_safety_factor": 0.8, + "default_auto_velocity_velocity_safety_factor": 1.0, + "default_auto_velocity_acceleration_safety_factor": 1.0, "default_auto_velocity_merge_tolerance_meters_per_sec": 0.3 } } \ No newline at end of file diff --git a/src/main/deploy/autos/paths/FirstNeutralBump.json b/src/main/deploy/autos/paths/FirstNeutralBump.json index 1d60cdc60..b9b8eb506 100644 --- a/src/main/deploy/autos/paths/FirstNeutralBump.json +++ b/src/main/deploy/autos/paths/FirstNeutralBump.json @@ -67,9 +67,9 @@ }, { "type": "translation", - "x_meters": 5.64549, - "y_meters": 2.65731, - "intermediate_handoff_radius_meters": 0.35 + "x_meters": 5.66151, + "y_meters": 2.55139, + "intermediate_handoff_radius_meters": 0.6 }, { "type": "waypoint", diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json index 9996937e9..c13e01ccb 100644 --- a/src/main/deploy/autos/paths/SecondNeutralBump.json +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -20,9 +20,9 @@ { "type": "waypoint", "translation_target": { - "x_meters": 5.76, - "y_meters": 0.47, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 5.6913, + "y_meters": 0.63044, + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { "rotation_radians": -1.5708, @@ -81,7 +81,7 @@ "type": "translation", "x_meters": 5.4365, "y_meters": 2.61687, - "intermediate_handoff_radius_meters": 0.2 + "intermediate_handoff_radius_meters": 0.4 }, { "type": "waypoint", @@ -99,6 +99,11 @@ "constraints": { "end_translation_tolerance_meters": 0.09, "max_velocity_meters_per_sec": [ + { + "value": 2.25, + "start_ordinal": 0, + "end_ordinal": 0 + }, { "value": 2.8, "start_ordinal": 1, @@ -110,7 +115,7 @@ "end_ordinal": 4 }, { - "value": 1.82, + "value": 1.41, "start_ordinal": 5, "end_ordinal": 5 }, @@ -118,6 +123,16 @@ "value": 3.5, "start_ordinal": 6, "end_ordinal": 6 + }, + { + "value": 3.34, + "start_ordinal": 7, + "end_ordinal": 7 + }, + { + "value": 4.05, + "start_ordinal": 8, + "end_ordinal": 8 } ] } diff --git a/src/main/deploy/autos/paths/SecondNeutralTrench.json b/src/main/deploy/autos/paths/SecondNeutralTrench.json index 37c26b5be..957f33ec2 100644 --- a/src/main/deploy/autos/paths/SecondNeutralTrench.json +++ b/src/main/deploy/autos/paths/SecondNeutralTrench.json @@ -22,7 +22,7 @@ "translation_target": { "x_meters": 5.72169, "y_meters": 0.51571, - "intermediate_handoff_radius_meters": 0.3 + "intermediate_handoff_radius_meters": 0.6 }, "rotation_target": { "rotation_radians": -1.5708, @@ -73,9 +73,9 @@ }, { "type": "translation", - "x_meters": 7.03744, - "y_meters": 1.07133, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 7.17921, + "y_meters": 1.41252, + "intermediate_handoff_radius_meters": 0.8 }, { "type": "waypoint", From 5f665b1320e9c295814453128eabee503caef2cc Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 17:33:03 -0700 Subject: [PATCH 088/114] depot auto WIP and spotless --- src/main/deploy/autos/paths/BumpDepot.json | 82 +++++++++++++++++++ .../autos/paths/FirstNeutralTrenchFAST.json | 1 + .../deploy/autos/paths/SecondNeutralBump.json | 7 +- .../autos/paths/SecondNeutralTrench.json | 7 +- src/main/deploy/autos/paths/bumptotrench.json | 7 +- .../subsystems/auto/BLine/BLineLogic.java | 24 ++++-- .../frc/robot/util/simulation/RobotSim.java | 6 +- 7 files changed, 110 insertions(+), 24 deletions(-) create mode 100644 src/main/deploy/autos/paths/BumpDepot.json diff --git a/src/main/deploy/autos/paths/BumpDepot.json b/src/main/deploy/autos/paths/BumpDepot.json new file mode 100644 index 000000000..0a146c372 --- /dev/null +++ b/src/main/deploy/autos/paths/BumpDepot.json @@ -0,0 +1,82 @@ +{ + "path_elements": [ + { + "type": "waypoint", + "translation_target": { + "x_meters": 3.37991, + "y_meters": 2.48256, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 1.5708, + "profiled_rotation": true + } + }, + { + "type": "event_trigger", + "t_ratio": 0.09, + "lib_key": "launch" + }, + { + "type": "event_trigger", + "t_ratio": 0.93, + "lib_key": "cancel" + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.91239, + "y_meters": 1.92528, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 0.6668, + "y_meters": 1.77522, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + }, + { + "type": "waypoint", + "translation_target": { + "x_meters": 1.61312, + "y_meters": 1.72334, + "intermediate_handoff_radius_meters": 0.2 + }, + "rotation_target": { + "rotation_radians": 0.0, + "profiled_rotation": true + } + } + ], + "constraints": { + "end_translation_tolerance_meters": 0.12, + "max_velocity_meters_per_sec": [ + { + "value": 1.8, + "start_ordinal": 0, + "end_ordinal": 0 + }, + { + "value": 2.0, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 1.8, + "start_ordinal": 2, + "end_ordinal": 3 + } + ] + } +} \ No newline at end of file diff --git a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json index 26ebad9c1..6019aa99d 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json @@ -91,6 +91,7 @@ } ], "constraints": { + "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { "value": 3.0, diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json index c13e01ccb..cb4ba2f9b 100644 --- a/src/main/deploy/autos/paths/SecondNeutralBump.json +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -12,11 +12,6 @@ "profiled_rotation": true } }, - { - "type": "event_trigger", - "t_ratio": 1.0, - "lib_key": "intake" - }, { "type": "waypoint", "translation_target": { @@ -97,7 +92,7 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.09, + "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { "value": 2.25, diff --git a/src/main/deploy/autos/paths/SecondNeutralTrench.json b/src/main/deploy/autos/paths/SecondNeutralTrench.json index 957f33ec2..06eabcf98 100644 --- a/src/main/deploy/autos/paths/SecondNeutralTrench.json +++ b/src/main/deploy/autos/paths/SecondNeutralTrench.json @@ -12,11 +12,6 @@ "profiled_rotation": true } }, - { - "type": "event_trigger", - "t_ratio": 1.0, - "lib_key": "intake" - }, { "type": "waypoint", "translation_target": { @@ -91,7 +86,7 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.09, + "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { "value": 2.25, diff --git a/src/main/deploy/autos/paths/bumptotrench.json b/src/main/deploy/autos/paths/bumptotrench.json index c088897ff..4739cf3db 100644 --- a/src/main/deploy/autos/paths/bumptotrench.json +++ b/src/main/deploy/autos/paths/bumptotrench.json @@ -3,7 +3,7 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.38, + "x_meters": 3.39, "y_meters": 2.48, "intermediate_handoff_radius_meters": 0.2 }, @@ -14,7 +14,7 @@ }, { "type": "event_trigger", - "t_ratio": 0.0, + "t_ratio": 0.25, "lib_key": "launch" }, { @@ -45,7 +45,7 @@ "translation_target": { "x_meters": 4.01, "y_meters": 0.47, - "intermediate_handoff_radius_meters": 0.2 + "intermediate_handoff_radius_meters": 0.5 }, "rotation_target": { "rotation_radians": -1.5708, @@ -54,6 +54,7 @@ } ], "constraints": { + "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { "value": 1.6, diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index f725050be..96a1cff49 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -111,9 +111,10 @@ public static void init(Subsystems subsystems) { defaultPath, new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), - new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), - new BLinePath("DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump")); - + new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), + new BLinePath( + "DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump"), + new BLinePath("BumpNeutralDepot", "RT", "FirstNeutralBump", "BumpDepot")); autos.clear(); autos.addAll(rebuiltPaths); @@ -286,7 +287,8 @@ public static Command buildDoubleNeutralTrenchAuto() { buildPath(new Path("SecondNeutralTrench"), false), launcherCommand()); } - public static Command buildSingleNeutralBumpAuto() { + + public static Command buildSingleNeutralBumpAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("FirstNeutralBump"), true), @@ -297,11 +299,19 @@ public static Command buildDoubleNeutralBumpAuto() { return BLineCommands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), buildPath(new Path("FirstNeutralBump"), true), - buildPath(new Path("BumpToTrench"), false), + buildPath(new Path("BumpToTrench"), false), buildPath(new Path("SecondNeutralBump"), false), launcherCommand()); } + public static Command buildSingleNeutralBumpDepotAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("FirstNeutralBump"), true), + buildPath(new Path("BumpDepot"), false), + launcherCommand()); + } + private static Command buildPath(Path path, boolean resetPose) { return (resetPose ? pathBuilder : continuingPathBuilder).build(path); } @@ -312,10 +322,12 @@ public static Command handleAutos() { return buildSingleNeutralTrenchAuto(); case "DoubleTrenchNeutral": return buildDoubleNeutralTrenchAuto(); - case "BumpNeutral": + case "BumpNeutral": return buildSingleNeutralBumpAuto(); case "DoubleBumpNeutral": return buildDoubleNeutralBumpAuto(); + case "BumpNeutralDepot": + return buildSingleNeutralBumpDepotAuto(); default: return pathBuilder.build(defaultPath.getPath()); } diff --git a/src/main/java/frc/robot/util/simulation/RobotSim.java b/src/main/java/frc/robot/util/simulation/RobotSim.java index 2796bd099..e3d961a7a 100644 --- a/src/main/java/frc/robot/util/simulation/RobotSim.java +++ b/src/main/java/frc/robot/util/simulation/RobotSim.java @@ -13,8 +13,8 @@ public class RobotSim { FuelSim fuelSim; public static final double UPDATE_S = 0.02; // 20 ms update rate - public static final double SIM_ROBOT_WIDTH_M = 0.8; - public static final double SIM_ROBOT_LENGTH_M = 0.8; + public static final double SIM_ROBOT_WIDTH_M = 0.937; + public static final double SIM_ROBOT_LENGTH_M = 0.781; public static final double SIM_ROBOT_BUMPER_HEIGHT = 0.7; static DoublePublisher scorePublisher; @@ -33,7 +33,7 @@ public RobotSim(CommandSwerveDrivetrain drive) { SIM_ROBOT_BUMPER_HEIGHT, () -> drive.getState().Pose, () -> drive.getState().Speeds); - fuelSim.registerIntake(0.1, 0.2, 0.1, 0.746, () -> true); + fuelSim.registerIntake(0.1, 2, 0.1, 0.746, () -> true); scorePublisher = NetworkTableInstance.getDefault() .getTable("Fuel Simulation") From bffba9e352473e10319c59f6bcdece2f6b8c4199 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 7 Jun 2026 18:27:18 -0700 Subject: [PATCH 089/114] sped up neutral auto --- .../autos/paths/FirstNeutralTrenchFAST.json | 37 +++++++++---------- 1 file changed, 18 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json index 6019aa99d..2d9ba0c7c 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json @@ -32,8 +32,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 6.72165, - "y_meters": 0.82345, + "x_meters": 6.50278, + "y_meters": 0.78978, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { @@ -43,8 +43,8 @@ }, { "type": "translation", - "x_meters": 7.46186, - "y_meters": 1.4098, + "x_meters": 7.3236, + "y_meters": 1.39777, "intermediate_handoff_radius_meters": 0.4 }, { @@ -52,7 +52,7 @@ "translation_target": { "x_meters": 7.80042, "y_meters": 3.38659, - "intermediate_handoff_radius_meters": 0.3 + "intermediate_handoff_radius_meters": 0.35 }, "rotation_target": { "rotation_radians": -1.5708, @@ -67,15 +67,9 @@ }, { "type": "translation", - "x_meters": 7.19068, - "y_meters": 2.31191, - "intermediate_handoff_radius_meters": 0.4 - }, - { - "type": "translation", - "x_meters": 6.85896, - "y_meters": 0.93211, - "intermediate_handoff_radius_meters": 0.5 + "x_meters": 6.78269, + "y_meters": 1.11611, + "intermediate_handoff_radius_meters": 0.32 }, { "type": "waypoint", @@ -95,8 +89,8 @@ "max_velocity_meters_per_sec": [ { "value": 3.0, - "start_ordinal": 7, - "end_ordinal": 7 + "start_ordinal": 6, + "end_ordinal": 6 }, { "value": 2.0, @@ -109,9 +103,14 @@ "end_ordinal": 5 }, { - "value": 4.5, - "start_ordinal": 6, - "end_ordinal": 6 + "value": 4.0, + "start_ordinal": 1, + "end_ordinal": 1 + }, + { + "value": 3.8, + "start_ordinal": 3, + "end_ordinal": 3 } ] } From 549d407161b94e4d5c6d1196523ab34dd2b185a5 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 8 Jun 2026 10:45:07 -0700 Subject: [PATCH 090/114] fixed depot auto --- src/main/deploy/autos/paths/BumpDepot.json | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/autos/paths/BumpDepot.json b/src/main/deploy/autos/paths/BumpDepot.json index 0a146c372..a7b66dfbf 100644 --- a/src/main/deploy/autos/paths/BumpDepot.json +++ b/src/main/deploy/autos/paths/BumpDepot.json @@ -3,8 +3,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 3.37991, - "y_meters": 2.48256, + "x_meters": 3.38, + "y_meters": 2.45, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { @@ -25,8 +25,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 1.91239, - "y_meters": 1.92528, + "x_meters": 1.87864, + "y_meters": 1.92629, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { @@ -37,8 +37,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 0.6668, - "y_meters": 1.77522, + "x_meters": 0.72384, + "y_meters": 2.06724, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { @@ -49,8 +49,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 1.61312, - "y_meters": 1.72334, + "x_meters": 1.59181, + "y_meters": 2.09673, "intermediate_handoff_radius_meters": 0.2 }, "rotation_target": { From 318ac300878f7b8bc3d38eb616803f7188896efe Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 8 Jun 2026 15:31:21 -0700 Subject: [PATCH 091/114] better SOTM --- src/main/deploy/autos/paths/bumptotrench.json | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/autos/paths/bumptotrench.json b/src/main/deploy/autos/paths/bumptotrench.json index 4739cf3db..8b25d3db3 100644 --- a/src/main/deploy/autos/paths/bumptotrench.json +++ b/src/main/deploy/autos/paths/bumptotrench.json @@ -31,9 +31,9 @@ }, { "type": "translation", - "x_meters": 2.9767, - "y_meters": 0.76884, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 3.01332, + "y_meters": 0.68216, + "intermediate_handoff_radius_meters": 0.2 }, { "type": "event_trigger", @@ -54,15 +54,15 @@ } ], "constraints": { - "end_translation_tolerance_meters": 0.12, + "end_translation_tolerance_meters": 0.09, "max_velocity_meters_per_sec": [ { - "value": 1.6, + "value": 1.3, "start_ordinal": 0, "end_ordinal": 0 }, { - "value": 1.6, + "value": 1.3, "start_ordinal": 1, "end_ordinal": 1 } From cca1ac2ab66460e3d7ddaea98981c2e62084d4b0 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 8 Jun 2026 15:44:36 -0700 Subject: [PATCH 092/114] intaking speeds --- src/main/deploy/autos/paths/FirstNeutralTrench.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/autos/paths/FirstNeutralTrench.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json index 900676ab4..91ca88f9a 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrench.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -67,8 +67,8 @@ }, { "type": "translation", - "x_meters": 6.69956, - "y_meters": 1.2829, + "x_meters": 6.6025, + "y_meters": 1.2611, "intermediate_handoff_radius_meters": 0.35 }, { @@ -98,7 +98,7 @@ "end_ordinal": 2 }, { - "value": 2.5, + "value": 2.25, "start_ordinal": 3, "end_ordinal": 3 }, From ca6cf41f94854410b9d205bf2c6b81cfb8a543c9 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 8 Jun 2026 15:53:22 -0700 Subject: [PATCH 093/114] better intaking speeds --- .../autos/paths/FirstNeutralTrench.json | 10 +++++----- .../autos/paths/FirstNeutralTrenchFAST.json | 20 +++++++++---------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/autos/paths/FirstNeutralTrench.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json index 91ca88f9a..a65081af9 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrench.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -50,9 +50,9 @@ { "type": "waypoint", "translation_target": { - "x_meters": 7.74195, - "y_meters": 3.09426, - "intermediate_handoff_radius_meters": 0.3 + "x_meters": 7.80442, + "y_meters": 3.11335, + "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { "rotation_radians": -1.5708, @@ -67,8 +67,8 @@ }, { "type": "translation", - "x_meters": 6.6025, - "y_meters": 1.2611, + "x_meters": 6.71926, + "y_meters": 1.24825, "intermediate_handoff_radius_meters": 0.35 }, { diff --git a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json index 2d9ba0c7c..c80db5b76 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrenchFAST.json @@ -43,8 +43,8 @@ }, { "type": "translation", - "x_meters": 7.3236, - "y_meters": 1.39777, + "x_meters": 7.53163, + "y_meters": 1.30249, "intermediate_handoff_radius_meters": 0.4 }, { @@ -88,29 +88,29 @@ "end_translation_tolerance_meters": 0.12, "max_velocity_meters_per_sec": [ { - "value": 3.0, + "value": 3.15, "start_ordinal": 6, "end_ordinal": 6 }, { - "value": 2.0, - "start_ordinal": 4, + "value": 2.15, + "start_ordinal": 3, "end_ordinal": 4 }, { - "value": 3.0, + "value": 3.22, "start_ordinal": 5, "end_ordinal": 5 }, { - "value": 4.0, + "value": 3.5, "start_ordinal": 1, "end_ordinal": 1 }, { - "value": 3.8, - "start_ordinal": 3, - "end_ordinal": 3 + "value": 3.0, + "start_ordinal": 2, + "end_ordinal": 2 } ] } From bc0039f9fef35e59861d807a7b726ae02e7b7aac Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 8 Jun 2026 16:11:05 -0700 Subject: [PATCH 094/114] hopefully no beach --- .../autos/paths/FirstNeutralTrench.json | 27 ++++++++----------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/src/main/deploy/autos/paths/FirstNeutralTrench.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json index a65081af9..024f8abeb 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrench.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -93,32 +93,22 @@ "end_ordinal": 7 }, { - "value": 2.5, + "value": 1.8, "start_ordinal": 2, - "end_ordinal": 2 - }, - { - "value": 2.25, - "start_ordinal": 3, - "end_ordinal": 3 - }, - { - "value": 2.0, - "start_ordinal": 4, "end_ordinal": 4 }, { "value": 2.5, "start_ordinal": 5, "end_ordinal": 6 + }, + { + "value": 2.6, + "start_ordinal": 1, + "end_ordinal": 1 } ], "max_acceleration_meters_per_sec2": [ - { - "value": 2.5, - "start_ordinal": 5, - "end_ordinal": 5 - }, { "value": 2.5, "start_ordinal": 6, @@ -128,6 +118,11 @@ "value": 3.0, "start_ordinal": 7, "end_ordinal": 7 + }, + { + "value": 3.0, + "start_ordinal": 2, + "end_ordinal": 4 } ] } From 51060100550ee75095b5b3906e38214196f014d7 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 8 Jun 2026 16:12:03 -0700 Subject: [PATCH 095/114] bug fix --- src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index 96a1cff49..f23d35640 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -406,6 +406,6 @@ private static void registerCommands() { FollowPath.registerEventTrigger("intake", intakeCommand()); FollowPath.registerEventTrigger("climb", climbCommand()); - FollowPath.registerEventTrigger("cancel", Commands.runOnce(() -> launchAllowed.set(false))); + FollowPath.registerEventTrigger("cancel", Commands.runOnce(() -> launchAllowed.set(false)).andThen(stowCommand())); } } From dfb98b2de0c07c36cea68392c9e0da75e1ed62f2 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 8 Jun 2026 17:03:17 -0700 Subject: [PATCH 096/114] kinda working now --- .../autos/paths/FirstNeutralTrench.json | 37 +++++++++++-------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/autos/paths/FirstNeutralTrench.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json index 024f8abeb..2efd03138 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrench.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -43,19 +43,19 @@ }, { "type": "translation", - "x_meters": 7.62013, - "y_meters": 1.40061, + "x_meters": 7.39084676942767, + "y_meters": 1.3854781600085788, "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.80442, - "y_meters": 3.11335, - "intermediate_handoff_radius_meters": 0.4 + "x_meters": 7.621265479463121, + "y_meters": 3.305143710495016, + "intermediate_handoff_radius_meters": 0.29 }, "rotation_target": { - "rotation_radians": -1.5708, + "rotation_radians": -2.356194490192345, "profiled_rotation": true } }, @@ -65,12 +65,24 @@ "y_meters": 2.04773, "intermediate_handoff_radius_meters": 0.3 }, + { + "type": "rotation", + "rotation_radians": -1.5707963267948966, + "t_ratio": 0.7499999999999996, + "profiled_rotation": true + }, { "type": "translation", "x_meters": 6.71926, "y_meters": 1.24825, "intermediate_handoff_radius_meters": 0.35 }, + { + "type": "translation", + "x_meters": 5.999606934935329, + "y_meters": 0.6286410102567475, + "intermediate_handoff_radius_meters": 0.2 + }, { "type": "waypoint", "translation_target": { @@ -89,8 +101,8 @@ "max_velocity_meters_per_sec": [ { "value": 2.5, - "start_ordinal": 7, - "end_ordinal": 7 + "start_ordinal": 8, + "end_ordinal": 8 }, { "value": 1.8, @@ -98,9 +110,9 @@ "end_ordinal": 4 }, { - "value": 2.5, + "value": 2.4, "start_ordinal": 5, - "end_ordinal": 6 + "end_ordinal": 7 }, { "value": 2.6, @@ -114,11 +126,6 @@ "start_ordinal": 6, "end_ordinal": 6 }, - { - "value": 3.0, - "start_ordinal": 7, - "end_ordinal": 7 - }, { "value": 3.0, "start_ordinal": 2, From 28183727ede01912bd99b17555c44b66b470c0c0 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 8 Jun 2026 17:46:00 -0700 Subject: [PATCH 097/114] fixed overall velocities --- .../deploy/autos/paths/FirstNeutralBump.json | 12 ++++++------ .../deploy/autos/paths/FirstNeutralTrench.json | 18 +++++++++--------- .../deploy/autos/paths/SecondNeutralBump.json | 8 ++++---- .../autos/paths/SecondNeutralTrench.json | 8 +++++++- 4 files changed, 26 insertions(+), 20 deletions(-) diff --git a/src/main/deploy/autos/paths/FirstNeutralBump.json b/src/main/deploy/autos/paths/FirstNeutralBump.json index b9b8eb506..4ab50087c 100644 --- a/src/main/deploy/autos/paths/FirstNeutralBump.json +++ b/src/main/deploy/autos/paths/FirstNeutralBump.json @@ -43,8 +43,8 @@ }, { "type": "translation", - "x_meters": 7.62013, - "y_meters": 1.40061, + "x_meters": 7.26254, + "y_meters": 1.38392, "intermediate_handoff_radius_meters": 0.3 }, { @@ -52,17 +52,17 @@ "translation_target": { "x_meters": 7.80486, "y_meters": 3.31046, - "intermediate_handoff_radius_meters": 0.3 + "intermediate_handoff_radius_meters": 0.48 }, "rotation_target": { - "rotation_radians": -1.5708, + "rotation_radians": -2.72134, "profiled_rotation": true } }, { "type": "translation", - "x_meters": 6.93776, - "y_meters": 2.87846, + "x_meters": 6.94491, + "y_meters": 2.81006, "intermediate_handoff_radius_meters": 0.6 }, { diff --git a/src/main/deploy/autos/paths/FirstNeutralTrench.json b/src/main/deploy/autos/paths/FirstNeutralTrench.json index 2efd03138..8032f6f63 100644 --- a/src/main/deploy/autos/paths/FirstNeutralTrench.json +++ b/src/main/deploy/autos/paths/FirstNeutralTrench.json @@ -43,19 +43,19 @@ }, { "type": "translation", - "x_meters": 7.39084676942767, - "y_meters": 1.3854781600085788, + "x_meters": 7.39085, + "y_meters": 1.38548, "intermediate_handoff_radius_meters": 0.3 }, { "type": "waypoint", "translation_target": { - "x_meters": 7.621265479463121, - "y_meters": 3.305143710495016, + "x_meters": 7.62127, + "y_meters": 3.30514, "intermediate_handoff_radius_meters": 0.29 }, "rotation_target": { - "rotation_radians": -2.356194490192345, + "rotation_radians": -2.35619, "profiled_rotation": true } }, @@ -67,8 +67,8 @@ }, { "type": "rotation", - "rotation_radians": -1.5707963267948966, - "t_ratio": 0.7499999999999996, + "rotation_radians": -1.5708, + "t_ratio": 0.75, "profiled_rotation": true }, { @@ -79,8 +79,8 @@ }, { "type": "translation", - "x_meters": 5.999606934935329, - "y_meters": 0.6286410102567475, + "x_meters": 5.99961, + "y_meters": 0.62864, "intermediate_handoff_radius_meters": 0.2 }, { diff --git a/src/main/deploy/autos/paths/SecondNeutralBump.json b/src/main/deploy/autos/paths/SecondNeutralBump.json index cb4ba2f9b..8514e4402 100644 --- a/src/main/deploy/autos/paths/SecondNeutralBump.json +++ b/src/main/deploy/autos/paths/SecondNeutralBump.json @@ -57,8 +57,8 @@ { "type": "waypoint", "translation_target": { - "x_meters": 7.64024, - "y_meters": 2.52584, + "x_meters": 7.43006, + "y_meters": 2.61144, "intermediate_handoff_radius_meters": 0.4 }, "rotation_target": { @@ -74,8 +74,8 @@ }, { "type": "translation", - "x_meters": 5.4365, - "y_meters": 2.61687, + "x_meters": 5.87577, + "y_meters": 2.63547, "intermediate_handoff_radius_meters": 0.4 }, { diff --git a/src/main/deploy/autos/paths/SecondNeutralTrench.json b/src/main/deploy/autos/paths/SecondNeutralTrench.json index 06eabcf98..5295532e5 100644 --- a/src/main/deploy/autos/paths/SecondNeutralTrench.json +++ b/src/main/deploy/autos/paths/SecondNeutralTrench.json @@ -62,10 +62,16 @@ "intermediate_handoff_radius_meters": 0.38 }, "rotation_target": { - "rotation_radians": 1.5708, + "rotation_radians": 2.35619, "profiled_rotation": true } }, + { + "type": "rotation", + "rotation_radians": 1.5708, + "t_ratio": 0.5, + "profiled_rotation": true + }, { "type": "translation", "x_meters": 7.17921, From 856e3e8ba0c2d1cff66861bcc165962faf478a95 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Mon, 22 Jun 2026 15:11:41 -0700 Subject: [PATCH 098/114] added basic unit test --- src/main/java/frc/robot/Robot.java | 2 +- .../subsystems/auto/BLine/BLineLogic.java | 95 +++++++++++++++---- .../subsystems/auto/BLine/BLinePath.java | 7 +- src/test/java/AutosTest.java | 74 ++++++++------- 4 files changed, 123 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f64933eeb..537f6a25a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -123,8 +123,8 @@ protected Robot() { robotSim = null; } // BLINE STUFF + BLineLogic.handleInit(subsystems, Robot.isSimulation()); - BLineLogic.init(subsystems); BLineLogic.configure(subsystems); BLineLogic.initSmartDashboard(); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index f23d35640..d945cea55 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -96,6 +96,16 @@ public static boolean isMirrored() { } // ========================= INIT ========================= + public static void handleInit(Subsystems subsystems, boolean unitTest) { + + registerCommands(); + + if (unitTest) { + unitTestInit(); + } else { + init(subsystems); + } + } public static void init(Subsystems subsystems) { s = subsystems; @@ -152,6 +162,20 @@ public static void configure(Subsystems s) { continuingPathBuilder = createPathBuilder(s); } + public static void unitTestInit() { + defaultPath = new BLinePath("default", "Center", "default"); + + rebuiltPaths = + List.of( + defaultPath, + new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), + new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), + new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), + new BLinePath( + "DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump"), + new BLinePath("BumpNeutralDepot", "RT", "FirstNeutralBump", "BumpDepot")); + } + private static FollowPath.Builder createPathBuilder(Subsystems s) { return new FollowPath.Builder( s.drivebaseSubsystem, @@ -264,14 +288,48 @@ public static Pose2d getSelectedAutoStartingPose() { } // ========================= AUTO EXECUTION ========================= - + public static List getPathsToBuild() { + BLinePath selected = getSelectedAutoPath(); + if (selected == null) { + return List.of(); + } + return selected.getAllPaths(); // Path objects already exist, just return them +} public static Command getSelectedAuto() { double delay = autoDelayEntry.getDouble(0.0); - BLinePath path = getSelectedAutoPath(); - s.drivebaseSubsystem.resetRotation(path.getPath().getInitialModuleDirection()); - return Commands.waitSeconds(delay).andThen(buildPath(path.getPath(), true)); + BLinePath selected = getSelectedAutoPath(); + + if (selected == null) { + return Commands.none(); + } + + s.drivebaseSubsystem.resetRotation(selected.getPath().getInitialModuleDirection()); + + List commands = new ArrayList<>(); + commands.add(Commands.waitSeconds(delay)); + + List paths = getPathsToBuild(); + for (int i = 0; i < paths.size(); i++) { + boolean resetPose = (i == 0); // Reset pose only on first path + commands.add(buildPath(paths.get(i), resetPose)); + } + + return Commands.sequence(commands.toArray(new Command[0])); +} + public static List getBLinePaths() { + return rebuiltPaths; + } + + public static List getBLinePathsNames() { + List pathsNames = new ArrayList<>(); + for(BLinePath path :getBLinePaths()) { + pathsNames.addAll(path.getDisplayingNames()); + } + return pathsNames; + } + public static Command buildSingleNeutralTrenchAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), @@ -318,20 +376,20 @@ private static Command buildPath(Path path, boolean resetPose) { public static Command handleAutos() { switch (getSelectedAutoName()) { - case "TrenchNeutral": - return buildSingleNeutralTrenchAuto(); - case "DoubleTrenchNeutral": - return buildDoubleNeutralTrenchAuto(); - case "BumpNeutral": - return buildSingleNeutralBumpAuto(); - case "DoubleBumpNeutral": - return buildDoubleNeutralBumpAuto(); - case "BumpNeutralDepot": - return buildSingleNeutralBumpDepotAuto(); - default: - return pathBuilder.build(defaultPath.getPath()); + case "TrenchNeutral": + return buildSingleNeutralTrenchAuto(); + case "DoubleTrenchNeutral": + return buildDoubleNeutralTrenchAuto(); + case "BumpNeutral": + return buildSingleNeutralBumpAuto(); + case "DoubleBumpNeutral": + return buildDoubleNeutralBumpAuto(); + case "BumpNeutralDepot": + return buildSingleNeutralBumpDepotAuto(); + default: + return getSelectedAuto(); // Use the new generic builder } - } +} private static void updateInitialHeading() { BLinePath selected = getSelectedAutoPath(); @@ -406,6 +464,7 @@ private static void registerCommands() { FollowPath.registerEventTrigger("intake", intakeCommand()); FollowPath.registerEventTrigger("climb", climbCommand()); - FollowPath.registerEventTrigger("cancel", Commands.runOnce(() -> launchAllowed.set(false)).andThen(stowCommand())); + FollowPath.registerEventTrigger( + "cancel", Commands.runOnce(() -> launchAllowed.set(false)).andThen(stowCommand())); } } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index d728d8899..28e80396b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -13,7 +13,7 @@ public class BLinePath { private final boolean vision; private final Path path; private final List allPaths; - + public List displayedPaths = new ArrayList<>(); private StartPosition positionType; private Pose2d startPose; @@ -28,8 +28,10 @@ public BLinePath( this.vision = vision; List loaded = new ArrayList<>(); + for (String name : displayedPathNames) { loaded.add(new Path(name)); + displayedPaths.add(name); } this.allPaths = List.copyOf(loaded); this.path = this.allPaths.get(0); @@ -39,6 +41,9 @@ public BLinePath( public String getDisplayName() { return displayName; } + public List getDisplayingNames() { + return displayedPaths; + } public String getStartingPosName() { return startingPosName; diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index d8fa5a248..c6fc4b1b9 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -1,49 +1,53 @@ -import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; -import com.pathplanner.lib.commands.PathPlannerAuto; -import frc.robot.generated.CompTunerConstants; -import frc.robot.subsystems.auto.PathPlanner.AutoBuilderConfig; -import frc.robot.subsystems.auto.PathPlanner.AutoPath; -import frc.robot.subsystems.auto.PathPlanner.PathPlannerLogic; -import java.io.IOException; -import java.util.Dictionary; -import java.util.Enumeration; -import java.util.Hashtable; -import org.json.simple.parser.ParseException; +import frc.robot.subsystems.auto.BLine.BLineLogic; +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; -class AutosTest { +public class AutosTest { - // @BeforeEach - void setup() { - - AutoBuilderConfig.buildAuto(CompTunerConstants.createDrivetrain(), true); + @BeforeEach + public void setup() { + BLineLogic.unitTestInit(); } - // @Test - void validateFileName() throws IOException, ParseException { - PathPlannerLogic.initCommandsAndPaths(true); + @Test + public void unusedFileNames() { + File subfolder = new File("src/main/deploy/autos/paths"); + assertNotNull(subfolder, "This folder should not be null."); - Dictionary pathDictionary = new Hashtable<>(); - for (AutoPath path : PathPlannerLogic.getAutos()) { // Every auto in the auto chooser - pathDictionary.put( - path.getAutoName(), false); // Putting each auto from the auto chooser in dictionary + if (!subfolder.exists() || !subfolder.isDirectory()) { + System.out.println( + "⚠️ WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); + return; } - // We expect >= 1 paths, otherwise its possible we forgot to call - // AutoLogic::initCommandsAndPaths(). - if (pathDictionary.isEmpty()) { - throw new IllegalStateException( - "No auto paths found. Did you forget to call AutoLogic::initCommandsAndPaths()?"); + File[] files = subfolder.listFiles(); + if (files == null || files.length == 0) { + System.out.println("⚠️ WARNING: Autos folder is empty"); + return; } - Enumeration k = pathDictionary.keys(); - while (k.hasMoreElements()) { // As long as there are more autos in the list - String key = k.nextElement(); - // Next entry in the dictionary - // Asserts false if file name does not exist + List validPathNames = BLineLogic.getBLinePathsNames(); + List unusedFiles = new ArrayList<>(); + + for (File file : files) { + String filename = file.getName().split("\\.")[0]; + if (!validPathNames.contains(filename)) { + unusedFiles.add(file.getName()); + } + } - assertFalse( - PathPlannerAuto.getPathGroupFromAutoFile(key).isEmpty(), "No file path matches" + key); + if (!unusedFiles.isEmpty()) { + System.out.println("⚠️ WARNING: Unused BLine Paths found:"); + for (String file : unusedFiles) { + System.out.println(" - " + file); + } + } else { + System.out.println("✓ All files in 'autos' folder are used"); } } } From d3d7c33af1529e0be86227b02b7018935d944bef Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 16:45:25 -0700 Subject: [PATCH 099/114] some debugging stuff --- .../subsystems/auto/BLine/BLineLogic.java | 48 +++++++++---------- .../subsystems/auto/BLine/BLinePath.java | 5 +- src/test/java/AutosTest.java | 11 +++-- 3 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index d945cea55..a087a3d72 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -288,19 +288,20 @@ public static Pose2d getSelectedAutoStartingPose() { } // ========================= AUTO EXECUTION ========================= - public static List getPathsToBuild() { + public static List getPathsToBuild() { BLinePath selected = getSelectedAutoPath(); if (selected == null) { - return List.of(); + return List.of(); } - return selected.getAllPaths(); // Path objects already exist, just return them -} + return selected.getAllPaths(); // Path objects already exist, just return them + } + public static Command getSelectedAuto() { double delay = autoDelayEntry.getDouble(0.0); BLinePath selected = getSelectedAutoPath(); if (selected == null) { - return Commands.none(); + return Commands.none(); } s.drivebaseSubsystem.resetRotation(selected.getPath().getInitialModuleDirection()); @@ -310,26 +311,25 @@ public static Command getSelectedAuto() { List paths = getPathsToBuild(); for (int i = 0; i < paths.size(); i++) { - boolean resetPose = (i == 0); // Reset pose only on first path - commands.add(buildPath(paths.get(i), resetPose)); + boolean resetPose = (i == 0); // Reset pose only on first path + commands.add(buildPath(paths.get(i), resetPose)); } return Commands.sequence(commands.toArray(new Command[0])); -} + } + public static List getBLinePaths() { return rebuiltPaths; } public static List getBLinePathsNames() { List pathsNames = new ArrayList<>(); - for(BLinePath path :getBLinePaths()) { + for (BLinePath path : getBLinePaths()) { pathsNames.addAll(path.getDisplayingNames()); } return pathsNames; - } - public static Command buildSingleNeutralTrenchAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), @@ -376,20 +376,20 @@ private static Command buildPath(Path path, boolean resetPose) { public static Command handleAutos() { switch (getSelectedAutoName()) { - case "TrenchNeutral": - return buildSingleNeutralTrenchAuto(); - case "DoubleTrenchNeutral": - return buildDoubleNeutralTrenchAuto(); - case "BumpNeutral": - return buildSingleNeutralBumpAuto(); - case "DoubleBumpNeutral": - return buildDoubleNeutralBumpAuto(); - case "BumpNeutralDepot": - return buildSingleNeutralBumpDepotAuto(); - default: - return getSelectedAuto(); // Use the new generic builder + case "TrenchNeutral": + return buildSingleNeutralTrenchAuto(); + case "DoubleTrenchNeutral": + return buildDoubleNeutralTrenchAuto(); + case "BumpNeutral": + return buildSingleNeutralBumpAuto(); + case "DoubleBumpNeutral": + return buildDoubleNeutralBumpAuto(); + case "BumpNeutralDepot": + return buildSingleNeutralBumpDepotAuto(); + default: + return getSelectedAuto(); // Use the new generic builder } -} + } private static void updateInitialHeading() { BLinePath selected = getSelectedAutoPath(); diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index 28e80396b..4063b002e 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -13,7 +13,7 @@ public class BLinePath { private final boolean vision; private final Path path; private final List allPaths; - public List displayedPaths = new ArrayList<>(); + public List displayedPaths = new ArrayList<>(); private StartPosition positionType; private Pose2d startPose; @@ -41,7 +41,8 @@ public BLinePath( public String getDisplayName() { return displayName; } - public List getDisplayingNames() { + + public List getDisplayingNames() { return displayedPaths; } diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index c6fc4b1b9..0e15ace89 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -1,5 +1,6 @@ import static org.junit.jupiter.api.Assertions.assertNotNull; +import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.auto.BLine.BLineLogic; import java.io.File; import java.util.ArrayList; @@ -20,18 +21,18 @@ public void unusedFileNames() { assertNotNull(subfolder, "This folder should not be null."); if (!subfolder.exists() || !subfolder.isDirectory()) { - System.out.println( - "⚠️ WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); + System.out.println("WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); return; } File[] files = subfolder.listFiles(); if (files == null || files.length == 0) { - System.out.println("⚠️ WARNING: Autos folder is empty"); + System.out.println("WARNING: Autos folder is empty"); return; } List validPathNames = BLineLogic.getBLinePathsNames(); + for (String s : validPathNames) Commands.print(s); List unusedFiles = new ArrayList<>(); for (File file : files) { @@ -42,12 +43,12 @@ public void unusedFileNames() { } if (!unusedFiles.isEmpty()) { - System.out.println("⚠️ WARNING: Unused BLine Paths found:"); + System.out.println("WARNING: Unused BLine Paths found:"); for (String file : unusedFiles) { System.out.println(" - " + file); } } else { - System.out.println("✓ All files in 'autos' folder are used"); + System.out.println("All files in 'autos' folder are used"); } } } From 30a44cf71e6d88c2a12a935a3cd192c86f96edd4 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 17:12:52 -0700 Subject: [PATCH 100/114] complete unit test for unused autos --- src/main/java/frc/robot/Robot.java | 18 +++--- .../subsystems/auto/BLine/BLineLogic.java | 63 +++++++++---------- .../subsystems/auto/BLine/BLinePath.java | 11 ++-- src/test/java/AutosTest.java | 3 +- 4 files changed, 44 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 537f6a25a..20ce6d2dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -118,21 +118,16 @@ protected Robot() { if (DRIVEBASE_ENABLED) { if (Robot.isSimulation()) { robotSim = new RobotSim(subsystems.drivebaseSubsystem); - } else { robotSim = null; } - // BLINE STUFF - BLineLogic.handleInit(subsystems, Robot.isSimulation()); - - BLineLogic.configure(subsystems); - BLineLogic.initSmartDashboard(); - BLineAutonomousField.initSmartDashBoard( - () -> "Autos", // tab name - 0, // column (unused, just pass 0) - 0, // row (unused, just pass 0) - this::addPeriodic); + // BLINE STUFF + BLineLogic.init(subsystems); // Handling init and unit test cases + BLineLogic.configure(subsystems); // configure the autobuilder to run autos + BLineLogic.initSmartDashboard(); // Logging + BLineAutonomousField.initSmartDashBoard( // Visualization + () -> "Autos", 0, 0, this::addPeriodic); } CommandScheduler.getInstance() @@ -286,6 +281,7 @@ public void autonomousInit() { subsystems.ledSubsystem.setMode(LEDSubsystem.LEDMode.RAINBOW); if (Robot.isSimulation()) { + robotSim.resetFuelSim(); } diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index a087a3d72..aba1569bb 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -96,24 +96,24 @@ public static boolean isMirrored() { } // ========================= INIT ========================= - public static void handleInit(Subsystems subsystems, boolean unitTest) { - registerCommands(); - - if (unitTest) { - unitTestInit(); - } else { - init(subsystems); - } - } public static void init(Subsystems subsystems) { s = subsystems; - registerCommands(); if (pathsInitialized) return; + initializePaths(); + pathsInitialized = true; + } + + public static void unitTestInit() { + s = null; // Explicitly set to null for unit tests + initializePaths(); + } + + private static void initializePaths() { defaultPath = new BLinePath("default", "Center", "default"); rebuiltPaths = @@ -122,13 +122,11 @@ public static void init(Subsystems subsystems) { new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), - new BLinePath( - "DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump"), - new BLinePath("BumpNeutralDepot", "RT", "FirstNeutralBump", "BumpDepot")); + new BLinePath("DoubleBumpNeutral", "RT", "FirstNeutralBump", "SecondNeutralBump"), + new BLinePath("BumpNeutralDepot", "RT", "FirstNeutralBump")); autos.clear(); autos.addAll(rebuiltPaths); - commandsMap = Map.of(0, rebuiltPaths); namesToAuto.clear(); @@ -138,8 +136,6 @@ public static void init(Subsystems subsystems) { namesToAuto.put(auto.getDisplayName(), auto); } } - - pathsInitialized = true; } public static void handleStartingPoses(BLinePath path) { @@ -162,20 +158,6 @@ public static void configure(Subsystems s) { continuingPathBuilder = createPathBuilder(s); } - public static void unitTestInit() { - defaultPath = new BLinePath("default", "Center", "default"); - - rebuiltPaths = - List.of( - defaultPath, - new BLinePath("TrenchNeutral", "RT", "FirstNeutralTrench"), - new BLinePath("DoubleTrenchNeutral", "RT", "FirstNeutralTrench", "SecondNeutralTrench"), - new BLinePath("BumpNeutral", "RT", "FirstNeutralBump"), - new BLinePath( - "DoubleBumpNeutral", "RT", "FirstNeutralBump", "BumpToTrench", "SecondNeutralBump"), - new BLinePath("BumpNeutralDepot", "RT", "FirstNeutralBump", "BumpDepot")); - } - private static FollowPath.Builder createPathBuilder(Subsystems s) { return new FollowPath.Builder( s.drivebaseSubsystem, @@ -293,10 +275,15 @@ public static List getPathsToBuild() { if (selected == null) { return List.of(); } - return selected.getAllPaths(); // Path objects already exist, just return them + return selected.getAllPaths(); } public static Command getSelectedAuto() { + if (s == null) { + // Unit test mode - return empty command + return Commands.none(); + } + double delay = autoDelayEntry.getDouble(0.0); BLinePath selected = getSelectedAutoPath(); @@ -311,7 +298,7 @@ public static Command getSelectedAuto() { List paths = getPathsToBuild(); for (int i = 0; i < paths.size(); i++) { - boolean resetPose = (i == 0); // Reset pose only on first path + boolean resetPose = (i == 0); commands.add(buildPath(paths.get(i), resetPose)); } @@ -371,6 +358,9 @@ public static Command buildSingleNeutralBumpDepotAuto() { } private static Command buildPath(Path path, boolean resetPose) { + if (s == null || pathBuilder == null || continuingPathBuilder == null) { + return Commands.none(); + } return (resetPose ? pathBuilder : continuingPathBuilder).build(path); } @@ -387,7 +377,7 @@ public static Command handleAutos() { case "BumpNeutralDepot": return buildSingleNeutralBumpDepotAuto(); default: - return getSelectedAuto(); // Use the new generic builder + return getSelectedAuto(); } } @@ -409,7 +399,7 @@ public static Command intakeCommand() { } public static Command launcherCommand(double timeout) { - if (Robot.isSimulation()) return RobotSim.launch(s, timeout); + if (s == null || Robot.isSimulation()) return RobotSim.launch(s, timeout); return Commands.parallel( Commands.runOnce(() -> s.flywheels.resetFuelCheck()), s.launcherSubsystem.launcherAimCommand(), @@ -421,6 +411,7 @@ public static Command launcherCommand(double timeout) { } public static Command launcherCommand() { + if (s == null) return Commands.none(); return Commands.parallel( Commands.runOnce(() -> s.flywheels.resetFuelCheck()), s.launcherSubsystem.launcherAimCommand(), @@ -430,6 +421,7 @@ public static Command launcherCommand() { } public static Command stowCommand() { + if (s == null) return Commands.none(); return s.launcherSubsystem.rawStowCommand(); } @@ -438,6 +430,7 @@ public static Command climbCommand() { } public static void cancelCommand() { + if (s == null) return; if (Robot.isSimulation()) { CommandScheduler.getInstance().cancel(bLineSimLaunching); } else { @@ -446,6 +439,8 @@ public static void cancelCommand() { } private static void registerCommands() { + if (s == null) return; // Skip registration during unit tests + AtomicBoolean launchAllowed = new AtomicBoolean(true); if (s.launcherSubsystem != null && s.indexerSubsystem != null) { diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java index 4063b002e..574149c20 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLinePath.java @@ -13,7 +13,7 @@ public class BLinePath { private final boolean vision; private final Path path; private final List allPaths; - public List displayedPaths = new ArrayList<>(); + private final List displayedPaths; // Make it final private StartPosition positionType; private Pose2d startPose; @@ -28,14 +28,17 @@ public BLinePath( this.vision = vision; List loaded = new ArrayList<>(); + List displayedList = new ArrayList<>(); for (String name : displayedPathNames) { loaded.add(new Path(name)); - displayedPaths.add(name); + displayedList.add(name); } + this.allPaths = List.copyOf(loaded); - this.path = this.allPaths.get(0); - this.startPose = this.path.getStartPose(); + this.displayedPaths = List.copyOf(displayedList); // Make it immutable + this.path = this.allPaths.isEmpty() ? null : this.allPaths.get(0); + this.startPose = this.path != null ? this.path.getStartPose() : new Pose2d(); } public String getDisplayName() { diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index 0e15ace89..1961cc593 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -1,6 +1,5 @@ import static org.junit.jupiter.api.Assertions.assertNotNull; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.auto.BLine.BLineLogic; import java.io.File; import java.util.ArrayList; @@ -32,7 +31,6 @@ public void unusedFileNames() { } List validPathNames = BLineLogic.getBLinePathsNames(); - for (String s : validPathNames) Commands.print(s); List unusedFiles = new ArrayList<>(); for (File file : files) { @@ -44,6 +42,7 @@ public void unusedFileNames() { if (!unusedFiles.isEmpty()) { System.out.println("WARNING: Unused BLine Paths found:"); + for (String file : unusedFiles) { System.out.println(" - " + file); } From ab1d2cdb4a8213e7b83ab5eda2bddb2cc5f8c7ab Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 17:18:04 -0700 Subject: [PATCH 101/114] added null checks --- .../frc/robot/subsystems/auto/BLine/BLineLogic.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java index aba1569bb..c8a2d531b 100644 --- a/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/BLine/BLineLogic.java @@ -97,7 +97,6 @@ public static boolean isMirrored() { // ========================= INIT ========================= - public static void init(Subsystems subsystems) { s = subsystems; registerCommands(); @@ -249,6 +248,9 @@ public static void filterAutos(int numGameObjects) { // ========================= SELECTION METHODS ========================= public static String getSelectedAutoName() { + if (autoChooser.getSelected() == null) { + return "Default"; + } return autoChooser.getSelected(); } @@ -357,6 +359,13 @@ public static Command buildSingleNeutralBumpDepotAuto() { launcherCommand()); } + public static Command buildDefaultAuto() { + return BLineCommands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0.0)), + buildPath(new Path("Default"), true), + launcherCommand()); + } + private static Command buildPath(Path path, boolean resetPose) { if (s == null || pathBuilder == null || continuingPathBuilder == null) { return Commands.none(); @@ -377,7 +386,7 @@ public static Command handleAutos() { case "BumpNeutralDepot": return buildSingleNeutralBumpDepotAuto(); default: - return getSelectedAuto(); + return buildDefaultAuto(); } } From 188f3b8b92d31d55d571c1ddddbd2175b164e1f5 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 18:44:18 -0700 Subject: [PATCH 102/114] added a LOT more unit tests but not quite done yet --- src/test/java/AutosTest.java | 160 ++++++++++++++++++++++++++++++++--- 1 file changed, 150 insertions(+), 10 deletions(-) diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java index 1961cc593..ce1436547 100644 --- a/src/test/java/AutosTest.java +++ b/src/test/java/AutosTest.java @@ -1,7 +1,8 @@ -import static org.junit.jupiter.api.Assertions.assertNotNull; - +import edu.wpi.first.wpilibj.Filesystem; import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.subsystems.auto.BLine.BLinePath; import java.io.File; +import java.io.IOException; import java.util.ArrayList; import java.util.List; import org.junit.jupiter.api.BeforeEach; @@ -9,32 +10,56 @@ public class AutosTest { + private static final String PATHS_PATH = "src/main/deploy/autos/paths"; + @BeforeEach public void setup() { BLineLogic.unitTestInit(); } - @Test - public void unusedFileNames() { - File subfolder = new File("src/main/deploy/autos/paths"); - assertNotNull(subfolder, "This folder should not be null."); + // ==================== METHODS ==================== + + private File[] getAutosFiles() { + File subfolder = new File(PATHS_PATH); if (!subfolder.exists() || !subfolder.isDirectory()) { System.out.println("WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); - return; + return null; } File[] files = subfolder.listFiles(); if (files == null || files.length == 0) { System.out.println("WARNING: Autos folder is empty"); - return; + return null; } + return files; + } + + private String getFilenamWithoutExtension(File file) { + return file.getName().split("\\.")[0]; + } + + private List getValidFileNames(File[] files) { + List validFileNames = new ArrayList<>(); + for (File file : files) { + validFileNames.add(getFilenamWithoutExtension(file)); + } + return validFileNames; + } + + // ==================== ACUTAL UNIT TESTS ==================== + + @Test + public void unusedFileNames() { + File[] files = getAutosFiles(); + if (files == null) return; + List validPathNames = BLineLogic.getBLinePathsNames(); List unusedFiles = new ArrayList<>(); for (File file : files) { - String filename = file.getName().split("\\.")[0]; + String filename = getFilenamWithoutExtension(file); if (!validPathNames.contains(filename)) { unusedFiles.add(file.getName()); } @@ -42,7 +67,6 @@ public void unusedFileNames() { if (!unusedFiles.isEmpty()) { System.out.println("WARNING: Unused BLine Paths found:"); - for (String file : unusedFiles) { System.out.println(" - " + file); } @@ -50,4 +74,120 @@ public void unusedFileNames() { System.out.println("All files in 'autos' folder are used"); } } + + @Test + public void validatePathFiles() { + File[] files = getAutosFiles(); + if (files == null) return; + + List validFileNames = getValidFileNames(files); + List invalidPaths = new ArrayList<>(); + + for (BLinePath auto : BLineLogic.getBLinePaths()) { + for (String pathName : auto.getDisplayingNames()) { + if (!validFileNames.contains(pathName)) { + invalidPaths.add( + "Auto '" + auto.getDisplayName() + "' references missing path: " + pathName); + } + } + } + + if (!invalidPaths.isEmpty()) { + System.out.println("WARNING: Autos reference invalid path files:"); + for (String invalid : invalidPaths) { + System.out.println(" - " + invalid); + } + } else { + System.out.println("All autos only reference valid path files"); + } + } + + @Test + public void validateUniqueAutoNames() { + File[] files = getAutosFiles(); + if (files == null) return; + + List invalidNames = new ArrayList<>(); + + for (File file : files) { + String filename = getFilenamWithoutExtension(file); + + if (filename.startsWith(" ")) { + invalidNames.add(file.getName() + " (contains leading spaces)"); + } + if (filename.endsWith(" ")) { + invalidNames.add(file.getName() + " (contains trailing spaces)"); + } + + if (filename.matches(".*[0-9]$")) { + invalidNames.add(file.getName() + " (ends with number)"); + } + } + + if (!invalidNames.isEmpty()) { + System.out.println("WARNING: Invalid auto names:"); + for (String name : invalidNames) { + System.out.println(" - " + name); + } + } else { + System.out.println("All auto names are valid"); + } + } + + @Test + public void compareJSONContents() throws IOException { + File deployDir = Filesystem.getDeployDirectory(); + + if (!deployDir.exists() || !deployDir.isDirectory()) { + System.out.println("WARNING: Deploy directory not found"); + return; + } + + List allJsonFiles = getAllJsonFiles(deployDir); + + if (allJsonFiles.size() < 2) { + System.out.println("WARNING: Not enough files to compare"); + return; + } + + // Compare each file with every other file + for (int i = 0; i < allJsonFiles.size(); i++) { + String content1 = new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(i).toPath())); + + for (int j = i + 1; j < allJsonFiles.size(); j++) { + String content2 = + new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(j).toPath())); + + String file1Name = allJsonFiles.get(i).getName(); + String file2Name = allJsonFiles.get(j).getName(); + + if (content1.equals(content2)) { + System.out.println(file1Name + " and " + file2Name + " are IDENTICAL"); + } + } + } + } + + private List getAllJsonFiles(File dir) { + List jsonFiles = new ArrayList<>(); + collectJsonFiles(dir, jsonFiles); + return jsonFiles; + } + + private void collectJsonFiles(File dir, List jsonFiles) { + if (!dir.exists() || !dir.isDirectory()) { + return; + } + + File[] files = dir.listFiles(); + if (files == null) return; + + for (File file : files) { + if (file.isFile() && file.getName().endsWith(".json")) { + jsonFiles.add(file); + } else if (file.isDirectory()) { + collectJsonFiles(file, jsonFiles); + } + } + } } From f4faf465f471d855890e47f63b0261ee5d885fdd Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 19:48:07 -0700 Subject: [PATCH 103/114] added some unit test debugging extensions --- .vscode/extensions.json | 10 ++++++++++ .vscode/settings.json | 4 +++- 2 files changed, 13 insertions(+), 1 deletion(-) create mode 100644 .vscode/extensions.json diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 000000000..aef6495bc --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // Recommended extensions for working with Java tests in this workspace. + "recommendations": [ + "vscjava.vscode-java-pack", // Java Extension Pack (convenience bundle) + "vscjava.vscode-java-test", // Java Test Runner (Test Explorer integration) + "redhat.java", // Language Support for Java(TM) by Red Hat + "vscjava.vscode-java-dependency", // Java Project Dependency Viewer + "vscjava.vscode-java-debug" // Java Debugger + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json index 80e40ce8a..37fb7f033 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -20,7 +20,9 @@ "workingDirectory": "${workspaceFolder}/build/jni/release", "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + // Ensure native libs are found on Windows as well as Unix-like systems + "PATH": "${workspaceFolder}\\build\\jni\\release;${env:PATH}", + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" } }, From eb26b61827521d8e1fd00e520357fde675ac13c4 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 20:03:03 -0700 Subject: [PATCH 104/114] far better organization --- src/test/java/AutoFilePathsTest.java | 1 + src/test/java/AutosTest.java | 193 ------------------------- src/test/java/JSONComparisionTest.java | 42 ++++++ src/test/java/TestConstantsUtils.java | 67 +++++++++ src/test/java/UniqueAutoNamesTest.java | 38 +++++ src/test/java/UnusedFilesTest.java | 32 ++++ 6 files changed, 180 insertions(+), 193 deletions(-) create mode 100644 src/test/java/AutoFilePathsTest.java delete mode 100644 src/test/java/AutosTest.java create mode 100644 src/test/java/JSONComparisionTest.java create mode 100644 src/test/java/TestConstantsUtils.java create mode 100644 src/test/java/UniqueAutoNamesTest.java create mode 100644 src/test/java/UnusedFilesTest.java diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/src/test/java/AutoFilePathsTest.java @@ -0,0 +1 @@ + diff --git a/src/test/java/AutosTest.java b/src/test/java/AutosTest.java deleted file mode 100644 index ce1436547..000000000 --- a/src/test/java/AutosTest.java +++ /dev/null @@ -1,193 +0,0 @@ -import edu.wpi.first.wpilibj.Filesystem; -import frc.robot.subsystems.auto.BLine.BLineLogic; -import frc.robot.subsystems.auto.BLine.BLinePath; -import java.io.File; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -public class AutosTest { - - private static final String PATHS_PATH = "src/main/deploy/autos/paths"; - - @BeforeEach - public void setup() { - BLineLogic.unitTestInit(); - } - - // ==================== METHODS ==================== - - private File[] getAutosFiles() { - File subfolder = new File(PATHS_PATH); - - if (!subfolder.exists() || !subfolder.isDirectory()) { - System.out.println("WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); - return null; - } - - File[] files = subfolder.listFiles(); - if (files == null || files.length == 0) { - System.out.println("WARNING: Autos folder is empty"); - return null; - } - - return files; - } - - private String getFilenamWithoutExtension(File file) { - return file.getName().split("\\.")[0]; - } - - private List getValidFileNames(File[] files) { - List validFileNames = new ArrayList<>(); - for (File file : files) { - validFileNames.add(getFilenamWithoutExtension(file)); - } - return validFileNames; - } - - // ==================== ACUTAL UNIT TESTS ==================== - - @Test - public void unusedFileNames() { - File[] files = getAutosFiles(); - if (files == null) return; - - List validPathNames = BLineLogic.getBLinePathsNames(); - List unusedFiles = new ArrayList<>(); - - for (File file : files) { - String filename = getFilenamWithoutExtension(file); - if (!validPathNames.contains(filename)) { - unusedFiles.add(file.getName()); - } - } - - if (!unusedFiles.isEmpty()) { - System.out.println("WARNING: Unused BLine Paths found:"); - for (String file : unusedFiles) { - System.out.println(" - " + file); - } - } else { - System.out.println("All files in 'autos' folder are used"); - } - } - - @Test - public void validatePathFiles() { - File[] files = getAutosFiles(); - if (files == null) return; - - List validFileNames = getValidFileNames(files); - List invalidPaths = new ArrayList<>(); - - for (BLinePath auto : BLineLogic.getBLinePaths()) { - for (String pathName : auto.getDisplayingNames()) { - if (!validFileNames.contains(pathName)) { - invalidPaths.add( - "Auto '" + auto.getDisplayName() + "' references missing path: " + pathName); - } - } - } - - if (!invalidPaths.isEmpty()) { - System.out.println("WARNING: Autos reference invalid path files:"); - for (String invalid : invalidPaths) { - System.out.println(" - " + invalid); - } - } else { - System.out.println("All autos only reference valid path files"); - } - } - - @Test - public void validateUniqueAutoNames() { - File[] files = getAutosFiles(); - if (files == null) return; - - List invalidNames = new ArrayList<>(); - - for (File file : files) { - String filename = getFilenamWithoutExtension(file); - - if (filename.startsWith(" ")) { - invalidNames.add(file.getName() + " (contains leading spaces)"); - } - if (filename.endsWith(" ")) { - invalidNames.add(file.getName() + " (contains trailing spaces)"); - } - - if (filename.matches(".*[0-9]$")) { - invalidNames.add(file.getName() + " (ends with number)"); - } - } - - if (!invalidNames.isEmpty()) { - System.out.println("WARNING: Invalid auto names:"); - for (String name : invalidNames) { - System.out.println(" - " + name); - } - } else { - System.out.println("All auto names are valid"); - } - } - - @Test - public void compareJSONContents() throws IOException { - File deployDir = Filesystem.getDeployDirectory(); - - if (!deployDir.exists() || !deployDir.isDirectory()) { - System.out.println("WARNING: Deploy directory not found"); - return; - } - - List allJsonFiles = getAllJsonFiles(deployDir); - - if (allJsonFiles.size() < 2) { - System.out.println("WARNING: Not enough files to compare"); - return; - } - - // Compare each file with every other file - for (int i = 0; i < allJsonFiles.size(); i++) { - String content1 = new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(i).toPath())); - - for (int j = i + 1; j < allJsonFiles.size(); j++) { - String content2 = - new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(j).toPath())); - - String file1Name = allJsonFiles.get(i).getName(); - String file2Name = allJsonFiles.get(j).getName(); - - if (content1.equals(content2)) { - System.out.println(file1Name + " and " + file2Name + " are IDENTICAL"); - } - } - } - } - - private List getAllJsonFiles(File dir) { - List jsonFiles = new ArrayList<>(); - collectJsonFiles(dir, jsonFiles); - return jsonFiles; - } - - private void collectJsonFiles(File dir, List jsonFiles) { - if (!dir.exists() || !dir.isDirectory()) { - return; - } - - File[] files = dir.listFiles(); - if (files == null) return; - - for (File file : files) { - if (file.isFile() && file.getName().endsWith(".json")) { - jsonFiles.add(file); - } else if (file.isDirectory()) { - collectJsonFiles(file, jsonFiles); - } - } - } -} diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java new file mode 100644 index 000000000..821b69b98 --- /dev/null +++ b/src/test/java/JSONComparisionTest.java @@ -0,0 +1,42 @@ +import edu.wpi.first.wpilibj.Filesystem; +import java.io.File; +import java.io.IOException; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class JSONComparisionTest { + + @Test + public void compareJSONContents() throws IOException { + File deployDir = Filesystem.getDeployDirectory(); + + if (!deployDir.exists() || !deployDir.isDirectory()) { + System.out.println("WARNING: Deploy directory not found"); + return; + } + + List allJsonFiles = TestConstantsUtils.getAllJsonFiles(deployDir); + + if (allJsonFiles.size() < 2) { + System.out.println("WARNING: Not enough files to compare"); + return; + } + + // Compare each file with every other file + for (int i = 0; i < allJsonFiles.size(); i++) { + String content1 = new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(i).toPath())); + + for (int j = i + 1; j < allJsonFiles.size(); j++) { + String content2 = + new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(j).toPath())); + + String file1Name = allJsonFiles.get(i).getName(); + String file2Name = allJsonFiles.get(j).getName(); + + if (content1.equals(content2)) { + System.out.println(file1Name + " and " + file2Name + " are IDENTICAL"); + } + } + } + } +} diff --git a/src/test/java/TestConstantsUtils.java b/src/test/java/TestConstantsUtils.java new file mode 100644 index 000000000..36b3c449d --- /dev/null +++ b/src/test/java/TestConstantsUtils.java @@ -0,0 +1,67 @@ +import frc.robot.subsystems.auto.BLine.BLineLogic; +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.BeforeEach; + +public class TestConstantsUtils { + public static final String PATHS_PATH = "src/main/deploy/autos/paths"; + + @BeforeEach + public void setup() { + BLineLogic.unitTestInit(); + } + + // ==================== METHODS ==================== + public static File[] getAutosFiles() { + File subfolder = new File(PATHS_PATH); + + if (!subfolder.exists() || !subfolder.isDirectory()) { + System.out.println("WARNING: Autos folder does not exist at: " + subfolder.getAbsolutePath()); + return null; + } + + File[] files = subfolder.listFiles(); + if (files == null || files.length == 0) { + System.out.println("WARNING: Autos folder is empty"); + return null; + } + + return files; + } + + public static String getFilenamWithoutExtension(File file) { + return file.getName().split("\\.")[0]; + } + + public static List getValidFileNames(File[] files) { + List validFileNames = new ArrayList<>(); + for (File file : files) { + validFileNames.add(getFilenamWithoutExtension(file)); + } + return validFileNames; + } + + public static List getAllJsonFiles(File dir) { + List jsonFiles = new ArrayList<>(); + collectJsonFiles(dir, jsonFiles); + return jsonFiles; + } + + public static void collectJsonFiles(File dir, List jsonFiles) { + if (!dir.exists() || !dir.isDirectory()) { + return; + } + + File[] files = dir.listFiles(); + if (files == null) return; + + for (File file : files) { + if (file.isFile() && file.getName().endsWith(".json")) { + jsonFiles.add(file); + } else if (file.isDirectory()) { + collectJsonFiles(file, jsonFiles); + } + } + } +} diff --git a/src/test/java/UniqueAutoNamesTest.java b/src/test/java/UniqueAutoNamesTest.java new file mode 100644 index 000000000..68aa85767 --- /dev/null +++ b/src/test/java/UniqueAutoNamesTest.java @@ -0,0 +1,38 @@ +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class UniqueAutoNamesTest { + @Test + public void validateUniqueAutoNames() { + File[] files = TestConstantsUtils.getAutosFiles(); + if (files == null) return; + + List invalidNames = new ArrayList<>(); + + for (File file : files) { + String filename = TestConstantsUtils.getFilenamWithoutExtension(file); + + if (filename.startsWith(" ")) { + invalidNames.add(file.getName() + " (contains leading spaces)"); + } + if (filename.endsWith(" ")) { + invalidNames.add(file.getName() + " (contains trailing spaces)"); + } + + if (filename.matches(".*[0-9]$")) { + invalidNames.add(file.getName() + " (ends with number)"); + } + } + + if (!invalidNames.isEmpty()) { + System.out.println("WARNING: Invalid auto names:"); + for (String name : invalidNames) { + System.out.println(" - " + name); + } + } else { + System.out.println("All auto names are valid"); + } + } +} diff --git a/src/test/java/UnusedFilesTest.java b/src/test/java/UnusedFilesTest.java new file mode 100644 index 000000000..40c6c9ccc --- /dev/null +++ b/src/test/java/UnusedFilesTest.java @@ -0,0 +1,32 @@ +import frc.robot.subsystems.auto.BLine.BLineLogic; +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class UnusedFilesTest { + @Test + public void unusedFileNames() { + File[] files = TestConstantsUtils.getAutosFiles(); + if (files == null) return; + + List validPathNames = BLineLogic.getBLinePathsNames(); + List unusedFiles = new ArrayList<>(); + + for (File file : files) { + String filename = TestConstantsUtils.getFilenamWithoutExtension(file); + if (!validPathNames.contains(filename)) { + unusedFiles.add(file.getName()); + } + } + + if (!unusedFiles.isEmpty()) { + System.out.println("WARNING: Unused BLine Paths found:"); + for (String file : unusedFiles) { + System.out.println(" - " + file); + } + } else { + System.out.println("All files in 'autos' folder are used"); + } + } +} From a322e28b6bf53db578ec1920723971f73cdcc1d4 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 20:03:36 -0700 Subject: [PATCH 105/114] more --- src/test/java/AutoFilePathsTest.java | 36 ++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java index 8b1378917..a55203010 100644 --- a/src/test/java/AutoFilePathsTest.java +++ b/src/test/java/AutoFilePathsTest.java @@ -1 +1,37 @@ +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.subsystems.auto.BLine.BLinePath; + +public class AutoFilePathsTest { + @Test + public void validatePathFiles() { + File[] files = TestConstantsUtils.getAutosFiles(); + if (files == null) return; + + List validFileNames = TestConstantsUtils.getValidFileNames(files); + List invalidPaths = new ArrayList<>(); + + for (BLinePath auto : BLineLogic.getBLinePaths()) { + for (String pathName : auto.getDisplayingNames()) { + if (!validFileNames.contains(pathName)) { + invalidPaths.add( + "Auto '" + auto.getDisplayName() + "' references missing path: " + pathName); + } + } + } + + if (!invalidPaths.isEmpty()) { + System.out.println("WARNING: Autos reference invalid path files:"); + for (String invalid : invalidPaths) { + System.out.println(" - " + invalid); + } + } else { + System.out.println("All autos only reference valid path files"); + } + } +} From eb8d3524f42c88f9fa4d3283356de131d83f6982 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Tue, 23 Jun 2026 20:04:04 -0700 Subject: [PATCH 106/114] spotless --- src/test/java/AutoFilePathsTest.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java index a55203010..8d86d4b1a 100644 --- a/src/test/java/AutoFilePathsTest.java +++ b/src/test/java/AutoFilePathsTest.java @@ -1,14 +1,12 @@ +import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.subsystems.auto.BLine.BLinePath; import java.io.File; import java.util.ArrayList; import java.util.List; - import org.junit.jupiter.api.Test; -import frc.robot.subsystems.auto.BLine.BLineLogic; -import frc.robot.subsystems.auto.BLine.BLinePath; - public class AutoFilePathsTest { - @Test + @Test public void validatePathFiles() { File[] files = TestConstantsUtils.getAutosFiles(); if (files == null) return; From 18f0a86b3e0b5e95bdd84cbbc3e42fa1d997404f Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Wed, 24 Jun 2026 14:36:31 -0700 Subject: [PATCH 107/114] renamed some files for clarity --- .../java/frc/robot/util/UnitTestHelpers.java} | 4 +++- src/test/java/AutoFilePathsTest.java | 5 +++-- src/test/java/JSONComparisionTest.java | 3 ++- src/test/java/UniqueAutoNamesTest.java | 5 +++-- src/test/java/UnusedFilesTest.java | 5 +++-- 5 files changed, 14 insertions(+), 8 deletions(-) rename src/{test/java/TestConstantsUtils.java => main/java/frc/robot/util/UnitTestHelpers.java} (96%) diff --git a/src/test/java/TestConstantsUtils.java b/src/main/java/frc/robot/util/UnitTestHelpers.java similarity index 96% rename from src/test/java/TestConstantsUtils.java rename to src/main/java/frc/robot/util/UnitTestHelpers.java index 36b3c449d..11e1928fb 100644 --- a/src/test/java/TestConstantsUtils.java +++ b/src/main/java/frc/robot/util/UnitTestHelpers.java @@ -1,10 +1,12 @@ +package frc.robot.util; + import frc.robot.subsystems.auto.BLine.BLineLogic; import java.io.File; import java.util.ArrayList; import java.util.List; import org.junit.jupiter.api.BeforeEach; -public class TestConstantsUtils { +public class UnitTestHelpers { public static final String PATHS_PATH = "src/main/deploy/autos/paths"; @BeforeEach diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java index 8d86d4b1a..bd458c9ee 100644 --- a/src/test/java/AutoFilePathsTest.java +++ b/src/test/java/AutoFilePathsTest.java @@ -1,5 +1,6 @@ import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.subsystems.auto.BLine.BLinePath; +import frc.robot.util.UnitTestHelpers; import java.io.File; import java.util.ArrayList; import java.util.List; @@ -8,10 +9,10 @@ public class AutoFilePathsTest { @Test public void validatePathFiles() { - File[] files = TestConstantsUtils.getAutosFiles(); + File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; - List validFileNames = TestConstantsUtils.getValidFileNames(files); + List validFileNames = UnitTestHelpers.getValidFileNames(files); List invalidPaths = new ArrayList<>(); for (BLinePath auto : BLineLogic.getBLinePaths()) { diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index 821b69b98..c903a4f27 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -1,4 +1,5 @@ import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.util.UnitTestHelpers; import java.io.File; import java.io.IOException; import java.util.List; @@ -15,7 +16,7 @@ public void compareJSONContents() throws IOException { return; } - List allJsonFiles = TestConstantsUtils.getAllJsonFiles(deployDir); + List allJsonFiles = UnitTestHelpers.getAllJsonFiles(deployDir); if (allJsonFiles.size() < 2) { System.out.println("WARNING: Not enough files to compare"); diff --git a/src/test/java/UniqueAutoNamesTest.java b/src/test/java/UniqueAutoNamesTest.java index 68aa85767..e0aaa9eed 100644 --- a/src/test/java/UniqueAutoNamesTest.java +++ b/src/test/java/UniqueAutoNamesTest.java @@ -1,3 +1,4 @@ +import frc.robot.util.UnitTestHelpers; import java.io.File; import java.util.ArrayList; import java.util.List; @@ -6,13 +7,13 @@ public class UniqueAutoNamesTest { @Test public void validateUniqueAutoNames() { - File[] files = TestConstantsUtils.getAutosFiles(); + File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; List invalidNames = new ArrayList<>(); for (File file : files) { - String filename = TestConstantsUtils.getFilenamWithoutExtension(file); + String filename = UnitTestHelpers.getFilenamWithoutExtension(file); if (filename.startsWith(" ")) { invalidNames.add(file.getName() + " (contains leading spaces)"); diff --git a/src/test/java/UnusedFilesTest.java b/src/test/java/UnusedFilesTest.java index 40c6c9ccc..a0119a5a5 100644 --- a/src/test/java/UnusedFilesTest.java +++ b/src/test/java/UnusedFilesTest.java @@ -1,4 +1,5 @@ import frc.robot.subsystems.auto.BLine.BLineLogic; +import frc.robot.util.UnitTestHelpers; import java.io.File; import java.util.ArrayList; import java.util.List; @@ -7,14 +8,14 @@ public class UnusedFilesTest { @Test public void unusedFileNames() { - File[] files = TestConstantsUtils.getAutosFiles(); + File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; List validPathNames = BLineLogic.getBLinePathsNames(); List unusedFiles = new ArrayList<>(); for (File file : files) { - String filename = TestConstantsUtils.getFilenamWithoutExtension(file); + String filename = UnitTestHelpers.getFilenamWithoutExtension(file); if (!validPathNames.contains(filename)) { unusedFiles.add(file.getName()); } From 681a44d558692518b436c5afdba8b9226c7d295a Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 28 Jun 2026 20:08:38 -0700 Subject: [PATCH 108/114] sort of hashing --- .../java/frc/robot/util/UnitTestHelpers.java | 43 ++++++++++++++- src/test/java/AutoFilePathsTest.java | 1 + src/test/java/JSONComparisionTest.java | 55 +++++++++++++------ src/test/java/UniqueAutoNamesTest.java | 2 + src/test/java/UnusedFilesTest.java | 1 + 5 files changed, 84 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/util/UnitTestHelpers.java b/src/main/java/frc/robot/util/UnitTestHelpers.java index 11e1928fb..9acba8497 100644 --- a/src/main/java/frc/robot/util/UnitTestHelpers.java +++ b/src/main/java/frc/robot/util/UnitTestHelpers.java @@ -1,15 +1,22 @@ package frc.robot.util; +import com.google.gson.Gson; +import com.google.gson.JsonObject; +import com.google.gson.JsonParser; +import edu.wpi.first.util.struct.parser.ParseException; import frc.robot.subsystems.auto.BLine.BLineLogic; import java.io.File; +import java.io.FileReader; +import java.io.IOException; import java.util.ArrayList; import java.util.List; -import org.junit.jupiter.api.BeforeEach; +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; public class UnitTestHelpers { public static final String PATHS_PATH = "src/main/deploy/autos/paths"; - @BeforeEach + // @BeforeEach public void setup() { BLineLogic.unitTestInit(); } @@ -66,4 +73,36 @@ public static void collectJsonFiles(File dir, List jsonFiles) { } } } + + // Source - https://stackoverflow.com/a/68606076 + // Posted by Aman Garg + // Retrieved 2026-06-28, License - CC BY-SA 4.0 + + @SuppressWarnings("unchecked") + public static Object getObjectFromJsonFile(String jsonData, Class classObject) { + Gson gson = new Gson(); + JsonParser parser = new JsonParser(); + JsonObject object = (JsonObject) parser.parse(jsonData); + return gson.fromJson(object, classObject); + } + + // Source - https://stackoverflow.com/a/42440060 + // Posted by Ankur Mahajan + // Retrieved 2026-06-28, License - CC BY-SA 3.0 + + public static JSONObject JSONtoObject(File file) + throws ParseException, org.json.simple.parser.ParseException { + try { + JSONParser parser = new JSONParser(); + // Use JSONObject for simple JSON and JSONArray for array of JSON. + JSONObject data = + (JSONObject) + parser.parse(new FileReader(file.getAbsolutePath())); // path to the JSON file. + return data; + // String json = data.toJSONString(); + } catch (IOException e) { + e.printStackTrace(); + return null; + } + } } diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java index bd458c9ee..1fff0d5d5 100644 --- a/src/test/java/AutoFilePathsTest.java +++ b/src/test/java/AutoFilePathsTest.java @@ -9,6 +9,7 @@ public class AutoFilePathsTest { @Test public void validatePathFiles() { + BLineLogic.unitTestInit(); File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index c903a4f27..279ca0156 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -1,15 +1,22 @@ -import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.util.struct.parser.ParseException; +import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.UnitTestHelpers; import java.io.File; -import java.io.IOException; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.HashSet; import java.util.List; import org.junit.jupiter.api.Test; public class JSONComparisionTest { + private final int MINIMUM_FILE_COUNT = 2; + @SuppressWarnings("unlikely-arg-type") @Test - public void compareJSONContents() throws IOException { - File deployDir = Filesystem.getDeployDirectory(); + public void compareJSONContents() + throws ParseException, ParseException, org.json.simple.parser.ParseException { + BLineLogic.unitTestInit(); + File deployDir = new File(UnitTestHelpers.PATHS_PATH); if (!deployDir.exists() || !deployDir.isDirectory()) { System.out.println("WARNING: Deploy directory not found"); @@ -17,27 +24,43 @@ public void compareJSONContents() throws IOException { } List allJsonFiles = UnitTestHelpers.getAllJsonFiles(deployDir); + HashMap contents = new HashMap<>(); + HashMap filesWithDuplicateContents = new HashMap<>(); - if (allJsonFiles.size() < 2) { + if (allJsonFiles.size() < MINIMUM_FILE_COUNT) { System.out.println("WARNING: Not enough files to compare"); return; } - // Compare each file with every other file - for (int i = 0; i < allJsonFiles.size(); i++) { - String content1 = new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(i).toPath())); + HashSet seenHashes = new HashSet<>(); - for (int j = i + 1; j < allJsonFiles.size(); j++) { - String content2 = - new String(java.nio.file.Files.readAllBytes(allJsonFiles.get(j).toPath())); + for (File file : allJsonFiles) { + Object parsedFile = UnitTestHelpers.JSONtoObject(file); - String file1Name = allJsonFiles.get(i).getName(); - String file2Name = allJsonFiles.get(j).getName(); + int hash = parsedFile.hashCode(); - if (content1.equals(content2)) { - System.out.println(file1Name + " and " + file2Name + " are IDENTICAL"); - } + if (!seenHashes.contains(hash)) { + contents.put(file.getName(), parsedFile.hashCode()); + + seenHashes.add(hash); + } else { + + filesWithDuplicateContents.put(file.getName(), file); + } + } + // STICKLER FOR GRAMMAR + if (filesWithDuplicateContents.size() < 1) { + System.out.println("No duplicate file contents"); + } else if (filesWithDuplicateContents.size() > 1) { + List iterableContents = new ArrayList<>(); + iterableContents.addAll(filesWithDuplicateContents.values()); + + for (int i = 0; i < iterableContents.size(); i++) { + System.out.println( + iterableContents.get(i).getName() + " has duplicate file contents with " + contents); } + } else { + System.out.println(filesWithDuplicateContents.values() + " has duplicate file contents"); } } } diff --git a/src/test/java/UniqueAutoNamesTest.java b/src/test/java/UniqueAutoNamesTest.java index e0aaa9eed..46d794b79 100644 --- a/src/test/java/UniqueAutoNamesTest.java +++ b/src/test/java/UniqueAutoNamesTest.java @@ -1,3 +1,4 @@ +import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.UnitTestHelpers; import java.io.File; import java.util.ArrayList; @@ -7,6 +8,7 @@ public class UniqueAutoNamesTest { @Test public void validateUniqueAutoNames() { + BLineLogic.unitTestInit(); File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; diff --git a/src/test/java/UnusedFilesTest.java b/src/test/java/UnusedFilesTest.java index a0119a5a5..490d79108 100644 --- a/src/test/java/UnusedFilesTest.java +++ b/src/test/java/UnusedFilesTest.java @@ -8,6 +8,7 @@ public class UnusedFilesTest { @Test public void unusedFileNames() { + BLineLogic.unitTestInit(); File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; From 8c30fb9c708d81522ad456af35297364ce6fa77c Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 28 Jun 2026 20:12:49 -0700 Subject: [PATCH 109/114] working just a bit better --- src/main/java/frc/robot/Robot.java | 2 +- src/test/java/JSONComparisionTest.java | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 20ce6d2dc..0be78a1a0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -116,7 +116,7 @@ protected Robot() { controls = new Controls(subsystems, m_simWrapper); if (DRIVEBASE_ENABLED) { - if (Robot.isSimulation()) { + if (!Robot.isSimulation()) { robotSim = new RobotSim(subsystems.drivebaseSubsystem); } else { robotSim = null; diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index 279ca0156..e9288cc5e 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -49,18 +49,18 @@ public void compareJSONContents() } } // STICKLER FOR GRAMMAR - if (filesWithDuplicateContents.size() < 1) { + List iterableContents = new ArrayList<>(); + iterableContents.addAll(filesWithDuplicateContents.values()); + if (iterableContents.size() < 1) { System.out.println("No duplicate file contents"); - } else if (filesWithDuplicateContents.size() > 1) { - List iterableContents = new ArrayList<>(); - iterableContents.addAll(filesWithDuplicateContents.values()); + } else if (iterableContents.size() > 1) { for (int i = 0; i < iterableContents.size(); i++) { System.out.println( - iterableContents.get(i).getName() + " has duplicate file contents with " + contents); + iterableContents.get(i).getName() + " has duplicate file contents with "); } } else { - System.out.println(filesWithDuplicateContents.values() + " has duplicate file contents"); + System.out.println(iterableContents.get(0).getName() + " has duplicate file contents"); } } } From f3d394f1332ed65c39f94e8b9d66f78059408095 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 28 Jun 2026 20:19:22 -0700 Subject: [PATCH 110/114] almost reduced runtime complexity --- src/test/java/JSONComparisionTest.java | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index e9288cc5e..4b8a6ca79 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -26,6 +26,7 @@ public void compareJSONContents() List allJsonFiles = UnitTestHelpers.getAllJsonFiles(deployDir); HashMap contents = new HashMap<>(); HashMap filesWithDuplicateContents = new HashMap<>(); + HashMap fileStringsWithDuplicateContents = new HashMap<>(); if (allJsonFiles.size() < MINIMUM_FILE_COUNT) { System.out.println("WARNING: Not enough files to compare"); @@ -41,7 +42,7 @@ public void compareJSONContents() if (!seenHashes.contains(hash)) { contents.put(file.getName(), parsedFile.hashCode()); - + fileStringsWithDuplicateContents.put(file.getName(), file); seenHashes.add(hash); } else { @@ -51,16 +52,23 @@ public void compareJSONContents() // STICKLER FOR GRAMMAR List iterableContents = new ArrayList<>(); iterableContents.addAll(filesWithDuplicateContents.values()); + List arrContents = new ArrayList<>(); + arrContents.addAll(fileStringsWithDuplicateContents.values()); if (iterableContents.size() < 1) { System.out.println("No duplicate file contents"); } else if (iterableContents.size() > 1) { for (int i = 0; i < iterableContents.size(); i++) { System.out.println( - iterableContents.get(i).getName() + " has duplicate file contents with "); + iterableContents.get(i).getName() + + " has duplicate file contents with " + + arrContents.get(i).getName()); } } else { - System.out.println(iterableContents.get(0).getName() + " has duplicate file contents"); + System.out.println( + iterableContents.get(0).getName() + + " has duplicate file contents with " + + arrContents.get(0).getName()); } } } From 76c7b52b0164c36320dc75d9b3ff53cee3e18adf Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 28 Jun 2026 20:47:43 -0700 Subject: [PATCH 111/114] reduced runtime complexity? --- src/test/java/JSONComparisionTest.java | 34 ++++++++++++-------------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index 4b8a6ca79..bde5735b6 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -4,7 +4,6 @@ import java.io.File; import java.util.ArrayList; import java.util.HashMap; -import java.util.HashSet; import java.util.List; import org.junit.jupiter.api.Test; @@ -24,26 +23,25 @@ public void compareJSONContents() } List allJsonFiles = UnitTestHelpers.getAllJsonFiles(deployDir); - HashMap contents = new HashMap<>(); + HashMap contents = new HashMap<>(); HashMap filesWithDuplicateContents = new HashMap<>(); - HashMap fileStringsWithDuplicateContents = new HashMap<>(); if (allJsonFiles.size() < MINIMUM_FILE_COUNT) { System.out.println("WARNING: Not enough files to compare"); return; } - HashSet seenHashes = new HashSet<>(); + HashMap seenHashes = new HashMap<>(); for (File file : allJsonFiles) { Object parsedFile = UnitTestHelpers.JSONtoObject(file); int hash = parsedFile.hashCode(); - if (!seenHashes.contains(hash)) { - contents.put(file.getName(), parsedFile.hashCode()); - fileStringsWithDuplicateContents.put(file.getName(), file); - seenHashes.add(hash); + if (!seenHashes.containsKey(hash)) { + contents.put(file.getName(), file); + + seenHashes.put(hash, file); } else { filesWithDuplicateContents.put(file.getName(), file); @@ -53,22 +51,22 @@ public void compareJSONContents() List iterableContents = new ArrayList<>(); iterableContents.addAll(filesWithDuplicateContents.values()); List arrContents = new ArrayList<>(); - arrContents.addAll(fileStringsWithDuplicateContents.values()); + arrContents.addAll(contents.values()); + List arrHash = new ArrayList<>(); + for (File file : seenHashes.values()) { + arrHash.add(file.hashCode()); + } if (iterableContents.size() < 1) { System.out.println("No duplicate file contents"); - } else if (iterableContents.size() > 1) { + } else { + + for (File file : filesWithDuplicateContents.values()) { - for (int i = 0; i < iterableContents.size(); i++) { System.out.println( - iterableContents.get(i).getName() + seenHashes.get(UnitTestHelpers.JSONtoObject(file).hashCode()).getName() + " has duplicate file contents with " - + arrContents.get(i).getName()); + + file.getName()); } - } else { - System.out.println( - iterableContents.get(0).getName() - + " has duplicate file contents with " - + arrContents.get(0).getName()); } } } From 7508b377a3f077fc810a3c5e9ed4eee22600b09e Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 28 Jun 2026 20:51:14 -0700 Subject: [PATCH 112/114] cleaned up code --- src/test/java/JSONComparisionTest.java | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index bde5735b6..f859f8c9c 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -2,7 +2,6 @@ import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.UnitTestHelpers; import java.io.File; -import java.util.ArrayList; import java.util.HashMap; import java.util.List; import org.junit.jupiter.api.Test; @@ -10,7 +9,6 @@ public class JSONComparisionTest { private final int MINIMUM_FILE_COUNT = 2; - @SuppressWarnings("unlikely-arg-type") @Test public void compareJSONContents() throws ParseException, ParseException, org.json.simple.parser.ParseException { @@ -23,8 +21,6 @@ public void compareJSONContents() } List allJsonFiles = UnitTestHelpers.getAllJsonFiles(deployDir); - HashMap contents = new HashMap<>(); - HashMap filesWithDuplicateContents = new HashMap<>(); if (allJsonFiles.size() < MINIMUM_FILE_COUNT) { System.out.println("WARNING: Not enough files to compare"); @@ -39,29 +35,9 @@ public void compareJSONContents() int hash = parsedFile.hashCode(); if (!seenHashes.containsKey(hash)) { - contents.put(file.getName(), file); seenHashes.put(hash, file); } else { - - filesWithDuplicateContents.put(file.getName(), file); - } - } - // STICKLER FOR GRAMMAR - List iterableContents = new ArrayList<>(); - iterableContents.addAll(filesWithDuplicateContents.values()); - List arrContents = new ArrayList<>(); - arrContents.addAll(contents.values()); - List arrHash = new ArrayList<>(); - for (File file : seenHashes.values()) { - arrHash.add(file.hashCode()); - } - if (iterableContents.size() < 1) { - System.out.println("No duplicate file contents"); - } else { - - for (File file : filesWithDuplicateContents.values()) { - System.out.println( seenHashes.get(UnitTestHelpers.JSONtoObject(file).hashCode()).getName() + " has duplicate file contents with " From fba40bda2421bbde19b2a93dab7dfef6f7205bd2 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Sun, 28 Jun 2026 21:34:54 -0700 Subject: [PATCH 113/114] works --- src/test/java/AutoFilePathsTest.java | 2 +- src/test/java/JSONComparisionTest.java | 9 ++++----- src/test/java/UniqueAutoNamesTest.java | 3 +-- src/test/java/UnusedFilesTest.java | 2 +- 4 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/test/java/AutoFilePathsTest.java b/src/test/java/AutoFilePathsTest.java index 1fff0d5d5..f52397cee 100644 --- a/src/test/java/AutoFilePathsTest.java +++ b/src/test/java/AutoFilePathsTest.java @@ -9,7 +9,7 @@ public class AutoFilePathsTest { @Test public void validatePathFiles() { - BLineLogic.unitTestInit(); + UnitTestHelpers.setup(); File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; diff --git a/src/test/java/JSONComparisionTest.java b/src/test/java/JSONComparisionTest.java index f859f8c9c..1ce958465 100644 --- a/src/test/java/JSONComparisionTest.java +++ b/src/test/java/JSONComparisionTest.java @@ -27,19 +27,18 @@ public void compareJSONContents() return; } - HashMap seenHashes = new HashMap<>(); - + HashMap Hashes = new HashMap<>(); for (File file : allJsonFiles) { Object parsedFile = UnitTestHelpers.JSONtoObject(file); int hash = parsedFile.hashCode(); - if (!seenHashes.containsKey(hash)) { + if (!Hashes.containsKey(hash)) { - seenHashes.put(hash, file); + Hashes.put(hash, file); } else { System.out.println( - seenHashes.get(UnitTestHelpers.JSONtoObject(file).hashCode()).getName() + Hashes.get(parsedFile.hashCode()).getName() + " has duplicate file contents with " + file.getName()); } diff --git a/src/test/java/UniqueAutoNamesTest.java b/src/test/java/UniqueAutoNamesTest.java index 46d794b79..f08707a8a 100644 --- a/src/test/java/UniqueAutoNamesTest.java +++ b/src/test/java/UniqueAutoNamesTest.java @@ -1,4 +1,3 @@ -import frc.robot.subsystems.auto.BLine.BLineLogic; import frc.robot.util.UnitTestHelpers; import java.io.File; import java.util.ArrayList; @@ -8,7 +7,7 @@ public class UniqueAutoNamesTest { @Test public void validateUniqueAutoNames() { - BLineLogic.unitTestInit(); + UnitTestHelpers.setup(); File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; diff --git a/src/test/java/UnusedFilesTest.java b/src/test/java/UnusedFilesTest.java index 490d79108..16f4cd8a8 100644 --- a/src/test/java/UnusedFilesTest.java +++ b/src/test/java/UnusedFilesTest.java @@ -8,7 +8,7 @@ public class UnusedFilesTest { @Test public void unusedFileNames() { - BLineLogic.unitTestInit(); + UnitTestHelpers.setup(); File[] files = UnitTestHelpers.getAutosFiles(); if (files == null) return; From 8d830d3db68986bd0fa8c7ad98d45ba4f3f0ce30 Mon Sep 17 00:00:00 2001 From: SnappleRamen <130863864+SnappleRamen@users.noreply.github.com> Date: Thu, 13 Aug 2026 21:29:03 -0700 Subject: [PATCH 114/114] fixed weird bugs --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/util/UnitTestHelpers.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0be78a1a0..20ce6d2dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -116,7 +116,7 @@ protected Robot() { controls = new Controls(subsystems, m_simWrapper); if (DRIVEBASE_ENABLED) { - if (!Robot.isSimulation()) { + if (Robot.isSimulation()) { robotSim = new RobotSim(subsystems.drivebaseSubsystem); } else { robotSim = null; diff --git a/src/main/java/frc/robot/util/UnitTestHelpers.java b/src/main/java/frc/robot/util/UnitTestHelpers.java index 9acba8497..8585942f8 100644 --- a/src/main/java/frc/robot/util/UnitTestHelpers.java +++ b/src/main/java/frc/robot/util/UnitTestHelpers.java @@ -17,7 +17,7 @@ public class UnitTestHelpers { public static final String PATHS_PATH = "src/main/deploy/autos/paths"; // @BeforeEach - public void setup() { + public static void setup() { BLineLogic.unitTestInit(); }