From f692b4511e260c3c8534b79acb575fd32517c4a0 Mon Sep 17 00:00:00 2001 From: Victor C Date: Tue, 7 Apr 2026 20:41:34 -0700 Subject: [PATCH 01/16] elastic competition dashboard --- elastic-layout/robot-configuration.json | 85 +++++++++++++++++++++++++ src/main/java/competition/Robot.java | 49 ++++++++++++++ 2 files changed, 134 insertions(+) diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index 098ff6d8..2201e864 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -583,6 +583,20 @@ "data_type": "string", "show_submit_button": true } + }, + { + "title": "ServoTargetPositionNormalized", + "x": 1280.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/HoodSubsystem/ServoTargetPositionNormalized", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } } ] } @@ -1733,6 +1747,77 @@ } ] } + }, + { + "name": "Competition", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 768.0, + "height": 896.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 90.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Current Match Time", + "x": 768.0, + "y": 0.0, + "width": 384.0, + "height": 384.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Current Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "Current Mode", + "x": 768.0, + "y": 640.0, + "width": 256.0, + "height": 256.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/Current Mode", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Match Shift", + "x": 768.0, + "y": 384.0, + "width": 256.0, + "height": 256.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/Match Shift", + "period": 0.06, + "data_type": "string" + } + } + ] + } } ] } \ No newline at end of file diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index a17042c2..e4dfa2b4 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -1,6 +1,10 @@ package competition; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -21,10 +25,13 @@ import xbot.common.math.FieldPose; import xbot.common.subsystems.pose.BasePoseSubsystem; +import java.sql.Driver; + public class Robot extends BaseRobot { Logger log = LogManager.getLogger(Robot.class); public static final double LOOP_INTERVAL = 0.04; + private final Field2d field = new Field2d(); BaseSimulator simulator; OperatorInterface oi; @@ -46,6 +53,7 @@ protected void initializeSystems() { getInjectorComponent().voltageMonitorSubsystem(); getInjectorComponent().climberSubsystem(); + if (BaseRobot.isSimulation()) { simulator = getInjectorComponent().simulator(); } @@ -69,6 +77,9 @@ protected void initializeSystems() { getInjectorComponent().superstructureMechanismSubsystem(); CommandScheduler.getInstance().schedule(getInjectorComponent().whenShooterReadyRumbleCommand()); + + SmartDashboard.putData("Field", field); + } protected BaseRobotComponent createDaggerComponent() { @@ -155,5 +166,43 @@ protected void sharedPeriodic() { if (this.oi != null) { this.oi.periodic(); } + + String mode; + String matchShift; + + if (DriverStation.isAutonomous()) { + mode = "Autonomous"; + } else if (DriverStation.isTeleop()) { + mode = "Teleoperated"; + } else if (DriverStation.isTeleop() && DriverStation.getMatchTime() <= 30) { + mode = "Endgame"; + } else if (DriverStation.isDisabled()) { + mode = "Disabled"; + } else { + mode = "Unknown"; + } + + if (DriverStation.getMatchTime() <= 140 && DriverStation.getMatchTime() >= 130) { + matchShift = "Transition Shift"; + } else if (DriverStation.getMatchTime() <= 130 && DriverStation.getMatchTime() >= 105) { + matchShift = "Shift 1"; + } else if (DriverStation.getMatchTime() <= 105 && DriverStation.getMatchTime() >= 80) { + matchShift = "Shift 2"; + } else if (DriverStation.getMatchTime() <= 80 && DriverStation.getMatchTime() >= 55) { + matchShift = "Shift 3"; + } else if (DriverStation.getMatchTime() <= 55 && DriverStation.getMatchTime() >= 30) { + matchShift = "Shift 4"; + } else if (DriverStation.getMatchTime() <= 30 && DriverStation.getMatchTime() >= 0) { + matchShift = "Endgame"; + } else { + matchShift = "Unknown"; + } + + PoseSubsystem pose = (PoseSubsystem) getInjectorComponent().poseSubsystem(); + field.setRobotPose(pose.getCurrentPose2d()); + + SmartDashboard.putString("Current Mode", mode); + SmartDashboard.putNumber("Current Match Time", DriverStation.getMatchTime()); + SmartDashboard.putString("Match Shift", matchShift); } } From b52601b385e348261c56c8bdd57afac65aaa50b9 Mon Sep 17 00:00:00 2001 From: Victor C Date: Thu, 9 Apr 2026 14:02:27 -0700 Subject: [PATCH 02/16] detector for is hood down or not --- src/main/java/competition/Robot.java | 19 +------------------ .../subsystems/hood/HoodSubsystem.java | 4 ++++ 2 files changed, 5 insertions(+), 18 deletions(-) diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index f095d84d..5f48157b 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -1,7 +1,6 @@ package competition; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,14 +24,11 @@ import xbot.common.math.FieldPose; import xbot.common.subsystems.pose.BasePoseSubsystem; -import java.sql.Driver; - public class Robot extends BaseRobot { Logger log = LogManager.getLogger(Robot.class); public static final double LOOP_INTERVAL = 0.04; private final Field2d field = new Field2d(); - BaseSimulator simulator; OperatorInterface oi; @@ -172,21 +168,8 @@ protected void sharedPeriodic() { this.oi.periodic(); } - String mode; String matchShift; - if (DriverStation.isAutonomous()) { - mode = "Autonomous"; - } else if (DriverStation.isTeleop()) { - mode = "Teleoperated"; - } else if (DriverStation.isTeleop() && DriverStation.getMatchTime() <= 30) { - mode = "Endgame"; - } else if (DriverStation.isDisabled()) { - mode = "Disabled"; - } else { - mode = "Unknown"; - } - if (DriverStation.getMatchTime() <= 140 && DriverStation.getMatchTime() >= 130) { matchShift = "Transition Shift"; } else if (DriverStation.getMatchTime() <= 130 && DriverStation.getMatchTime() >= 105) { @@ -206,8 +189,8 @@ protected void sharedPeriodic() { PoseSubsystem pose = (PoseSubsystem) getInjectorComponent().poseSubsystem(); field.setRobotPose(pose.getCurrentPose2d()); - SmartDashboard.putString("Current Mode", mode); SmartDashboard.putNumber("Current Match Time", DriverStation.getMatchTime()); SmartDashboard.putString("Match Shift", matchShift); + SmartDashboard.putBoolean("Is Hood Down", getInjectorComponent().hoodSubsystem().isHoodDown()); } } diff --git a/src/main/java/competition/subsystems/hood/HoodSubsystem.java b/src/main/java/competition/subsystems/hood/HoodSubsystem.java index 847fd837..1934e458 100644 --- a/src/main/java/competition/subsystems/hood/HoodSubsystem.java +++ b/src/main/java/competition/subsystems/hood/HoodSubsystem.java @@ -101,6 +101,10 @@ public void retract() { setTargetValue(getTargetValue()); } + public boolean isHoodDown() { + return hoodServoLeft.getNormalizedCurrentPosition() <= 0.05; + } + public void runServo() { if (hoodServoLeft != null && hoodServoRight != null) { hoodServoLeft.setNormalizedTargetPosition(servoTargetNormalized.get()); From 6f564dc662a70cce62d1528da0ca842501e2e550 Mon Sep 17 00:00:00 2001 From: Victor C Date: Thu, 9 Apr 2026 14:50:38 -0700 Subject: [PATCH 03/16] fix merge conflicts --- src/main/java/competition/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index f7113e07..72f9b020 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -28,6 +28,7 @@ public class Robot extends BaseRobot { Logger log = LogManager.getLogger(Robot.class); public static final double LOOP_INTERVAL = 0.04; + private final Field2d field = new Field2d(); BaseSimulator simulator; OperatorInterface oi; @@ -74,7 +75,6 @@ protected void initializeSystems() { getInjectorComponent().superstructureMechanismSubsystem(); CommandScheduler.getInstance().schedule(getInjectorComponent().gamepadRumbleCommand()); - CommandScheduler.getInstance().schedule(getInjectorComponent().whenShooterReadyRumbleCommand()); SmartDashboard.putData("Field", field); From feba9fd61d57e197b9105428e37d49e2a3ee18b1 Mon Sep 17 00:00:00 2001 From: Victor C Date: Thu, 9 Apr 2026 15:01:18 -0700 Subject: [PATCH 04/16] have a blue and red alliance competition file --- elastic-layout/robot-configuration.json | 121 ++++++++++++++++++------ 1 file changed, 93 insertions(+), 28 deletions(-) diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index 8839b36b..0c252ef6 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -158,20 +158,6 @@ "data_type": "string", "show_submit_button": false } - }, - { - "title": "Collect and shoot twice.", - "x": 512.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/Collect and shoot twice.", - "period": 0.06, - "show_type": true, - "maximize_button_space": false - } } ] } @@ -1763,7 +1749,7 @@ } }, { - "name": "Competition", + "name": "Red Alliance Competition", "grid_layout": { "layouts": [], "containers": [ @@ -1771,7 +1757,7 @@ "title": "Field", "x": 0.0, "y": 0.0, - "width": 768.0, + "width": 384.0, "height": 896.0, "type": "Field", "properties": { @@ -1790,10 +1776,10 @@ }, { "title": "Current Match Time", - "x": 768.0, + "x": 384.0, "y": 0.0, - "width": 384.0, - "height": 384.0, + "width": 768.0, + "height": 896.0, "type": "Match Time", "properties": { "topic": "/SmartDashboard/Current Match Time", @@ -1805,30 +1791,109 @@ } }, { - "title": "Current Mode", - "x": 768.0, - "y": 640.0, - "width": 256.0, - "height": 256.0, + "title": "Match Shift", + "x": 1152.0, + "y": 384.0, + "width": 512.0, + "height": 512.0, "type": "Large Text Display", "properties": { - "topic": "/SmartDashboard/Current Mode", + "topic": "/SmartDashboard/Match Shift", "period": 0.06, "data_type": "string" } }, + { + "title": "Is Hood Down", + "x": 1152.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Is Hood Down", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Blue Alliance Competition", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 896.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": -90.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Current Match Time", + "x": 384.0, + "y": 0.0, + "width": 768.0, + "height": 896.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Current Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, { "title": "Match Shift", - "x": 768.0, + "x": 1152.0, "y": 384.0, - "width": 256.0, - "height": 256.0, + "width": 512.0, + "height": 512.0, "type": "Large Text Display", "properties": { "topic": "/SmartDashboard/Match Shift", "period": 0.06, "data_type": "string" } + }, + { + "title": "Is Hood Down", + "x": 1152.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Is Hood Down", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } } ] } From 2c239149c1d87ed41c94061ba2d8d064e58c5338 Mon Sep 17 00:00:00 2001 From: Victor C Date: Thu, 9 Apr 2026 17:06:45 -0700 Subject: [PATCH 05/16] finished autonomous menu --- elastic-layout/robot-configuration.json | 31 +++++++++++++++++-- src/main/java/competition/Robot.java | 22 +++++++++++++ .../components/BaseRobotComponent.java | 11 +++++-- 3 files changed, 60 insertions(+), 4 deletions(-) diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index 0c252ef6..4432ce0d 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -1793,9 +1793,9 @@ { "title": "Match Shift", "x": 1152.0, - "y": 384.0, + "y": 640.0, "width": 512.0, - "height": 512.0, + "height": 256.0, "type": "Large Text Display", "properties": { "topic": "/SmartDashboard/Match Shift", @@ -1819,6 +1819,33 @@ "true_icon": "None", "false_icon": "None" } + }, + { + "title": "Auto Selector", + "x": 1152.0, + "y": 384.0, + "width": 512.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Selector", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "selected", + "x": 1152.0, + "y": 512.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Auto Selector/selected", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } } ] } diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 7dc05761..50f00561 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -1,9 +1,15 @@ package competition; +import com.pathplanner.lib.commands.PathPlannerAuto; +import competition.auto_programs.CollectAndShootTwiceCommand; +import competition.auto_programs.ShootFromHubCommandGroup; +import competition.auto_programs.ShootFromTrenchCommandGroup; import edu.wpi.first.wpilibj.DriverStation; 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; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -29,6 +35,8 @@ public class Robot extends BaseRobot { public static final double LOOP_INTERVAL = 0.04; private final Field2d field = new Field2d(); + private final SendableChooser autoChooser = new SendableChooser<>(); + BaseSimulator simulator; OperatorInterface oi; @@ -51,6 +59,16 @@ protected void initializeSystems() { getInjectorComponent().voltageMonitorSubsystem(); getInjectorComponent().climberSubsystem(); + autoChooser.setDefaultOption("Normal Bump Auto Right", new PathPlannerAuto("NormalBumpAutoRight")); + autoChooser.addOption("Normal Bump Auto Left", new PathPlannerAuto("NormalBumpAutoLeft")); + + autoChooser.addOption("Collect and Shoot Twice", getInjectorComponent().collectAndShootTwiceCommand()); + autoChooser.addOption("Shoot From Hub", getInjectorComponent().shootFromHubCommandGroup()); + autoChooser.addOption("Shoot From Trench", getInjectorComponent().shootFromTrenchCommandGroup()); + + SmartDashboard.putData("Auto Selector", autoChooser); + + if (BaseRobot.isSimulation()) { simulator = getInjectorComponent().simulator(); @@ -121,6 +139,10 @@ public void autonomousInit() { super.autonomousInit(); var pose = (PoseSubsystem) getInjectorComponent().poseSubsystem(); CommandScheduler.getInstance().schedule(pose.getResetTranslationToVisionEstimateCommand()); + Command autonomousCommand = autoChooser.getSelected(); + if (autonomousCommand != null) { + CommandScheduler.getInstance().schedule(autonomousCommand); + } if(BaseRobot.isSimulation()) { getInjectorComponent().simulator().resetForAuto(); diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 6374a317..26891d64 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -1,6 +1,8 @@ package competition.injection.components; import competition.ConfigurePathPlannerLib; +import competition.auto_programs.CollectAndShootTwiceCommand; +import competition.auto_programs.ShootFromHubCommandGroup; import competition.auto_programs.ShootFromTrenchCommandGroup; import competition.operator_interface.OperatorCommandMap; import competition.operator_interface.OperatorInterface; @@ -56,8 +58,6 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract ClimberSubsystem climberSubsystem(); - public abstract ShootFromTrenchCommandGroup shootFromTrenchCommandGroup(); - public abstract OperatorInterface operatorInterface(); public abstract GamepadRumbleCommand gamepadRumbleCommand(); @@ -65,4 +65,11 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract SuperstructureMechanismSubsystem superstructureMechanismSubsystem(); public abstract ConfigurePathPlannerLib configurePathPlannerLib(); + + public abstract CollectAndShootTwiceCommand collectAndShootTwiceCommand(); + + public abstract ShootFromHubCommandGroup shootFromHubCommandGroup(); + + public abstract ShootFromTrenchCommandGroup shootFromTrenchCommandGroup(); + } From fac440b03c1b423cbb99f9d08175f01489e59e5a Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 10:09:45 -0700 Subject: [PATCH 06/16] depo auto :) --- .../pathplanner/autos/HubToDepoToTower.auto | 69 ++++++++++++++ .../autos/NormalBumpAutoRight.auto | 21 +---- .../pathplanner/paths/DepoToTowerPath.path | 75 +++++++++++++++ .../pathplanner/paths/HubToDepoPath.path | 94 +++++++++++++++++++ .../ppl/LeftBumpAutoCommand.java | 3 +- .../ppl/RightBumpAutoCommand.java | 3 +- 6 files changed, 243 insertions(+), 22 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/HubToDepoToTower.auto create mode 100644 src/main/deploy/pathplanner/paths/DepoToTowerPath.path create mode 100644 src/main/deploy/pathplanner/paths/HubToDepoPath.path diff --git a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto new file mode 100644 index 00000000..9d16f489 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto @@ -0,0 +1,69 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WarmupShooterNear" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "AimAndShootFromHere" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "HubToDepoPath" + } + }, + { + "type": "path", + "data": { + "pathName": "DepoToTowerPath" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AimAndShootFromHere" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto b/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto index 553a63b4..a3c81153 100644 --- a/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto @@ -32,26 +32,7 @@ { "type": "path", "data": { - "pathName": "SecondCycleRight" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 4.5 - } - }, - { - "type": "named", - "data": { - "name": "AimAndShootFromHere" - } - } - ] + "pathName": "SecondCycleRightNoShooting" } } ] diff --git a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path new file mode 100644 index 00000000..780e0541 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.749, + "y": 5.945 + }, + "prevControl": null, + "nextControl": { + "x": 0.7493238434163707, + "y": 5.471352313167261 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0828588374851729, + "y": 4.933392645314354 + }, + "prevControl": { + "x": 1.093941874258602, + "y": 5.503629893238435 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.1, + "maxWaypointRelativePos": 0.4, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "WarmupShooterNear", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubToDepoPath.path b/src/main/deploy/pathplanner/paths/HubToDepoPath.path new file mode 100644 index 00000000..c5a23546 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubToDepoPath.path @@ -0,0 +1,94 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4498813760379603, + "y": 4.018861209964412 + }, + "prevControl": null, + "nextControl": { + "x": 3.363807829181496, + "y": 6.439679715302491 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7493238434163707, + "y": 5.944756820877817 + }, + "prevControl": { + "x": 1.4917081850533813, + "y": 5.9124792408066424 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.821147356580429, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.09223847019122663, + "maxWaypointRelativePos": 0.3959505061867341, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeDeployExtend", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "CollectorIntake", + "waypointRelativePos": 0.39145106861641804, + "endWaypointRelativePos": 2.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java b/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java index 36819259..434a52a6 100644 --- a/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java +++ b/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java @@ -1,6 +1,7 @@ package competition.auto_programs.ppl; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; @@ -16,6 +17,6 @@ public LeftBumpAutoCommand( super(autoSelector); pf.setPrefix(this.getName()); - this.addCommands(AutoBuilder.buildAuto("NormalBumpAutoLeft")); + this.addCommands(new PathPlannerAuto("NormalBumpAutoLeft")); } } diff --git a/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java b/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java index c3b8a4ec..f5f5831d 100644 --- a/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java +++ b/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java @@ -1,6 +1,7 @@ package competition.auto_programs.ppl; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; @@ -16,6 +17,6 @@ public RightBumpAutoCommand( super(autoSelector); pf.setPrefix(this.getName()); - this.addCommands(AutoBuilder.buildAuto("NormalBumpAutoRight")); + this.addCommands(new PathPlannerAuto("NormalBumpAutoRight")); } } From 762e8d0d939bfd0c804c1b1caf7569beded9f0a9 Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 10:20:53 -0700 Subject: [PATCH 07/16] Update Robot.java --- src/main/java/competition/Robot.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 50f00561..6d0883d3 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -65,6 +65,7 @@ protected void initializeSystems() { autoChooser.addOption("Collect and Shoot Twice", getInjectorComponent().collectAndShootTwiceCommand()); autoChooser.addOption("Shoot From Hub", getInjectorComponent().shootFromHubCommandGroup()); autoChooser.addOption("Shoot From Trench", getInjectorComponent().shootFromTrenchCommandGroup()); + autoChooser.addOption("HubToDepoToTower", new PathPlannerAuto("HubToDepoToTower")); SmartDashboard.putData("Auto Selector", autoChooser); From 38cf97bb600f65e3194b2aa7c9b87c3fa8837458 Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 10:53:59 -0700 Subject: [PATCH 08/16] removed auto select from robot.java and added it to operator command map --- src/main/java/competition/Robot.java | 12 ------------ .../operator_interface/OperatorCommandMap.java | 9 +++++++++ 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 6d0883d3..7f020d40 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -35,7 +35,6 @@ public class Robot extends BaseRobot { public static final double LOOP_INTERVAL = 0.04; private final Field2d field = new Field2d(); - private final SendableChooser autoChooser = new SendableChooser<>(); BaseSimulator simulator; @@ -59,16 +58,6 @@ protected void initializeSystems() { getInjectorComponent().voltageMonitorSubsystem(); getInjectorComponent().climberSubsystem(); - autoChooser.setDefaultOption("Normal Bump Auto Right", new PathPlannerAuto("NormalBumpAutoRight")); - autoChooser.addOption("Normal Bump Auto Left", new PathPlannerAuto("NormalBumpAutoLeft")); - - autoChooser.addOption("Collect and Shoot Twice", getInjectorComponent().collectAndShootTwiceCommand()); - autoChooser.addOption("Shoot From Hub", getInjectorComponent().shootFromHubCommandGroup()); - autoChooser.addOption("Shoot From Trench", getInjectorComponent().shootFromTrenchCommandGroup()); - autoChooser.addOption("HubToDepoToTower", new PathPlannerAuto("HubToDepoToTower")); - - SmartDashboard.putData("Auto Selector", autoChooser); - if (BaseRobot.isSimulation()) { @@ -140,7 +129,6 @@ public void autonomousInit() { super.autonomousInit(); var pose = (PoseSubsystem) getInjectorComponent().poseSubsystem(); CommandScheduler.getInstance().schedule(pose.getResetTranslationToVisionEstimateCommand()); - Command autonomousCommand = autoChooser.getSelected(); if (autonomousCommand != null) { CommandScheduler.getInstance().schedule(autonomousCommand); } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index fa133c07..5c91778c 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -4,6 +4,7 @@ import javax.inject.Provider; import javax.inject.Singleton; +import com.pathplanner.lib.commands.PathPlannerAuto; import competition.auto_programs.AimAndShootFromHereCommand; import competition.auto_programs.ppl.LeftBumpAutoCommand; import competition.auto_programs.ShootFromHubCommandGroup; @@ -213,6 +214,14 @@ public void setupAutoCommands(Provider setAutonomousComman var collectAndShootTwice = setAutonomousCommandProvider.get(); collectAndShootTwice.setAutoCommand(collectAndShootTwiceCommand, Landmarks.blueStartTrenchToOutpost); collectAndShootTwice.includeOnSmartDashboard("Collect and shoot twice."); + + var hubToDepoToTower = setAutonomousCommandProvider.get(); + hubToDepoToTower.setAutoCommand(new PathPlannerAuto("Hub To Depo To Tower Auto")); + hubToDepoToTower.includeOnSmartDashboard("Hub to Depo to Tower Auto"); + + var normalBumpAutoRight = setAutonomousCommandProvider.get(); + normalBumpAutoRight.setAutoCommand(new PathPlannerAuto("NormalBumpAutoRight")); + normalBumpAutoRight.includeOnSmartDashboard("Normal Bump Auto Right"); } @Inject From bc663666f9f4c2067cf52880fe119a53e19a9205 Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 11:24:21 -0700 Subject: [PATCH 09/16] make command for hubtodepototower --- .../ppl/HubToDepoToTowerAutoCommand.java | 22 +++++++++++++++++++ .../ppl/LeftBumpAutoCommand.java | 2 +- .../ppl/RightBumpAutoCommand.java | 2 +- 3 files changed, 24 insertions(+), 2 deletions(-) create mode 100644 src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java diff --git a/src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java b/src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java new file mode 100644 index 00000000..cfb22bc3 --- /dev/null +++ b/src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java @@ -0,0 +1,22 @@ +package competition.auto_programs.ppl; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import competition.auto_programs.BaseAutonomousSequentialCommandGroup; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.autonomous.AutonomousCommandSelector; + +import javax.inject.Inject; + +public class HubToDepoToTowerAutoCommand extends BaseAutonomousSequentialCommandGroup { + @Inject + public HubToDepoToTowerAutoCommand( + AutonomousCommandSelector autoSelector, + PropertyFactory pf + ) { + super(autoSelector); + pf.setPrefix(this.getName()); + + this.addCommands(AutoBuilder.buildAuto("HubToDepoToTowerAuto")); + } +} diff --git a/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java b/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java index 434a52a6..faf29347 100644 --- a/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java +++ b/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java @@ -17,6 +17,6 @@ public LeftBumpAutoCommand( super(autoSelector); pf.setPrefix(this.getName()); - this.addCommands(new PathPlannerAuto("NormalBumpAutoLeft")); + this.addCommands(AutoBuilder.buildAuto("NormalBumpAutoLeft")); } } diff --git a/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java b/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java index f5f5831d..de0cbfcc 100644 --- a/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java +++ b/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java @@ -17,6 +17,6 @@ public RightBumpAutoCommand( super(autoSelector); pf.setPrefix(this.getName()); - this.addCommands(new PathPlannerAuto("NormalBumpAutoRight")); + this.addCommands(AutoBuilder.buildAuto("NormalBumpAutoRight")); } } From 68b02180c717ef4c634f79247326aad9daa5cccc Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 11:24:43 -0700 Subject: [PATCH 10/16] elastic update --- elastic-layout/robot-configuration.json | 245 ++++++------------------ 1 file changed, 54 insertions(+), 191 deletions(-) diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index 4432ce0d..1fdc73cf 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -158,6 +158,60 @@ "data_type": "string", "show_submit_button": false } + }, + { + "title": "Collect and shoot twice.", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Collect and shoot twice.", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "DeviceHealth", + "x": 1152.0, + "y": 128.0, + "width": 256.0, + "height": 384.0, + "type": "Alerts", + "properties": { + "topic": "/AdvantageKit/RealOutputs/DeviceHealth", + "period": 0.06 + } + }, + { + "title": "Hub to Depo to Tower Auto", + "x": 384.0, + "y": 768.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Hub to Depo to Tower Auto", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Normal Bump Auto Right", + "x": 640.0, + "y": 768.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Normal Bump Auto Right", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } } ] } @@ -583,20 +637,6 @@ "data_type": "string", "show_submit_button": true } - }, - { - "title": "ServoTargetPositionNormalized", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/HoodSubsystem/ServoTargetPositionNormalized", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } } ] } @@ -1747,183 +1787,6 @@ } ] } - }, - { - "name": "Red Alliance Competition", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Field", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 896.0, - "type": "Field", - "properties": { - "topic": "/SmartDashboard/Field", - "period": 0.06, - "field_game": "Rebuilt", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 90.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295, - "show_robot_outside_widget": true - } - }, - { - "title": "Current Match Time", - "x": 384.0, - "y": 0.0, - "width": 768.0, - "height": 896.0, - "type": "Match Time", - "properties": { - "topic": "/SmartDashboard/Current Match Time", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 20, - "yellow_start_time": 30 - } - }, - { - "title": "Match Shift", - "x": 1152.0, - "y": 640.0, - "width": 512.0, - "height": 256.0, - "type": "Large Text Display", - "properties": { - "topic": "/SmartDashboard/Match Shift", - "period": 0.06, - "data_type": "string" - } - }, - { - "title": "Is Hood Down", - "x": 1152.0, - "y": 0.0, - "width": 512.0, - "height": 384.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/Is Hood Down", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Auto Selector", - "x": 1152.0, - "y": 384.0, - "width": 512.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Auto Selector", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "selected", - "x": 1152.0, - "y": 512.0, - "width": 512.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Auto Selector/selected", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - } - ] - } - }, - { - "name": "Blue Alliance Competition", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Field", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 896.0, - "type": "Field", - "properties": { - "topic": "/SmartDashboard/Field", - "period": 0.06, - "field_game": "Rebuilt", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": -90.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295, - "show_robot_outside_widget": true - } - }, - { - "title": "Current Match Time", - "x": 384.0, - "y": 0.0, - "width": 768.0, - "height": 896.0, - "type": "Match Time", - "properties": { - "topic": "/SmartDashboard/Current Match Time", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 20, - "yellow_start_time": 30 - } - }, - { - "title": "Match Shift", - "x": 1152.0, - "y": 384.0, - "width": 512.0, - "height": 512.0, - "type": "Large Text Display", - "properties": { - "topic": "/SmartDashboard/Match Shift", - "period": 0.06, - "data_type": "string" - } - }, - { - "title": "Is Hood Down", - "x": 1152.0, - "y": 0.0, - "width": 512.0, - "height": 384.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/Is Hood Down", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - } - ] - } } ] } \ No newline at end of file From 8c76f478332eb497f46164213ffd45071f4621aa Mon Sep 17 00:00:00 2001 From: Stephen Just Date: Fri, 10 Apr 2026 12:01:17 -0700 Subject: [PATCH 11/16] Fixes so far --- .../autos/NormalBumpAutoRight.auto | 21 ++++++++++++++++++- .../ppl/LeftBumpAutoCommand.java | 1 - .../ppl/RightBumpAutoCommand.java | 1 - .../OperatorCommandMap.java | 6 +++--- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto b/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto index a3c81153..553a63b4 100644 --- a/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto +++ b/src/main/deploy/pathplanner/autos/NormalBumpAutoRight.auto @@ -32,7 +32,26 @@ { "type": "path", "data": { - "pathName": "SecondCycleRightNoShooting" + "pathName": "SecondCycleRight" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 4.5 + } + }, + { + "type": "named", + "data": { + "name": "AimAndShootFromHere" + } + } + ] } } ] diff --git a/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java b/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java index faf29347..36819259 100644 --- a/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java +++ b/src/main/java/competition/auto_programs/ppl/LeftBumpAutoCommand.java @@ -1,7 +1,6 @@ package competition.auto_programs.ppl; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; diff --git a/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java b/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java index de0cbfcc..c3b8a4ec 100644 --- a/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java +++ b/src/main/java/competition/auto_programs/ppl/RightBumpAutoCommand.java @@ -1,7 +1,6 @@ package competition.auto_programs.ppl; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 5c91778c..521682ef 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -4,7 +4,7 @@ import javax.inject.Provider; import javax.inject.Singleton; -import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.auto.AutoBuilder; import competition.auto_programs.AimAndShootFromHereCommand; import competition.auto_programs.ppl.LeftBumpAutoCommand; import competition.auto_programs.ShootFromHubCommandGroup; @@ -216,11 +216,11 @@ public void setupAutoCommands(Provider setAutonomousComman collectAndShootTwice.includeOnSmartDashboard("Collect and shoot twice."); var hubToDepoToTower = setAutonomousCommandProvider.get(); - hubToDepoToTower.setAutoCommand(new PathPlannerAuto("Hub To Depo To Tower Auto")); + hubToDepoToTower.setAutoCommand(AutoBuilder.buildAuto("HubToDepoToTower")); hubToDepoToTower.includeOnSmartDashboard("Hub to Depo to Tower Auto"); var normalBumpAutoRight = setAutonomousCommandProvider.get(); - normalBumpAutoRight.setAutoCommand(new PathPlannerAuto("NormalBumpAutoRight")); + normalBumpAutoRight.setAutoCommand(AutoBuilder.buildAuto("NormalBumpAutoRight")); normalBumpAutoRight.includeOnSmartDashboard("Normal Bump Auto Right"); } From 3c34e7e659e68f6c71d176c7914444d47b783b4c Mon Sep 17 00:00:00 2001 From: Stephen Just Date: Fri, 10 Apr 2026 12:05:45 -0700 Subject: [PATCH 12/16] Fixes so far --- elastic-layout/robot-configuration.json | 4 +- src/main/java/competition/Robot.java | 45 ------------------- .../ppl/HubToDepoToTowerAutoCommand.java | 22 --------- 3 files changed, 2 insertions(+), 69 deletions(-) delete mode 100644 src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index 1fdc73cf..0c794420 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -188,7 +188,7 @@ { "title": "Hub to Depo to Tower Auto", "x": 384.0, - "y": 768.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Command", @@ -202,7 +202,7 @@ { "title": "Normal Bump Auto Right", "x": 640.0, - "y": 768.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Command", diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 48add3eb..9c6bbe37 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -1,15 +1,6 @@ package competition; -import com.pathplanner.lib.commands.PathPlannerAuto; -import competition.auto_programs.CollectAndShootTwiceCommand; -import competition.auto_programs.ShootFromHubCommandGroup; -import competition.auto_programs.ShootFromTrenchCommandGroup; -import edu.wpi.first.wpilibj.DriverStation; -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; import edu.wpi.first.wpilibj2.command.CommandScheduler; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -34,8 +25,6 @@ public class Robot extends BaseRobot { Logger log = LogManager.getLogger(Robot.class); public static final double LOOP_INTERVAL = 0.04; - private final Field2d field = new Field2d(); - BaseSimulator simulator; OperatorInterface oi; @@ -58,8 +47,6 @@ protected void initializeSystems() { getInjectorComponent().voltageMonitorSubsystem(); getInjectorComponent().climberSubsystem(); - - if (BaseRobot.isSimulation()) { simulator = getInjectorComponent().simulator(); } @@ -83,10 +70,6 @@ protected void initializeSystems() { getInjectorComponent().superstructureMechanismSubsystem(); CommandScheduler.getInstance().schedule(getInjectorComponent().gamepadRumbleCommand()); - - SmartDashboard.putData("Field", field); - - CommandScheduler.getInstance().schedule(getInjectorComponent().gamepadRumbleCommand()); } protected BaseRobotComponent createDaggerComponent() { @@ -129,9 +112,6 @@ public void autonomousInit() { super.autonomousInit(); var pose = (PoseSubsystem) getInjectorComponent().poseSubsystem(); CommandScheduler.getInstance().schedule(pose.getResetTranslationToVisionEstimateCommand()); - if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(autonomousCommand); - } if(BaseRobot.isSimulation()) { getInjectorComponent().simulator().resetForAuto(); @@ -180,30 +160,5 @@ protected void sharedPeriodic() { if (this.oi != null) { this.oi.periodic(); } - - String matchShift; - - if (DriverStation.getMatchTime() <= 140 && DriverStation.getMatchTime() >= 130) { - matchShift = "Transition Shift"; - } else if (DriverStation.getMatchTime() <= 130 && DriverStation.getMatchTime() >= 105) { - matchShift = "Shift 1"; - } else if (DriverStation.getMatchTime() <= 105 && DriverStation.getMatchTime() >= 80) { - matchShift = "Shift 2"; - } else if (DriverStation.getMatchTime() <= 80 && DriverStation.getMatchTime() >= 55) { - matchShift = "Shift 3"; - } else if (DriverStation.getMatchTime() <= 55 && DriverStation.getMatchTime() >= 30) { - matchShift = "Shift 4"; - } else if (DriverStation.getMatchTime() <= 30 && DriverStation.getMatchTime() >= 0) { - matchShift = "Endgame"; - } else { - matchShift = "Unknown"; - } - - PoseSubsystem pose = (PoseSubsystem) getInjectorComponent().poseSubsystem(); - field.setRobotPose(pose.getCurrentPose2d()); - - SmartDashboard.putNumber("Current Match Time", DriverStation.getMatchTime()); - SmartDashboard.putString("Match Shift", matchShift); - SmartDashboard.putBoolean("Is Hood Down", getInjectorComponent().hoodSubsystem().isHoodDown()); } } diff --git a/src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java b/src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java deleted file mode 100644 index cfb22bc3..00000000 --- a/src/main/java/competition/auto_programs/ppl/HubToDepoToTowerAutoCommand.java +++ /dev/null @@ -1,22 +0,0 @@ -package competition.auto_programs.ppl; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import competition.auto_programs.BaseAutonomousSequentialCommandGroup; -import xbot.common.properties.PropertyFactory; -import xbot.common.subsystems.autonomous.AutonomousCommandSelector; - -import javax.inject.Inject; - -public class HubToDepoToTowerAutoCommand extends BaseAutonomousSequentialCommandGroup { - @Inject - public HubToDepoToTowerAutoCommand( - AutonomousCommandSelector autoSelector, - PropertyFactory pf - ) { - super(autoSelector); - pf.setPrefix(this.getName()); - - this.addCommands(AutoBuilder.buildAuto("HubToDepoToTowerAuto")); - } -} From 0ae3cb1c4c99bb6f80603379d956137d2743da63 Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 12:17:07 -0700 Subject: [PATCH 13/16] remove warmup shooter from depototower --- src/main/deploy/pathplanner/paths/DepoToTowerPath.path | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path index 780e0541..11521cbf 100644 --- a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path +++ b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path @@ -45,14 +45,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "WarmupShooterNear", - "waypointRelativePos": 0.3, - "endWaypointRelativePos": 1.0, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, "maxAcceleration": 3.0, From 8d3bad4019e22b647eabfd48682f711520fc8370 Mon Sep 17 00:00:00 2001 From: Victor C Date: Fri, 10 Apr 2026 12:23:01 -0700 Subject: [PATCH 14/16] remove another warmup shooter that caused the auto to not run --- src/main/deploy/pathplanner/autos/HubToDepoToTower.auto | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto index 9d16f489..a2a834ab 100644 --- a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto +++ b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "WarmupShooterNear" - } - }, { "type": "deadline", "data": { From d03c9133c10fff26f24f1bfa904b08f0eb0fea19 Mon Sep 17 00:00:00 2001 From: Victor C Date: Sat, 11 Apr 2026 10:24:02 -0700 Subject: [PATCH 15/16] update the hub depot tower auto with improvements --- .../pathplanner/autos/HubToDepoToTower.auto | 25 +++++++++ .../pathplanner/paths/BackUpFromHub.path | 54 +++++++++++++++++++ .../pathplanner/paths/DepoToTowerPath.path | 4 +- .../pathplanner/paths/HubToDepoPath.path | 20 +++---- 4 files changed, 91 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/BackUpFromHub.path diff --git a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto index a2a834ab..4ba8298e 100644 --- a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto +++ b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "path", + "data": { + "pathName": "BackUpFromHub" + } + }, { "type": "deadline", "data": { @@ -29,6 +35,25 @@ "pathName": "HubToDepoPath" } }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "CollectorIntake" + } + } + ] + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/BackUpFromHub.path b/src/main/deploy/pathplanner/paths/BackUpFromHub.path new file mode 100644 index 00000000..2653c6c0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackUpFromHub.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5574733096085422, + "y": 4.008102016607354 + }, + "prevControl": null, + "nextControl": { + "x": 3.073309608540926, + "y": 3.997342823250296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.105587188612101, + "y": 4.008102016607354 + }, + "prevControl": { + "x": 3.355587188612101, + "y": 4.008102016607354 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.19871266989296 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.33380002981693 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path index 11521cbf..646e2f05 100644 --- a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path +++ b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 0.749, + "x": 0.8, "y": 5.945 }, "prevControl": null, "nextControl": { - "x": 0.7493238434163707, + "x": 0.8003238434163708, "y": 5.471352313167261 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HubToDepoPath.path b/src/main/deploy/pathplanner/paths/HubToDepoPath.path index c5a23546..60f1adef 100644 --- a/src/main/deploy/pathplanner/paths/HubToDepoPath.path +++ b/src/main/deploy/pathplanner/paths/HubToDepoPath.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 3.4498813760379603, - "y": 4.018861209964412 + "x": 3.106, + "y": 4.008 }, "prevControl": null, "nextControl": { - "x": 3.363807829181496, - "y": 6.439679715302491 + "x": 3.0199402040181123, + "y": 6.428818994218959 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.7493238434163707, + "x": 0.8, "y": 5.944756820877817 }, "prevControl": { - "x": 1.4917081850533813, + "x": 1.5423843416370107, "y": 5.9124792408066424 }, "nextControl": null, @@ -61,14 +61,14 @@ "eventMarkers": [ { "name": "IntakeDeployExtend", - "waypointRelativePos": 0.1, - "endWaypointRelativePos": null, + "waypointRelativePos": 0.04949381327333687, + "endWaypointRelativePos": 0.19347581552306164, "command": null }, { "name": "CollectorIntake", - "waypointRelativePos": 0.39145106861641804, - "endWaypointRelativePos": 2.0, + "waypointRelativePos": 0.33070866141732724, + "endWaypointRelativePos": 1.0, "command": null } ], From 7e69861f00aeb68e0ff59e32fbb4364723e320ab Mon Sep 17 00:00:00 2001 From: Victor C Date: Sat, 11 Apr 2026 11:52:54 -0700 Subject: [PATCH 16/16] fixed auto --- .../pathplanner/autos/HubToDepoToTower.auto | 45 +++++++++---------- .../pathplanner/paths/BackUpFromHub.path | 8 ++-- .../pathplanner/paths/DepoToTowerPath.path | 10 ++--- .../pathplanner/paths/HubToDepoPath.path | 22 ++++----- .../competition/ConfigurePathPlannerLib.java | 5 ++- 5 files changed, 46 insertions(+), 44 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto index 4ba8298e..6241a1d0 100644 --- a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto +++ b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "BackUpFromHub" - } - }, { "type": "deadline", "data": { @@ -17,18 +11,36 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 4.0 } }, { "type": "named", "data": { - "name": "AimAndShootFromHere" + "name": "WarmupShooterNear" + } + }, + { + "type": "named", + "data": { + "name": "FireWhenShooterAndHoodReady" } } ] } }, + { + "type": "named", + "data": { + "name": "IntakeDeployExtend" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, { "type": "path", "data": { @@ -36,22 +48,9 @@ } }, { - "type": "deadline", + "type": "parallel", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "CollectorIntake" - } - } - ] + "commands": [] } }, { diff --git a/src/main/deploy/pathplanner/paths/BackUpFromHub.path b/src/main/deploy/pathplanner/paths/BackUpFromHub.path index 2653c6c0..366b2dfa 100644 --- a/src/main/deploy/pathplanner/paths/BackUpFromHub.path +++ b/src/main/deploy/pathplanner/paths/BackUpFromHub.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.105587188612101, - "y": 4.008102016607354 + "x": 3.396, + "y": 5.095 }, "prevControl": { - "x": 3.355587188612101, - "y": 4.008102016607354 + "x": 3.5251957295373675, + "y": 5.955516014234875 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path index 646e2f05..bb5bb83c 100644 --- a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path +++ b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.8, + "x": 0.69, "y": 5.945 }, "prevControl": null, "nextControl": { - "x": 0.8003238434163708, - "y": 5.471352313167261 + "x": 0.7610709357401938, + "y": 5.6925275432902565 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.933392645314354 }, "prevControl": { - "x": 1.093941874258602, - "y": 5.503629893238435 + "x": 0.9434991997628981, + "y": 5.4827779536440655 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HubToDepoPath.path b/src/main/deploy/pathplanner/paths/HubToDepoPath.path index 60f1adef..3f2287e6 100644 --- a/src/main/deploy/pathplanner/paths/HubToDepoPath.path +++ b/src/main/deploy/pathplanner/paths/HubToDepoPath.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 3.106, - "y": 4.008 + "x": 3.4, + "y": 5.152 }, "prevControl": null, "nextControl": { - "x": 3.0199402040181123, - "y": 6.428818994218959 + "x": 1.6746144721233698, + "y": 6.3966429418742585 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.8, + "x": 0.69, "y": 5.944756820877817 }, "prevControl": { - "x": 1.5423843416370107, + "x": 1.4323843416370106, "y": 5.9124792408066424 }, "nextControl": null, @@ -35,8 +35,8 @@ "minWaypointRelativePos": 0.821147356580429, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.5, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -61,8 +61,8 @@ "eventMarkers": [ { "name": "IntakeDeployExtend", - "waypointRelativePos": 0.04949381327333687, - "endWaypointRelativePos": 0.19347581552306164, + "waypointRelativePos": 0.10123734533183272, + "endWaypointRelativePos": null, "command": null }, { @@ -88,7 +88,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 132.20841353697384 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/competition/ConfigurePathPlannerLib.java b/src/main/java/competition/ConfigurePathPlannerLib.java index a7601a1d..865237ed 100644 --- a/src/main/java/competition/ConfigurePathPlannerLib.java +++ b/src/main/java/competition/ConfigurePathPlannerLib.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import competition.auto_programs.AimAndShootFromHereCommand; +import competition.command_groups.FireWhenShooterAndHoodReady; import competition.command_groups.PrepareToShootCommandGroup; import competition.command_groups.WaitForRotationAndHoodAndShooterToBeAtGoalCommandGroup; import competition.subsystems.collector_intake.commands.CollectorIntakeCommand; @@ -27,7 +28,8 @@ public ConfigurePathPlannerLib(PoseSubsystem pose, DriveSubsystem drive, CollectorIntakeCommand collectorIntakeCommand, AimAndShootFromHereCommand aimAndShootFromHereCommand, PrepareToShootCommandGroup prepareToShootCommandGroup, - WaitForRotationAndHoodAndShooterToBeAtGoalCommandGroup waitForRotationAndHoodAndShooterToBeAtGoalCommandGroup + WaitForRotationAndHoodAndShooterToBeAtGoalCommandGroup waitForRotationAndHoodAndShooterToBeAtGoalCommandGroup, + FireWhenShooterAndHoodReady fireWhenShooterAndHoodReady ) { NamedCommands.registerCommand("IntakeDeployExtend", intakeDeployExtendCommand); NamedCommands.registerCommand("CollectorIntake", collectorIntakeCommand); @@ -35,6 +37,7 @@ public ConfigurePathPlannerLib(PoseSubsystem pose, DriveSubsystem drive, NamedCommands.registerCommand("WaitForRotationAndHoodAndShooterToBeAtGoal", waitForRotationAndHoodAndShooterToBeAtGoalCommandGroup); NamedCommands.registerCommand("WarmupShooterNear", prepareToShootCommandGroup.setPresetLocation(TrajectoriesCalculation.PresetShootingDistance.NEAR)); + NamedCommands.registerCommand("FireWhenShooterAndHoodReady", fireWhenShooterAndHoodReady); try { AutoBuilder.configure(