From 6ef42dbca7609b2dcce27728576a0cfc86562a00 Mon Sep 17 00:00:00 2001 From: braelynandthefrogs Date: Mon, 31 Mar 2025 19:58:58 -0700 Subject: [PATCH 1/4] moved superstructure outside of the subsystem folder --- src/main/java/frc/robot/Controls.java | 1 - src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/{subsystems => }/SuperStructure.java | 5 ++++- 3 files changed, 4 insertions(+), 3 deletions(-) rename src/main/java/frc/robot/{subsystems => }/SuperStructure.java (98%) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 59b75a34..98865f94 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -22,7 +22,6 @@ import frc.robot.subsystems.ArmPivot; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.GroundArm; -import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoAlign; import frc.robot.util.AlgaeIntakeHeight; import frc.robot.util.BranchHeight; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 127db667..713e669c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Sensors.SensorConstants; import frc.robot.Subsystems.SubsystemConstants; -import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoBuilderConfig; import frc.robot.subsystems.auto.AutoLogic; import frc.robot.subsystems.auto.AutonomousField; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/SuperStructure.java similarity index 98% rename from src/main/java/frc/robot/subsystems/SuperStructure.java rename to src/main/java/frc/robot/SuperStructure.java index 1bb57d15..c3e87f8a 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/SuperStructure.java @@ -1,10 +1,13 @@ -package frc.robot.subsystems; +package frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.sensors.ArmSensor; import frc.robot.sensors.ElevatorLight; +import frc.robot.subsystems.ArmPivot; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.SpinnyClaw; import java.util.function.BooleanSupplier; public class SuperStructure { From 1c07552e20639adc249f8de1679f7722cfc45144 Mon Sep 17 00:00:00 2001 From: Mark Peterson Date: Fri, 28 Nov 2025 19:57:23 -0800 Subject: [PATCH 2/4] Fixing merge conflict --- .vscode/settings.json | 3 +- src/main/deploy/elastic-layout.json | 634 ++++++++++++++++++ .../autos/M alliance push to G.auto | 6 + .../autos/M alliance push to H.auto | 6 + .../deploy/pathplanner/autos/MLSF_H-G.auto | 23 +- .../deploy/pathplanner/autos/MLSF_H-I.auto | 23 +- .../pathplanner/autos/MLSF_H-I_WithWait.auto | 23 +- .../pathplanner/autos/MLSF_H-K_Cooking.auto | 93 +++ .../deploy/pathplanner/autos/MRSF_G-F.auto | 23 +- .../pathplanner/autos/MRSF_G-F_WithWait.auto | 23 +- .../deploy/pathplanner/autos/MRSF_G-H.auto | 23 +- src/main/deploy/pathplanner/autos/M_G.auto | 6 + .../deploy/pathplanner/autos/M_H-GHA-IJA.auto | 73 ++ src/main/deploy/pathplanner/autos/M_H.auto | 6 + .../deploy/pathplanner/autos/OSMRSF_D-C.auto | 23 +- .../deploy/pathplanner/autos/OSMRSF_E-D.auto | 23 +- .../deploy/pathplanner/autos/OSMRSF_F-E.auto | 23 +- src/main/deploy/pathplanner/autos/OSM_F.auto | 6 + .../pathplanner/autos/OSWRSF_D-C-B.auto | 40 +- .../deploy/pathplanner/autos/OSWRSF_D-C.auto | 23 +- .../pathplanner/autos/OSWRSF_E-D-C.auto | 131 ++++ .../deploy/pathplanner/autos/OSWRSF_E-D.auto | 23 +- .../deploy/pathplanner/autos/OSWRSF_F-E.auto | 23 +- src/main/deploy/pathplanner/autos/OSW_E.auto | 6 + src/main/deploy/pathplanner/autos/OSW_F.auto | 6 + .../deploy/pathplanner/autos/PID TESTING.auto | 6 + .../pathplanner/autos/YSMLSC_K-L-A.auto | 40 +- .../pathplanner/autos/YSMLSF_I-J-K.auto | 40 +- .../deploy/pathplanner/autos/YSMLSF_I-J.auto | 23 +- .../pathplanner/autos/YSMLSF_J-K-L.auto | 40 +- .../deploy/pathplanner/autos/YSMLSF_J-K.auto | 23 +- .../pathplanner/autos/YSMLSF_K-L-A.auto | 40 +- .../deploy/pathplanner/autos/YSMLSF_K-L.auto | 23 +- src/main/deploy/pathplanner/autos/YSM_I.auto | 6 + .../pathplanner/autos/YSWLSC_K-L-A.auto | 40 +- .../pathplanner/autos/YSWLSF_I-J-K-L.auto | 57 +- .../pathplanner/autos/YSWLSF_I-J-K.auto | 40 +- .../deploy/pathplanner/autos/YSWLSF_I-J.auto | 23 +- .../pathplanner/autos/YSWLSF_J-K-L-A.auto | 57 +- .../pathplanner/autos/YSWLSF_J-K-L.auto | 40 +- .../deploy/pathplanner/autos/YSWLSF_J-K.auto | 23 +- .../pathplanner/autos/YSWLSF_K-L-A.auto | 40 +- .../deploy/pathplanner/autos/YSWLSF_K-L.auto | 23 +- src/main/deploy/pathplanner/autos/YSW_I.auto | 6 + src/main/deploy/pathplanner/autos/YSW_J.auto | 6 + .../deploy/pathplanner/paths/GHA to Net.path | 54 ++ .../deploy/pathplanner/paths/H to GHA.path | 54 ++ .../deploy/pathplanner/paths/IJA to Net.path | 54 ++ .../deploy/pathplanner/paths/LSC to A.path | 12 +- .../deploy/pathplanner/paths/LSC to L.path | 12 +- .../deploy/pathplanner/paths/LSF to A.path | 12 +- .../deploy/pathplanner/paths/LSF to G.path | 8 +- .../deploy/pathplanner/paths/LSF to I.path | 12 +- .../deploy/pathplanner/paths/LSF to J.path | 12 +- .../deploy/pathplanner/paths/LSF to K.path | 12 +- .../deploy/pathplanner/paths/LSF to L.path | 12 +- src/main/deploy/pathplanner/paths/M to G.path | 12 +- src/main/deploy/pathplanner/paths/M to H.path | 12 +- .../deploy/pathplanner/paths/Net to IJA.path | 54 ++ .../deploy/pathplanner/paths/OSM to D.path | 12 +- .../deploy/pathplanner/paths/OSM to E.path | 12 +- .../deploy/pathplanner/paths/OSM to F.path | 12 +- .../deploy/pathplanner/paths/OSW to D.path | 12 +- .../deploy/pathplanner/paths/OSW to E.path | 12 +- .../deploy/pathplanner/paths/OSW to F.path | 12 +- .../deploy/pathplanner/paths/RSF to B.path | 12 +- .../deploy/pathplanner/paths/RSF to C.path | 12 +- .../deploy/pathplanner/paths/RSF to D.path | 12 +- .../deploy/pathplanner/paths/RSF to E.path | 12 +- .../deploy/pathplanner/paths/RSF to F.path | 12 +- .../deploy/pathplanner/paths/RSF to H.path | 8 +- .../deploy/pathplanner/paths/YSM to I.path | 12 +- .../deploy/pathplanner/paths/YSM to J.path | 12 +- .../deploy/pathplanner/paths/YSM to K.path | 12 +- .../deploy/pathplanner/paths/YSW to I.path | 12 +- .../deploy/pathplanner/paths/YSW to J.path | 12 +- .../deploy/pathplanner/paths/YSW to K.path | 12 +- src/main/java/frc/robot/Controls.java | 464 +++++++++++-- src/main/java/frc/robot/Hardware.java | 2 +- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/SuperStructure.java | 309 ++++++++- .../robot/generated/CompTunerConstants.java | 52 +- .../java/frc/robot/sensors/ArmSensor.java | 16 +- .../java/frc/robot/sensors/BranchSensors.java | 12 +- .../java/frc/robot/sensors/ElevatorLight.java | 12 +- .../java/frc/robot/sensors/IntakeSensor.java | 18 +- .../java/frc/robot/subsystems/ArmPivot.java | 65 +- .../java/frc/robot/subsystems/ClimbPivot.java | 118 +++- .../robot/subsystems/DrivebaseWrapper.java | 1 + .../robot/subsystems/ElevatorSubsystem.java | 14 +- .../java/frc/robot/subsystems/GroundArm.java | 22 +- .../frc/robot/subsystems/GroundSpinny.java | 34 +- .../java/frc/robot/subsystems/SpinnyClaw.java | 2 +- .../frc/robot/subsystems/VisionSubsystem.java | 85 ++- .../frc/robot/subsystems/auto/AlgaeAlign.java | 147 ++++ .../subsystems/auto/AutoAlgaeHeights.java | 97 +++ .../frc/robot/subsystems/auto/AutoAlign.java | 367 ++++++---- .../subsystems/auto/AutoBuilderConfig.java | 9 +- .../frc/robot/subsystems/auto/AutoLogic.java | 108 ++- .../frc/robot/subsystems/auto/BargeAlign.java | 70 +- .../auto/DynamicSendableChooser.java | 15 +- .../drivebase/CommandSwerveDrivetrain.java | 3 + .../java/frc/robot/util/AllianceUtils.java | 19 + src/main/java/frc/robot/util/MatchTab.java | 13 +- src/main/java/frc/robot/util/ScoringType.java | 7 + .../java/frc/robot/util/SoloScoringMode.java | 7 + ....2.6.json => PathplannerLib-2025.2.7.json} | 8 +- 107 files changed, 3950 insertions(+), 584 deletions(-) create mode 100644 src/main/deploy/elastic-layout.json create mode 100644 src/main/deploy/pathplanner/autos/MLSF_H-K_Cooking.auto create mode 100644 src/main/deploy/pathplanner/autos/M_H-GHA-IJA.auto create mode 100644 src/main/deploy/pathplanner/autos/OSWRSF_E-D-C.auto create mode 100644 src/main/deploy/pathplanner/paths/GHA to Net.path create mode 100644 src/main/deploy/pathplanner/paths/H to GHA.path create mode 100644 src/main/deploy/pathplanner/paths/IJA to Net.path create mode 100644 src/main/deploy/pathplanner/paths/Net to IJA.path create mode 100644 src/main/java/frc/robot/subsystems/auto/AlgaeAlign.java create mode 100644 src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java create mode 100644 src/main/java/frc/robot/util/AllianceUtils.java create mode 100644 src/main/java/frc/robot/util/ScoringType.java create mode 100644 src/main/java/frc/robot/util/SoloScoringMode.java rename vendordeps/{PathplannerLib-2025.2.6.json => PathplannerLib-2025.2.7.json} (87%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 2cc7f2e5..a992d31a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "editor.tabSize": 2 + "editor.tabSize": 2, + "wpilib.autoStartRioLog": false } diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 00000000..40397540 --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,634 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "ArmSensor", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "inTrough", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/ArmSensor/inTrough", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "IntakeSensor", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "In Intake", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/IntakeSensor/In Intake", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "BranchSensor", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "LeftDistance(m)", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/BranchSensor/LeftDistance(m)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "AprilTags", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Last raw timestamp", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/AprilTags/Last raw timestamp", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Num targets", + "x": 0.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/AprilTags/Num targets", + "period": 0.06, + "data_type": "int", + "show_submit_button": false + } + }, + { + "title": "Last timestamp", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/AprilTags/Last timestamp", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "april tag distance meters", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/AprilTags/april tag distance meters", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "time since last reading", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/AprilTags/time since last reading", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Disable vision", + "x": 512.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/AprilTags/Disable vision", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + }, + { + "name": "Elevator", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Motor Current Position", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Elevator/Motor Current Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Arm Pivot", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Climb", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Is Climb OUT?", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Climb/Is Climb OUT?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Claw", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Claw Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Claw/Claw Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "GroundIntake", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Ground Arm", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Controls", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Swerve Coast Mode", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Swerve Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + }, + { + "name": "Autos", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "readyToScore?", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Autos/readyToScore?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Starting Position", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/Shuffleboard/Autos/Starting Position", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Launch Type", + "x": 512.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/Shuffleboard/Autos/Launch Type", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Game Objects", + "x": 640.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/Shuffleboard/Autos/Game Objects", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Available Auto Variants", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/Shuffleboard/Autos/Available Auto Variants", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Auto Delay", + "x": 512.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Autos/Auto Delay", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "photonvision_Port_1182_Output_MJPEG_Server", + "x": 256.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "photonvision_Port_1186_Output_MJPEG_Server", + "x": 640.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + }, + { + "name": "Field", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Auto display speed", + "x": 1280.0, + "y": 640.0, + "width": 384.0, + "height": 128.0, + "type": "Number Slider", + "properties": { + "topic": "/Shuffleboard/Field/Auto display speed", + "period": 0.06, + "data_type": "double", + "min_value": -1.0, + "max_value": 1.0, + "divisions": 5, + "update_continuously": false + } + }, + { + "title": "Est. Time (s)", + "x": 1280.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Field/Est. Time (s)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Selected auto", + "x": 0.0, + "y": 0.0, + "width": 1280.0, + "height": 768.0, + "type": "Field", + "properties": { + "topic": "/Shuffleboard/Field/Selected auto", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 + } + } + ] + } + }, + { + "name": "Match", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "has arm sensor", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has arm sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has ground intake sensor", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has ground intake sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has left branch sensor", + "x": 0.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has left branch sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has right branch sensor", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has right branch sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has recent vision measurements", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has recent vision measurements", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "left cam", + "x": 256.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/left", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "right cam", + "x": 768.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/right", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/M alliance push to G.auto b/src/main/deploy/pathplanner/autos/M alliance push to G.auto index fd68d1b6..4af2548c 100644 --- a/src/main/deploy/pathplanner/autos/M alliance push to G.auto +++ b/src/main/deploy/pathplanner/autos/M alliance push to G.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/M alliance push to H.auto b/src/main/deploy/pathplanner/autos/M alliance push to H.auto index 1cd9d382..5b5a0dbf 100644 --- a/src/main/deploy/pathplanner/autos/M alliance push to H.auto +++ b/src/main/deploy/pathplanner/autos/M alliance push to H.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/MLSF_H-G.auto b/src/main/deploy/pathplanner/autos/MLSF_H-G.auto index c28b77f6..09d9901a 100644 --- a/src/main/deploy/pathplanner/autos/MLSF_H-G.auto +++ b/src/main/deploy/pathplanner/autos/MLSF_H-G.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "H to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "H to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/MLSF_H-I.auto b/src/main/deploy/pathplanner/autos/MLSF_H-I.auto index a4eb9601..298b794c 100644 --- a/src/main/deploy/pathplanner/autos/MLSF_H-I.auto +++ b/src/main/deploy/pathplanner/autos/MLSF_H-I.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "H to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "H to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/MLSF_H-I_WithWait.auto b/src/main/deploy/pathplanner/autos/MLSF_H-I_WithWait.auto index 73d00664..0672aab3 100644 --- a/src/main/deploy/pathplanner/autos/MLSF_H-I_WithWait.auto +++ b/src/main/deploy/pathplanner/autos/MLSF_H-I_WithWait.auto @@ -23,9 +23,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "H to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "H to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -58,6 +71,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/MLSF_H-K_Cooking.auto b/src/main/deploy/pathplanner/autos/MLSF_H-K_Cooking.auto new file mode 100644 index 00000000..4b827a76 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MLSF_H-K_Cooking.auto @@ -0,0 +1,93 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M to H" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.5 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "H to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LSF to K" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MRSF_G-F.auto b/src/main/deploy/pathplanner/autos/MRSF_G-F.auto index ffd2eb2a..f36abd97 100644 --- a/src/main/deploy/pathplanner/autos/MRSF_G-F.auto +++ b/src/main/deploy/pathplanner/autos/MRSF_G-F.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "G to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "G to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/MRSF_G-F_WithWait.auto b/src/main/deploy/pathplanner/autos/MRSF_G-F_WithWait.auto index 07e9b909..46840309 100644 --- a/src/main/deploy/pathplanner/autos/MRSF_G-F_WithWait.auto +++ b/src/main/deploy/pathplanner/autos/MRSF_G-F_WithWait.auto @@ -23,9 +23,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "G to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "G to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -58,6 +71,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/MRSF_G-H.auto b/src/main/deploy/pathplanner/autos/MRSF_G-H.auto index 7fd2fec9..d64ec86c 100644 --- a/src/main/deploy/pathplanner/autos/MRSF_G-H.auto +++ b/src/main/deploy/pathplanner/autos/MRSF_G-H.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "G to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "G to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/M_G.auto b/src/main/deploy/pathplanner/autos/M_G.auto index 51f8c7c3..710fda95 100644 --- a/src/main/deploy/pathplanner/autos/M_G.auto +++ b/src/main/deploy/pathplanner/autos/M_G.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/M_H-GHA-IJA.auto b/src/main/deploy/pathplanner/autos/M_H-GHA-IJA.auto new file mode 100644 index 00000000..1be5e198 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/M_H-GHA-IJA.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M to H" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "path", + "data": { + "pathName": "H to GHA" + } + }, + { + "type": "named", + "data": { + "name": "algaeAlign23" + } + }, + { + "type": "path", + "data": { + "pathName": "GHA to Net" + } + }, + { + "type": "named", + "data": { + "name": "net" + } + }, + { + "type": "path", + "data": { + "pathName": "Net to IJA" + } + }, + { + "type": "named", + "data": { + "name": "algaeAlign34" + } + }, + { + "type": "path", + "data": { + "pathName": "IJA to Net" + } + }, + { + "type": "named", + "data": { + "name": "net" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/M_H.auto b/src/main/deploy/pathplanner/autos/M_H.auto index c177b290..7b007430 100644 --- a/src/main/deploy/pathplanner/autos/M_H.auto +++ b/src/main/deploy/pathplanner/autos/M_H.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSMRSF_D-C.auto b/src/main/deploy/pathplanner/autos/OSMRSF_D-C.auto index 863e34e8..e1be3597 100644 --- a/src/main/deploy/pathplanner/autos/OSMRSF_D-C.auto +++ b/src/main/deploy/pathplanner/autos/OSMRSF_D-C.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "D to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "D to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSMRSF_E-D.auto b/src/main/deploy/pathplanner/autos/OSMRSF_E-D.auto index 6f563b70..a179a262 100644 --- a/src/main/deploy/pathplanner/autos/OSMRSF_E-D.auto +++ b/src/main/deploy/pathplanner/autos/OSMRSF_E-D.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "E to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "E to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSMRSF_F-E.auto b/src/main/deploy/pathplanner/autos/OSMRSF_F-E.auto index 6e2b59c6..1474ae19 100644 --- a/src/main/deploy/pathplanner/autos/OSMRSF_F-E.auto +++ b/src/main/deploy/pathplanner/autos/OSMRSF_F-E.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "F to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "F to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSM_F.auto b/src/main/deploy/pathplanner/autos/OSM_F.auto index ee2d44e8..469e0ae5 100644 --- a/src/main/deploy/pathplanner/autos/OSM_F.auto +++ b/src/main/deploy/pathplanner/autos/OSM_F.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSWRSF_D-C-B.auto b/src/main/deploy/pathplanner/autos/OSWRSF_D-C-B.auto index e6cac332..2935d5f0 100644 --- a/src/main/deploy/pathplanner/autos/OSWRSF_D-C-B.auto +++ b/src/main/deploy/pathplanner/autos/OSWRSF_D-C-B.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "D to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "D to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "C to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "C to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSWRSF_D-C.auto b/src/main/deploy/pathplanner/autos/OSWRSF_D-C.auto index 1182d33a..7b03f10d 100644 --- a/src/main/deploy/pathplanner/autos/OSWRSF_D-C.auto +++ b/src/main/deploy/pathplanner/autos/OSWRSF_D-C.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "D to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "D to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSWRSF_E-D-C.auto b/src/main/deploy/pathplanner/autos/OSWRSF_E-D-C.auto new file mode 100644 index 00000000..5ad91c9b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/OSWRSF_E-D-C.auto @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OSW to E" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "E to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RSF to D" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "D to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RSF to C" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/OSWRSF_E-D.auto b/src/main/deploy/pathplanner/autos/OSWRSF_E-D.auto index 9e04289b..83c6c5ba 100644 --- a/src/main/deploy/pathplanner/autos/OSWRSF_E-D.auto +++ b/src/main/deploy/pathplanner/autos/OSWRSF_E-D.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "E to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "E to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSWRSF_F-E.auto b/src/main/deploy/pathplanner/autos/OSWRSF_F-E.auto index 4ce0c5f2..a4691f6d 100644 --- a/src/main/deploy/pathplanner/autos/OSWRSF_F-E.auto +++ b/src/main/deploy/pathplanner/autos/OSWRSF_F-E.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "F to RSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "F to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSW_E.auto b/src/main/deploy/pathplanner/autos/OSW_E.auto index aed8c787..4d060be2 100644 --- a/src/main/deploy/pathplanner/autos/OSW_E.auto +++ b/src/main/deploy/pathplanner/autos/OSW_E.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/OSW_F.auto b/src/main/deploy/pathplanner/autos/OSW_F.auto index 0d4f27a3..6e8f8349 100644 --- a/src/main/deploy/pathplanner/autos/OSW_F.auto +++ b/src/main/deploy/pathplanner/autos/OSW_F.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/PID TESTING.auto b/src/main/deploy/pathplanner/autos/PID TESTING.auto index 99e203c7..c061c423 100644 --- a/src/main/deploy/pathplanner/autos/PID TESTING.auto +++ b/src/main/deploy/pathplanner/autos/PID TESTING.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSC_K-L-A.auto b/src/main/deploy/pathplanner/autos/YSMLSC_K-L-A.auto index fead7d9c..6541b55d 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSC_K-L-A.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSC_K-L-A.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSC" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSC" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L to LSC" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L to LSC" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSF_I-J-K.auto b/src/main/deploy/pathplanner/autos/YSMLSF_I-J-K.auto index e4d8b8cb..63e22d8e 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSF_I-J-K.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSF_I-J-K.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "I to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "I to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSF_I-J.auto b/src/main/deploy/pathplanner/autos/YSMLSF_I-J.auto index 4f1f3e3d..72cdd3f0 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSF_I-J.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSF_I-J.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "I to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "I to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSF_J-K-L.auto b/src/main/deploy/pathplanner/autos/YSMLSF_J-K-L.auto index bd54118b..489afc64 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSF_J-K-L.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSF_J-K-L.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSF_J-K.auto b/src/main/deploy/pathplanner/autos/YSMLSF_J-K.auto index be253fde..9c1e8f29 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSF_J-K.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSF_J-K.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSF_K-L-A.auto b/src/main/deploy/pathplanner/autos/YSMLSF_K-L-A.auto index 31ed636c..de9d73bd 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSF_K-L-A.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSF_K-L-A.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSMLSF_K-L.auto b/src/main/deploy/pathplanner/autos/YSMLSF_K-L.auto index 34c108e0..54334bf7 100644 --- a/src/main/deploy/pathplanner/autos/YSMLSF_K-L.auto +++ b/src/main/deploy/pathplanner/autos/YSMLSF_K-L.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSM_I.auto b/src/main/deploy/pathplanner/autos/YSM_I.auto index e01625c4..099c9965 100644 --- a/src/main/deploy/pathplanner/autos/YSM_I.auto +++ b/src/main/deploy/pathplanner/autos/YSM_I.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSC_K-L-A.auto b/src/main/deploy/pathplanner/autos/YSWLSC_K-L-A.auto index 7951575f..30a7110d 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSC_K-L-A.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSC_K-L-A.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSC" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSC" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L to LSC" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L to LSC" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K-L.auto b/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K-L.auto index 85b21499..feaa279e 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K-L.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K-L.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "I to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "I to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -91,9 +117,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -126,6 +165,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K.auto b/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K.auto index ea4fc65a..43096066 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_I-J-K.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "I to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "I to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_I-J.auto b/src/main/deploy/pathplanner/autos/YSWLSF_I-J.auto index d3d3e0bf..942d34e0 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_I-J.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_I-J.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "I to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "I to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L-A.auto b/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L-A.auto index ec10acaa..e68b1048 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L-A.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L-A.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -91,9 +117,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -126,6 +165,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L.auto b/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L.auto index 6e9cd4ec..964c2f3f 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_J-K-L.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_J-K.auto b/src/main/deploy/pathplanner/autos/YSWLSF_J-K.auto index 1c79b339..854b7044 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_J-K.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_J-K.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "J to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "J to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_K-L-A.auto b/src/main/deploy/pathplanner/autos/YSWLSF_K-L-A.auto index 4fad1b87..675e615b 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_K-L-A.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_K-L-A.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -54,9 +67,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "L to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -89,6 +115,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSWLSF_K-L.auto b/src/main/deploy/pathplanner/autos/YSWLSF_K-L.auto index dad134a0..c5464da6 100644 --- a/src/main/deploy/pathplanner/autos/YSWLSF_K-L.auto +++ b/src/main/deploy/pathplanner/autos/YSWLSF_K-L.auto @@ -17,9 +17,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K to LSF" + "commands": [ + { + "type": "path", + "data": { + "pathName": "K to LSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] } }, { @@ -52,6 +65,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSW_I.auto b/src/main/deploy/pathplanner/autos/YSW_I.auto index 1e15a5c5..f90a9d80 100644 --- a/src/main/deploy/pathplanner/autos/YSW_I.auto +++ b/src/main/deploy/pathplanner/autos/YSW_I.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/YSW_J.auto b/src/main/deploy/pathplanner/autos/YSW_J.auto index 5ae4d38c..45107c9c 100644 --- a/src/main/deploy/pathplanner/autos/YSW_J.auto +++ b/src/main/deploy/pathplanner/autos/YSW_J.auto @@ -15,6 +15,12 @@ "data": { "name": "scoreCommand" } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/GHA to Net.path b/src/main/deploy/pathplanner/paths/GHA to Net.path new file mode 100644 index 00000000..ea38397e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/GHA to Net.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8, + "y": 4.001 + }, + "prevControl": null, + "nextControl": { + "x": 7.3, + "y": 4.001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.05, + "y": 5.024208047945206 + }, + "prevControl": { + "x": 6.582479508196721, + "y": 4.8563801790927466 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 1.6278356934807232 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.32596310201552 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H to GHA.path b/src/main/deploy/pathplanner/paths/H to GHA.path new file mode 100644 index 00000000..f0fce3f7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H to GHA.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.775, + "y": 4.19 + }, + "prevControl": null, + "nextControl": { + "x": 6.009923155196478, + "y": 4.104494964168583 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.417680921052631, + "y": 4.034621710526316 + }, + "prevControl": { + "x": 6.167680921052631, + "y": 4.034621710526316 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.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/IJA to Net.path b/src/main/deploy/pathplanner/paths/IJA to Net.path new file mode 100644 index 00000000..b82c3fd6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/IJA to Net.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.147615131578947, + "y": 5.131496710526315 + }, + "prevControl": null, + "nextControl": { + "x": 6.079392304067351, + "y": 5.222059452599042 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.4946546052631575, + "y": 6.084046052631579 + }, + "prevControl": { + "x": 5.352186266507238, + "y": 5.843643800581356 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LSC to A.path b/src/main/deploy/pathplanner/paths/LSC to A.path index a0f6414b..26e59ce3 100644 --- a/src/main/deploy/pathplanner/paths/LSC to A.path +++ b/src/main/deploy/pathplanner/paths/LSC to A.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.6035532994923858, - "y": 5.98489847715736 + "x": 1.3517075126618086, + "y": 5.840955845167175 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.2, - "y": 4.19 + "x": 2.4191352739726026, + "y": 4.257898116438356 }, "prevControl": { - "x": 2.2, - "y": 4.19 + "x": 2.059971697459254, + "y": 4.88991200854941 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSC to L.path b/src/main/deploy/pathplanner/paths/LSC to L.path index 89a0226d..0e40b122 100644 --- a/src/main/deploy/pathplanner/paths/LSC to L.path +++ b/src/main/deploy/pathplanner/paths/LSC to L.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.837, - "y": 6.69 + "x": 1.9277054790901271, + "y": 6.279908266152331 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7, - "y": 5.06 + "x": 3.4108304794520548, + "y": 5.6703125 }, "prevControl": { - "x": 2.7, - "y": 5.06 + "x": 2.6918068871123304, + "y": 5.927924463171795 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSF to A.path b/src/main/deploy/pathplanner/paths/LSF to A.path index 491eee9c..17eb58cd 100644 --- a/src/main/deploy/pathplanner/paths/LSF to A.path +++ b/src/main/deploy/pathplanner/paths/LSF to A.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.331091370558376, - "y": 5.212817258883249 + "x": 2.1613698438852404, + "y": 4.98487230542828 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.689597602739726, - "y": 4.227846746575342 + "x": 2.404109589041096, + "y": 4.167744006849315 }, "prevControl": { - "x": 2.3289811643835616, - "y": 5.144413527397259 + "x": 2.276151052080414, + "y": 4.825363581373981 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSF to G.path b/src/main/deploy/pathplanner/paths/LSF to G.path index b84c44e5..a5ba3ad9 100644 --- a/src/main/deploy/pathplanner/paths/LSF to G.path +++ b/src/main/deploy/pathplanner/paths/LSF to G.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.491095890410959, - "y": 3.777076198630137 + "x": 6.551198630136986, + "y": 3.8221532534246574 }, "prevControl": { - "x": 6.052345890410958, - "y": 5.853826198630138 + "x": 6.112448630136985, + "y": 5.89890325342466 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSF to I.path b/src/main/deploy/pathplanner/paths/LSF to I.path index 43609903..188c178a 100644 --- a/src/main/deploy/pathplanner/paths/LSF to I.path +++ b/src/main/deploy/pathplanner/paths/LSF to I.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.607, - "y": 7.268 + "x": 2.514588927591259, + "y": 7.077481604901863 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.46, - "y": 5.516749999999999 + "x": 5.754837328767123, + "y": 5.95580051369863 }, "prevControl": { - "x": 4.59225, - "y": 6.053 + "x": 4.853565610349696, + "y": 6.271865295143289 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSF to J.path b/src/main/deploy/pathplanner/paths/LSF to J.path index 99a9f51c..477ee1f9 100644 --- a/src/main/deploy/pathplanner/paths/LSF to J.path +++ b/src/main/deploy/pathplanner/paths/LSF to J.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.19746192893401, - "y": 6.95 + "x": 2.2388674985580006, + "y": 7.048265356051788 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0468237704918035, - "y": 5.427561475409837 + "x": 5.394220890410959, + "y": 6.106057363013699 }, "prevControl": { - "x": 3.3381689481567776, - "y": 6.806064013480902 + "x": 3.4200319836467106, + "y": 6.847607102170903 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSF to K.path b/src/main/deploy/pathplanner/paths/LSF to K.path index 9db00ddd..94236467 100644 --- a/src/main/deploy/pathplanner/paths/LSF to K.path +++ b/src/main/deploy/pathplanner/paths/LSF to K.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.167766497461929, - "y": 6.742131979695432 + "x": 2.1266851132688425, + "y": 6.954344033298825 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8600409836065577, - "y": 5.463524590163933 + "x": 3.6512414383561644, + "y": 5.940774828767124 }, "prevControl": { - "x": 2.538391237413664, - "y": 6.6303976866106344 + "x": 2.214511186489447, + "y": 6.827138096880983 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LSF to L.path b/src/main/deploy/pathplanner/paths/LSF to L.path index f1ff5d59..f3de7bc1 100644 --- a/src/main/deploy/pathplanner/paths/LSF to L.path +++ b/src/main/deploy/pathplanner/paths/LSF to L.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.68743654822335, - "y": 6.4451776649746195 + "x": 2.402245626080396, + "y": 6.563708882388173 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.516, - "y": 5.28 + "x": 3.3357020547945204, + "y": 5.7604666095890416 }, "prevControl": { - "x": 2.906629441624365, - "y": 5.935335025380711 + "x": 2.4141259140149467, + "y": 6.548351862061951 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/M to G.path b/src/main/deploy/pathplanner/paths/M to G.path index 27c5a637..6002622d 100644 --- a/src/main/deploy/pathplanner/paths/M to G.path +++ b/src/main/deploy/pathplanner/paths/M to G.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.443908629441624, - "y": 3.9210659898477163 + "x": 6.368946195869601, + "y": 3.8630765406060297 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.257581967213115, - "y": 3.8451844262295083 + "x": 6.4159674657534245, + "y": 3.8972816780821917 }, "prevControl": { - "x": 7.065054048431389, - "y": 3.926098131813265 + "x": 7.2716550043204755, + "y": 4.040735752369564 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/M to H.path b/src/main/deploy/pathplanner/paths/M to H.path index 32c5db3a..698fb430 100644 --- a/src/main/deploy/pathplanner/paths/M to H.path +++ b/src/main/deploy/pathplanner/paths/M to H.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.7177500000000006, - "y": 4.38575 + "x": 6.5570653690961676, + "y": 4.206783764695107 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.171749999999999, - "y": 4.2395 + "x": 6.400941780821918, + "y": 4.227846746575342 }, "prevControl": { - "x": 6.711963197969543, - "y": 4.217129441624365 + "x": 6.7731094850003, + "y": 4.122258082524106 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Net to IJA.path b/src/main/deploy/pathplanner/paths/Net to IJA.path new file mode 100644 index 00000000..d192575b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Net to IJA.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.8, + "y": 5.024 + }, + "prevControl": null, + "nextControl": { + "x": 7.5834936490538905, + "y": 5.149 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.36891447368421, + "y": 5.477878289473683 + }, + "prevControl": { + "x": 5.86891447368421, + "y": 6.343903693258121 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OSM to D.path b/src/main/deploy/pathplanner/paths/OSM to D.path index 947835e6..e77e7606 100644 --- a/src/main/deploy/pathplanner/paths/OSM to D.path +++ b/src/main/deploy/pathplanner/paths/OSM to D.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.161802030456853, - "y": 2.1393401015228424 + "x": 6.0913733447195035, + "y": 2.0003144001678237 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.681292808219178, - "y": 2.259482020547945 + "x": 3.5460616438356163, + "y": 2.034096746575342 }, "prevControl": { - "x": 4.0529780874069985, - "y": 1.8435378581113966 + "x": 3.7713919296055787, + "y": 1.9258088787653838 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/OSM to E.path b/src/main/deploy/pathplanner/paths/OSM to E.path index 3f525c0b..90c53422 100644 --- a/src/main/deploy/pathplanner/paths/OSM to E.path +++ b/src/main/deploy/pathplanner/paths/OSM to E.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.820304568527919, - "y": 2.30266497461929 + "x": 6.048653144755262, + "y": 2.040745098676402 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.226639344262295, - "y": 2.5145491803278683 + "x": 5.334118150684932, + "y": 2.0491224315068495 }, "prevControl": { - "x": 5.537055587917116, - "y": 2.2322344595156864 + "x": 5.572243236352498, + "y": 1.9729880530972612 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/OSM to F.path b/src/main/deploy/pathplanner/paths/OSM to F.path index 41fad33e..a0ca4648 100644 --- a/src/main/deploy/pathplanner/paths/OSM to F.path +++ b/src/main/deploy/pathplanner/paths/OSM to F.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.161802030456853, - "y": 2.094796954314721 + "x": 6.160239237638252, + "y": 2.19488475987901 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.4304303278688515, - "y": 2.6703893442622952 + "x": 5.649657534246575, + "y": 2.319584760273973 }, "prevControl": { - "x": 5.895191749188648, - "y": 2.4342472122825995 + "x": 6.433115085082489, + "y": 2.073535689064093 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/OSW to D.path b/src/main/deploy/pathplanner/paths/OSW to D.path index b61659d4..edb54bac 100644 --- a/src/main/deploy/pathplanner/paths/OSW to D.path +++ b/src/main/deploy/pathplanner/paths/OSW to D.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.130737704918032, - "y": 0.9201844262295075 + "x": 5.242329344865722, + "y": 0.9282979258133319 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7401639344262296, - "y": 2.167 + "x": 3.4764344262295084, + "y": 1.2078893442622962 }, "prevControl": { - "x": 4.0638319672131145, - "y": 1.9272459016393444 + "x": 3.7264342131609878, + "y": 1.2082157400571705 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/OSW to E.path b/src/main/deploy/pathplanner/paths/OSW to E.path index 98052dd1..1067a92b 100644 --- a/src/main/deploy/pathplanner/paths/OSW to E.path +++ b/src/main/deploy/pathplanner/paths/OSW to E.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.85, - "y": 1.8275380710659896 + "x": 5.451570336444508, + "y": 1.979872337991913 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.082786885245901, - "y": 2.5265368852459016 + "x": 5.319092465753425, + "y": 2.064148116438356 }, "prevControl": { - "x": 5.318725971540316, - "y": 2.123201859865192 + "x": 6.063055521702051, + "y": 1.5528540823655326 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/OSW to F.path b/src/main/deploy/pathplanner/paths/OSW to F.path index 0a7f46e2..3448ecf8 100644 --- a/src/main/deploy/pathplanner/paths/OSW to F.path +++ b/src/main/deploy/pathplanner/paths/OSW to F.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.939086294416243, - "y": 2.094796954314721 + "x": 6.234010211085658, + "y": 1.7072198901948554 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.370491803278688, - "y": 2.7183401639344265 + "x": 5.649657534246575, + "y": 2.169327910958904 }, "prevControl": { - "x": 5.761014645918282, - "y": 2.066461991345594 + "x": 6.555582519045872, + "y": 1.3775916415487004 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to B.path b/src/main/deploy/pathplanner/paths/RSF to B.path index d6770140..8507eeec 100644 --- a/src/main/deploy/pathplanner/paths/RSF to B.path +++ b/src/main/deploy/pathplanner/paths/RSF to B.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.2327500000000002, - "y": 1.96775 + "x": 1.8846663891273412, + "y": 1.7890603511905645 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.7982500000000003, - "y": 3.8299999999999996 + "x": 2.5393407534246575, + "y": 3.9123073630136984 }, "prevControl": { - "x": 1.7982500000000003, - "y": 3.8299999999999996 + "x": 2.3872239539700715, + "y": 3.4071940981639575 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to C.path b/src/main/deploy/pathplanner/paths/RSF to C.path index 79d7a7ab..7be86f74 100644 --- a/src/main/deploy/pathplanner/paths/RSF to C.path +++ b/src/main/deploy/pathplanner/paths/RSF to C.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.402561475409836, - "y": 0.5965163934426232 + "x": 1.3142503318659007, + "y": 0.5107884755766519 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.261, - "y": 2.3946721311475403 + "x": 3.1704195205479455, + "y": 2.2144049657534244 }, "prevControl": { - "x": 2.4893045685279187, - "y": 1.5484690854622616 + "x": 2.320630657121808, + "y": 1.3862376905206348 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to D.path b/src/main/deploy/pathplanner/paths/RSF to D.path index 8552328b..cccf5c75 100644 --- a/src/main/deploy/pathplanner/paths/RSF to D.path +++ b/src/main/deploy/pathplanner/paths/RSF to D.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.583502538071066, - "y": 1.4118020304568528 + "x": 2.6774238324811903, + "y": 1.519962632106293 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.65625, - "y": 2.4066598360655735 + "x": 3.6211900684931506, + "y": 2.1392765410958905 }, "prevControl": { - "x": 2.6322499999999995, - "y": 1.426502475659483 + "x": 2.630879869599968, + "y": 1.4494190758590544 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to E.path b/src/main/deploy/pathplanner/paths/RSF to E.path index 1e902e98..b70669ff 100644 --- a/src/main/deploy/pathplanner/paths/RSF to E.path +++ b/src/main/deploy/pathplanner/paths/RSF to E.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.316243654822335, - "y": 0.8475888324873097 + "x": 2.36511185839093, + "y": 0.9996614485949848 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.1786885245901635, - "y": 2.442622950819673 + "x": 5.334118150684932, + "y": 2.0491224315068495 }, "prevControl": { - "x": 4.317048930681533, - "y": 1.700029042190232 + "x": 4.004679879204526, + "y": 1.4330609458452868 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to F.path b/src/main/deploy/pathplanner/paths/RSF to F.path index dc43a6ae..4f3b0a25 100644 --- a/src/main/deploy/pathplanner/paths/RSF to F.path +++ b/src/main/deploy/pathplanner/paths/RSF to F.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.607, - "y": 0.7539999999999996 + "x": 2.3529008587231566, + "y": 0.986468401334046 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.60625, - "y": 2.61125 + "x": 5.694734589041096, + "y": 2.169327910958904 }, "prevControl": { - "x": 4.5435, - "y": 1.8117499999999995 + "x": 4.5432025596974785, + "y": 1.6990414476620956 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to H.path b/src/main/deploy/pathplanner/paths/RSF to H.path index a99e28af..a43b766a 100644 --- a/src/main/deploy/pathplanner/paths/RSF to H.path +++ b/src/main/deploy/pathplanner/paths/RSF to H.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.36675, - "y": 4.17125 + "x": 6.671404109589041, + "y": 4.167744006849315 }, "prevControl": { - "x": 6.46425, - "y": 2.1919999999999993 + "x": 6.768904109589041, + "y": 2.1884940068493144 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/YSM to I.path b/src/main/deploy/pathplanner/paths/YSM to I.path index d8c98630..f44536a0 100644 --- a/src/main/deploy/pathplanner/paths/YSM to I.path +++ b/src/main/deploy/pathplanner/paths/YSM to I.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.35482233502538, - "y": 5.777030456852792 + "x": 5.880240962362071, + "y": 5.957582945903331 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.38247950819672, - "y": 5.3196721311475414 + "x": 5.709760273972603, + "y": 5.940774828767124 }, "prevControl": { - "x": 6.233281538653573, - "y": 5.871073146375967 + "x": 6.830692519001458, + "y": 6.154561560002686 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/YSM to J.path b/src/main/deploy/pathplanner/paths/YSM to J.path index 23634528..5dbfc778 100644 --- a/src/main/deploy/pathplanner/paths/YSM to J.path +++ b/src/main/deploy/pathplanner/paths/YSM to J.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.2360406091370555, - "y": 6.103680203045685 + "x": 6.6882137928684, + "y": 6.137183914739877 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0468237704918035, - "y": 5.475512295081967 + "x": 5.304066780821918, + "y": 6.171 }, "prevControl": { - "x": 5.744194328867437, - "y": 6.01008589914288 + "x": 6.389022032523196, + "y": 6.201879275443106 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/YSM to K.path b/src/main/deploy/pathplanner/paths/YSM to K.path index cb9465d3..526d108d 100644 --- a/src/main/deploy/pathplanner/paths/YSM to K.path +++ b/src/main/deploy/pathplanner/paths/YSM to K.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.107614213197969, - "y": 5.925507614213197 + "x": 5.147120736189634, + "y": 6.184941755377682 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8600409836065577, - "y": 5.427561475409837 + "x": 3.6512414383561644, + "y": 6.171 }, "prevControl": { - "x": 5.196827785637015, - "y": 6.3036325413996845 + "x": 4.57160709116394, + "y": 6.137715331588841 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/YSW to I.path b/src/main/deploy/pathplanner/paths/YSW to I.path index d65e0385..02a25482 100644 --- a/src/main/deploy/pathplanner/paths/YSW to I.path +++ b/src/main/deploy/pathplanner/paths/YSW to I.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.755710659898476, - "y": 6.7718274111675125 + "x": 6.370865383050579, + "y": 6.4299044996567 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.394467213114754, - "y": 5.367622950819673 + "x": 5.649657534246575, + "y": 5.7604666095890416 }, "prevControl": { - "x": 5.894467213114754, - "y": 6.233648354604111 + "x": 6.562710492530654, + "y": 6.628686908383531 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/YSW to J.path b/src/main/deploy/pathplanner/paths/YSW to J.path index 573a3db2..2ef091ff 100644 --- a/src/main/deploy/pathplanner/paths/YSW to J.path +++ b/src/main/deploy/pathplanner/paths/YSW to J.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.731218274111675, - "y": 6.088832487309645 + "x": 5.4389626445185355, + "y": 6.030396607560231 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.094774590163934, - "y": 5.535450819672132 + "x": 5.454323630136986, + "y": 6.106057363013699 }, "prevControl": { - "x": 5.850383727219771, - "y": 6.331131022717817 + "x": 6.551033231330316, + "y": 6.892553961575598 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/YSW to K.path b/src/main/deploy/pathplanner/paths/YSW to K.path index 31fa8cd1..e99c62aa 100644 --- a/src/main/deploy/pathplanner/paths/YSW to K.path +++ b/src/main/deploy/pathplanner/paths/YSW to K.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.723155737704918, - "y": 7.08186475409836 + "x": 6.358042231584079, + "y": 7.150379460409699 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8916523972602737, - "y": 5.535081335616439 + "x": 3.464446721311475, + "y": 6.530430327868853 }, "prevControl": { - "x": 4.363524590163935, - "y": 6.698258196721311 + "x": 5.032497674168523, + "y": 6.843219247840884 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 1667f5bc..39bbf9fd 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -28,25 +29,31 @@ import frc.robot.subsystems.ArmPivot; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.GroundArm; +import frc.robot.subsystems.auto.AutoAlgaeHeights; import frc.robot.subsystems.auto.AutoAlign; import frc.robot.subsystems.auto.BargeAlign; import frc.robot.util.AlgaeIntakeHeight; import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; +import frc.robot.util.ScoringType; +import frc.robot.util.SoloScoringMode; +import java.util.function.BooleanSupplier; public class Controls { - private static final int DRIVER_CONTROLLER_PORT = 0; - private static final int OPERATOR_CONTROLLER_PORT = 1; - private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 2; - private static final int ELEVATOR_CONTROLLER_PORT = 3; - private static final int CLIMB_TEST_CONTROLLER_PORT = 4; + private static final int SOLO_CONTROLLER_PORT = 0; + private static final int DRIVER_CONTROLLER_PORT = 1; + private static final int OPERATOR_CONTROLLER_PORT = 2; + private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 3; + private static final int ELEVATOR_CONTROLLER_PORT = 4; + private static final int CLIMB_TEST_CONTROLLER_PORT = 5; private final CommandXboxController driverController; private final CommandXboxController operatorController; private final CommandXboxController armPivotSpinnyClawController; private final CommandXboxController elevatorTestController; private final CommandXboxController climbTestController; + private final CommandXboxController soloController; private final Subsystems s; private final Sensors sensors; @@ -54,9 +61,12 @@ public class Controls { private BranchHeight branchHeight = BranchHeight.CORAL_LEVEL_FOUR; private ScoringMode scoringMode = ScoringMode.CORAL; + public ScoringMode intakeMode = ScoringMode.CORAL; + private SoloScoringMode soloScoringMode = SoloScoringMode.NO_GAME_PIECE; private AlgaeIntakeHeight algaeIntakeHeight = AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR; // Swerve stuff + // setting the max speed nad other similar variables depending on which drivebase it is public static final double MaxSpeed = RobotType.getCurrent() == RobotType.BONK ? BonkTunerConstants.kSpeedAt12Volts.in(MetersPerSecond) @@ -73,20 +83,23 @@ public class Controls { new SwerveRequest.FieldCentric() .withDeadband(0.0001) .withRotationalDeadband(0.0001) - .withDriveRequestType( - DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + .withDriveRequestType(DriveRequestType.Velocity); private final Telemetry logger = new Telemetry(MaxSpeed); + private final BooleanSupplier driveSlowMode; + public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { driverController = new CommandXboxController(DRIVER_CONTROLLER_PORT); operatorController = new CommandXboxController(OPERATOR_CONTROLLER_PORT); armPivotSpinnyClawController = new CommandXboxController(ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT); elevatorTestController = new CommandXboxController(ELEVATOR_CONTROLLER_PORT); climbTestController = new CommandXboxController(CLIMB_TEST_CONTROLLER_PORT); + soloController = new CommandXboxController(SOLO_CONTROLLER_PORT); this.s = s; this.sensors = sensors; this.superStructure = superStructure; + driveSlowMode = driverController.start(); configureDrivebaseBindings(); configureSuperStructureBindings(); configureElevatorBindings(); @@ -97,12 +110,49 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureAutoAlignBindings(); configureGroundSpinnyBindings(); configureGroundArmBindings(); + configureSoloControllerBindings(); + Shuffleboard.getTab("Elevator") + .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE); + Shuffleboard.getTab("Elevator") + .addString("Scoring Mode", () -> getSoloScoringMode().toString()); + } + + public SoloScoringMode getSoloScoringMode() { + return soloScoringMode; } private Trigger connected(CommandXboxController controller) { return new Trigger(() -> controller.isConnected()); } + // takes the X value from the joystick, and applies a deadband and input scaling + private double getDriveX() { + // Joystick +Y is back + // Robot +X is forward + double input = MathUtil.applyDeadband(-driverController.getLeftY(), 0.1); + double inputScale = driveSlowMode.getAsBoolean() ? 0.5 : 1; + return input * MaxSpeed * inputScale; + } + + // takes the Y value from the joystick, and applies a deadband and input scaling + private double getDriveY() { + // Joystick +X is right + // Robot +Y is left + double input = MathUtil.applyDeadband(-driverController.getLeftX(), 0.1); + double inputScale = driveSlowMode.getAsBoolean() ? 0.5 : 1; + return input * MaxSpeed * inputScale; + } + + // takes the rotation value from the joystick, and applies a deadband and input scaling + private double getDriveRotate() { + // Joystick +X is right + // Robot +angle is CCW (left) + double input = MathUtil.applyDeadband(-driverController.getRightX(), 0.1); + double inputScale = driveSlowMode.getAsBoolean() ? 0.5 : 1; + return input * MaxSpeed * inputScale; + } + + // all the current control bidings private void configureDrivebaseBindings() { if (s.drivebaseSubsystem == null) { // Stop running this method @@ -111,28 +161,24 @@ private void configureDrivebaseBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. + + // the driving command for just driving around s.drivebaseSubsystem.setDefaultCommand( // s.drivebaseSubsystem will execute this command periodically + + // applying the request to drive with the inputs s.drivebaseSubsystem .applyRequest( - () -> { - double getLeftX = MathUtil.applyDeadband(driverController.getLeftX(), 0.1); - double getLeftY = MathUtil.applyDeadband(driverController.getLeftY(), 0.1); - double getRightX = MathUtil.applyDeadband(driverController.getRightX(), 0.1); - double inputScale = driverController.start().getAsBoolean() ? 0.5 : 1; - return drive - .withVelocityX( - -getLeftY - * MaxSpeed - * inputScale) // Drive forward with negative Y (forward) - .withVelocityY( - -getLeftX * MaxSpeed * inputScale) // Drive left with negative X (left) - .withRotationalRate( - -getRightX - * MaxAngularRate - * inputScale); // Drive counterclockwise with negative X (left) - }) + () -> + drive + .withVelocityX(soloController.isConnected() ? getSoloDriveX() : getDriveX()) + .withVelocityY(soloController.isConnected() ? getSoloDriveY() : getDriveY()) + .withRotationalRate( + soloController.isConnected() ? getSoloDriveRotate() : getDriveRotate())) .withName("Drive")); + + // various former controls that were previously used and could be referenced in the future + // operatorController // .povUp() // .whileTrue( @@ -176,12 +222,16 @@ private void configureDrivebaseBindings() { .alongWith(rumble(driverController, 0.5, Seconds.of(0.3))) .withName("Reset gyro")); + // logging the telemetry s.drivebaseSubsystem.registerTelemetry(logger::telemeterize); + + // creats a swerve button that coasts the wheels var swerveCoastButton = Shuffleboard.getTab("Controls") .add("Swerve Coast Mode", false) .withWidget(BuiltInWidgets.kToggleButton) .getEntry(); + // coast the wheels new Trigger(() -> swerveCoastButton.getBoolean(false)) .whileTrue(s.drivebaseSubsystem.coastMotors()); } @@ -190,6 +240,7 @@ private void configureSuperStructureBindings() { if (superStructure == null) { return; } + superStructure.setBranchHeightSupplier(() -> branchHeight); // operator start button used for climb - bound in climb bindings operatorController .y() @@ -243,7 +294,7 @@ private void configureSuperStructureBindings() { Commands.deferredProxy( () -> switch (scoringMode) { - case CORAL -> Commands.none().withName("coral mode - no command"); + case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER); case ALGAE -> Commands.sequence( superStructure.algaeProcessorScore( driverController.rightBumper()), @@ -316,7 +367,8 @@ private void configureSuperStructureBindings() { driverController .b() - .onTrue(superStructure.groundIntake(driverController.x()).withName("Ground intake")); + .onTrue( + superStructure.quickGroundIntake(driverController.x()).withName("Quick Gound intake")); if (sensors.armSensor != null) { sensors @@ -336,6 +388,36 @@ private void configureSuperStructureBindings() { .withName("Automatic Intake")); } + if (sensors.armSensor != null) { + sensors + .armSensor + .inClaw() + .and(RobotModeTriggers.teleop()) + .onTrue( + Commands.runOnce( + () -> { + switch (intakeMode) { + case CORAL -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW; + case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; + } + }) + .withName("Set solo scoring mode")) + .onFalse( + Commands.runOnce( + () -> { + soloScoringMode = SoloScoringMode.NO_GAME_PIECE; + }) + .withName("Clear solo scoring mode")); + } + + if (s.climbPivotSubsystem.sensor != null) { + s.climbPivotSubsystem + .cageDetected() + .and(new Trigger(() -> s.climbPivotSubsystem.isClimbOut)) + .and(RobotModeTriggers.teleop()) + .onTrue(s.climbPivotSubsystem.toClimbed().withName("Automatic sensor Climbing")); + } + driverController .rightTrigger() .onTrue( @@ -343,9 +425,15 @@ private void configureSuperStructureBindings() { () -> { Command scoreCommand = switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER); case ALGAE -> Commands.sequence( - BargeAlign.bargeScore(s.drivebaseSubsystem, superStructure), + BargeAlign.bargeScore( + s.drivebaseSubsystem, + superStructure, + () -> getDriveX(), + () -> getDriveY(), + () -> getDriveRotate(), + driverController.rightBumper()), getAlgaeIntakeCommand()) .withName("Algae score then intake"); }; @@ -362,13 +450,55 @@ private Command getAlgaeIntakeCommand() { }; } - private Command getCoralBranchHeightCommand() { - return switch (branchHeight) { - case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper()); - case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper()); - case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper()); - case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper()); - }; + private Command getCoralBranchHeightCommand(ScoringType version) { + if (version == ScoringType.SOLOC_LEFT) { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne( + soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } else if (version == ScoringType.SOLOC_RIGHT) { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne( + soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } else { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper()); + case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper()); + case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper()); + case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper()); + }; + } } private void configureElevatorBindings() { @@ -454,6 +584,14 @@ private void configureElevatorBindings() { rumble(operatorController, 0.5, Seconds.of(0.3))) .ignoringDisable(true) .withName("Reset elevator zero")); + if (RobotBase.isSimulation()) { + s.elevatorSubsystem + .resetPosZero() + .ignoringDisable(true) + .withName("Sim - reset elevator zero") + .schedule(); + } + // operatorController.rightBumper().whileTrue(s.elevatorSubsystem.holdCoastMode()); var elevatorCoastButton = Shuffleboard.getTab("Controls") @@ -535,14 +673,19 @@ private void configureClimbPivotBindings() { s.climbPivotSubsystem.isClimbing().whileTrue(setClimbLEDs); } + // regularly run the advanced climb check s.climbPivotSubsystem.setDefaultCommand( s.climbPivotSubsystem.advanceClimbCheck().withName("Advance Climb Check")); + // check if the climb controller is connected, and whne start is pressed move to the next climb + // position connected(climbTestController) .and(climbTestController.start()) .onTrue(s.climbPivotSubsystem.advanceClimbTarget()); - operatorController.start().onTrue(s.climbPivotSubsystem.toClimbed()); - operatorController.rightTrigger().onTrue(s.climbPivotSubsystem.toClimbOut()); + // on start or right trigger, move to climb out or climbed respectively on operator + operatorController.start().onTrue(s.climbPivotSubsystem.toClimbOut()); + operatorController.rightTrigger().onTrue(s.climbPivotSubsystem.toClimbed()); + // manual control for climb test controller for negative direction connected(climbTestController) .and(climbTestController.rightTrigger(0.1)) .whileTrue( @@ -553,6 +696,7 @@ private void configureClimbPivotBindings() { * MathUtil.applyDeadband( climbTestController.getRightTriggerAxis(), 0.1)) .withName("Climb Manual Control")); + // manual control for climb test controller for positive direction connected(climbTestController) .and(climbTestController.leftTrigger(0.1)) .whileTrue( @@ -561,11 +705,14 @@ private void configureClimbPivotBindings() { () -> 0.6 * MathUtil.applyDeadband(climbTestController.getLeftTriggerAxis(), 0.1)) .withName("Climb Manual Control")); + + // climb coast button var climbCoastButton = Shuffleboard.getTab("Controls") .add("Climb Coast Mode", false) .withWidget(BuiltInWidgets.kToggleButton) .getEntry(); + // utilizes the climb coast button and coasts the climb new Trigger(() -> climbCoastButton.getBoolean(false)) .whileTrue(s.climbPivotSubsystem.coastMotors()); } @@ -610,10 +757,11 @@ private void configureElevatorLEDBindings() { } s.elevatorLEDSubsystem.setDefaultCommand( - s.elevatorLEDSubsystem.showScoringMode(() -> scoringMode)); + s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode)); if (s.elevatorSubsystem != null) { Trigger hasBeenZeroed = new Trigger(s.elevatorSubsystem::getHasBeenZeroed); + Trigger subZeroPosition = new Trigger(s.elevatorSubsystem::getPositionSubZero); Commands.waitSeconds(1) .andThen( s.elevatorLEDSubsystem @@ -633,6 +781,13 @@ private void configureElevatorLEDBindings() { s.elevatorLEDSubsystem .blink(120, 0, 0, "Red - Elevator Not Zeroed") .ignoringDisable(true)); + subZeroPosition.whileTrue( + Commands.waitSeconds(0.5) + .andThen( + s.elevatorLEDSubsystem + .blink(120, 0, 0, "Red - Elevator Position Error") + .ignoringDisable(true) + .withName("Red - Elevator Position Error"))); } RobotModeTriggers.autonomous() .whileTrue(s.elevatorLEDSubsystem.animate(LEDPattern.rainbow(255, 255), "Auto Rainbow")); @@ -678,7 +833,12 @@ private void configureAutoAlignBindings() { .rightTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); + driverController + .leftTrigger() + .and(() -> scoringMode == ScoringMode.CORAL) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); } private void configureGroundSpinnyBindings() { @@ -711,7 +871,7 @@ private Command selectScoringHeight(BranchHeight b, AlgaeIntakeHeight a) { () -> { branchHeight = b; algaeIntakeHeight = a; - if (scoringMode == ScoringMode.ALGAE + if (intakeMode == ScoringMode.ALGAE && (sensors.armSensor == null || !sensors.armSensor.booleanInClaw())) { CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand()); } @@ -750,4 +910,228 @@ public void vibrateCoDriveController(double vibration) { operatorController.getHID().setRumble(RumbleType.kBothRumble, vibration); } } + + private double getJoystickInput(double input) { + if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { + return 0; // stop driving if either stick is pressed + } + // Apply a deadband to the joystick input + double deadbandedInput = MathUtil.applyDeadband(input, 0.1); + return deadbandedInput; + } + + // Drive for Solo controller + // takes the X value from the joystick, and applies a deadband and input scaling + private double getSoloDriveX() { + // Joystick +Y is back + // Robot +X is forward + return getJoystickInput(-soloController.getLeftY()) * MaxSpeed; + } + + // takes the Y value from the joystick, and applies a deadband and input scaling + private double getSoloDriveY() { + // Joystick +X is right + // Robot +Y is left + return getJoystickInput(-soloController.getLeftX()) * MaxSpeed; + } + + // takes the rotation value from the joystick, and applies a deadband and input scaling + private double getSoloDriveRotate() { + // Joystick +X is right + // Robot +angle is CCW (left) + return getJoystickInput(-soloController.getRightX()) * MaxSpeed; + } + + private void configureSoloControllerBindings() { + // Barge + Auto align left + Select scoring mode Algae + soloController + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> { + Command scoreCommand; + switch (soloScoringMode) { + case CORAL_IN_CLAW -> { + scoreCommand = + getCoralBranchHeightCommand(ScoringType.SOLOC_LEFT) + .until( + () -> + soloController.a().getAsBoolean() + || soloController.b().getAsBoolean() + || soloController.x().getAsBoolean() + || soloController.y().getAsBoolean()); + } + case ALGAE_IN_CLAW -> { + Command bargeScoreCommand = + BargeAlign.bargeScore( + s.drivebaseSubsystem, + superStructure, + () -> getSoloDriveX(), + () -> getSoloDriveY(), + () -> getSoloDriveRotate(), + soloController.rightBumper()) + .withName("Algae score then intake"); + scoreCommand = + Commands.sequence( + bargeScoreCommand, + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); + } + case NO_GAME_PIECE -> { + scoreCommand = + Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) + .alongWith(scoringModeSelectRumble()) + .withName("Algae Scoring Mode"), + AutoAlgaeHeights.autoAlgaeIntakeCommand( + s.drivebaseSubsystem, superStructure, this) + .until(() -> sensors.armSensor.booleanInClaw())); + } + default -> scoreCommand = Commands.none(); + } + CommandScheduler.getInstance().schedule(scoreCommand); + })); + soloController + .leftTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); + soloController + .leftTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB)); + // Processor + Auto align right + Select scoring mode Coral + soloController + .rightTrigger() + .onTrue( + Commands.runOnce( + () -> { + Command scoreCommand = + switch (soloScoringMode) { + case CORAL_IN_CLAW -> getCoralBranchHeightCommand( + ScoringType.SOLOC_RIGHT) + .until( + () -> + soloController.a().getAsBoolean() + || soloController.b().getAsBoolean() + || soloController.x().getAsBoolean() + || soloController.y().getAsBoolean()); + case ALGAE_IN_CLAW -> Commands.sequence( + superStructure.algaeProcessorScore( + soloController.rightBumper()), + Commands.waitSeconds(0.7), + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + .withName("Processor score"); + case NO_GAME_PIECE -> Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure.coralPreIntake(), + s.climbPivotSubsystem.toStow()); + }; + CommandScheduler.getInstance().schedule(scoreCommand); + }) + .withName("score")); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB)); + // Ground Intake + soloController + .leftBumper() + .onTrue( + Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.CORAL), + superStructure.quickGroundIntake(soloController.povUp())) + .withName("Quick Gound intake")); + // Scoring levels coral and algae intake heights + soloController + .y() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) + .withName("coral level 4, algae level 3-4")); + soloController + .x() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) + .withName("coral level 3, algae level 2-3")); + soloController + .b() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) + .withName("coral level 2, algae level 2-3")); + soloController + .a() + .onTrue( + selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) + .withName("coral level 1, algae ground level")); + // Zero Elevator + soloController + .back() + .onTrue( + Commands.parallel( + s.elevatorSubsystem.resetPosZero(), + rumble(soloController, 0.5, Seconds.of(0.3))) + .ignoringDisable(true) + .withName("Reset elevator zero")); + // Reset gyro + soloController + .start() + .onTrue( + s.drivebaseSubsystem + .runOnce(() -> s.drivebaseSubsystem.seedFieldCentric()) + .alongWith(rumble(soloController, 0.5, Seconds.of(0.3))) + .withName("Reset gyro")); + // Funnel Out + soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbOut()); + // Funnel Climbed + soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbed()); + // Intake button + soloController + .povDown() + .onTrue( + superStructure + .coralIntake() + .alongWith( + s.elevatorLEDSubsystem != null + ? s.elevatorLEDSubsystem + .tripleBlink(255, 92, 0, "Orange - Manual Coral Intake") + .asProxy() + : Commands.none()) + .withName("Manual Coral Intake")); + // Ground Intake Hold out + soloController + .povUp() + .whileTrue( + s.groundArm + .moveToPosition(GroundArm.GROUND_POSITION) + .andThen(Commands.idle()) + .withName("ground up position")); + // Arm manual + soloController + .rightStick() + .whileTrue( + s.armPivotSubsystem + .startMovingVoltage(() -> Volts.of(3 * soloController.getRightY())) + .withName("Arm Manual Control")); + // Elevator manual + soloController + .leftStick() + .whileTrue( + s.elevatorSubsystem + .startMovingVoltage( + () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) + .withName("Elevator Manual Control")); + } } diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java index c1e01d95..08553e5f 100644 --- a/src/main/java/frc/robot/Hardware.java +++ b/src/main/java/frc/robot/Hardware.java @@ -43,7 +43,7 @@ public class Hardware { public static final int ELEVATOR_ZERO_BUTTON = 0; // climb DIO - public static final int CLIMB_SENSOR = 1; + public static final int CLIMB_SENSOR = 8; // vision public static final String PHOTON_IP = "10.24.12.11"; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 83ae2322..77cb3362 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,8 +6,10 @@ import au.grapplerobotics.CanBridge; import com.pathplanner.lib.commands.FollowPathCommand; +import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; @@ -36,7 +38,6 @@ public static Robot getInstance() { private final RobotType robotType; public final Controls controls; public final Subsystems subsystems; - public final Sensors sensors; public final SuperStructure superStructure; private final PowerDistribution PDH; @@ -106,6 +107,7 @@ protected Robot() { FollowPathCommand.warmupCommand().schedule(); } MatchTab.create(sensors, subsystems); + WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); } @Override @@ -124,6 +126,8 @@ public void disabledPeriodic() {} @Override public void disabledExit() { + // on the end of diabling, make sure all of the motors are set to break and wont move upon + // enabling if (subsystems.drivebaseSubsystem != null) { subsystems.drivebaseSubsystem.brakeMotors(); } diff --git a/src/main/java/frc/robot/SuperStructure.java b/src/main/java/frc/robot/SuperStructure.java index 510c9675..8aabfdf1 100644 --- a/src/main/java/frc/robot/SuperStructure.java +++ b/src/main/java/frc/robot/SuperStructure.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.sensors.ArmSensor; import frc.robot.sensors.BranchSensors; @@ -12,7 +13,10 @@ import frc.robot.subsystems.GroundArm; import frc.robot.subsystems.GroundSpinny; import frc.robot.subsystems.SpinnyClaw; +import frc.robot.util.BranchHeight; +import java.util.Set; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; public class SuperStructure { private final ElevatorSubsystem elevator; @@ -24,6 +28,7 @@ public class SuperStructure { private final ArmSensor armSensor; private final BranchSensors branchSensors; private final IntakeSensor intakeSensor; + private Supplier branchHeight = () -> BranchHeight.CORAL_LEVEL_TWO; public SuperStructure( ElevatorSubsystem elevator, @@ -46,6 +51,10 @@ public SuperStructure( this.intakeSensor = intakeSensor; } + public void setBranchHeightSupplier(Supplier branchHeight) { + this.branchHeight = branchHeight; + } + private Command colorSet(int r, int g, int b, String name) { if (elevatorLight == null) { return Commands.none(); @@ -54,6 +63,7 @@ private Command colorSet(int r, int g, int b, String name) { } private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) { + // repeats scoring sequence if the coral is still in the claw if (armSensor == null) { return Commands.sequence( command, Commands.waitUntil(() -> !score.getAsBoolean()), Commands.waitUntil(score)); @@ -62,16 +72,21 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) } } - private Command untilClawFull(Command command) { + private Command repeatPrescoreScoreSwing( + Command command, BooleanSupplier score, BooleanSupplier ipScore) { + // repeats scoring sequence if the coral is still in the claw if (armSensor == null) { - return command; + return Commands.sequence( + command, + Commands.waitUntil(() -> !score.getAsBoolean()), + Commands.waitUntil(ipScore).until(score)); } else { - return command.withDeadline(Commands.waitUntil(armSensor.inClaw())); + return command.repeatedly().onlyWhile(armSensor.inClaw()); } } public Command coralLevelFour(BooleanSupplier score) { - if (branchSensors != null) { + if (branchSensors != null) { // checks if sensor enabled then use for faster scoring score = branchSensors.withinScoreRange().or(score); } return Commands.sequence( @@ -79,7 +94,7 @@ public Command coralLevelFour(BooleanSupplier score) { Commands.print("Pre position"), elevator .setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS) - .deadlineFor( + .deadlineFor( // keeps spinny claw engaged until coral has been scored armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), spinnyClaw.stop()) .withTimeout(0.7), @@ -88,20 +103,27 @@ public Command coralLevelFour(BooleanSupplier score) { Commands.parallel( elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS), armPivot.moveToPosition(ArmPivot.CORAL_PRESET_PRE_L4)) - .withDeadline(Commands.waitUntil(score)), + .withDeadline( + Commands.waitUntil( + score)), // waits until driver presses the score button or until + // auto scoring happens armPivot .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) .withTimeout(1.5) .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), score), Commands.print("Pre preIntake()"), - coralPreIntake(), + coralPreIntake() + .unless( + RobotModeTriggers + .autonomous()), // doesn't go to preintake if auto, should add for otehr + // commands Commands.print("Post preIntake()")) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L4").asProxy()) .withName("Coral Level 4"); } - public Command coralLevelThree(BooleanSupplier score) { + public Command coralLevelThree(BooleanSupplier score) { // same as L4 return Commands.sequence( Commands.parallel( elevator @@ -125,7 +147,7 @@ public Command coralLevelThree(BooleanSupplier score) { .withName("Coral Level 3"); } - public Command coralLevelTwo(BooleanSupplier score) { + public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3 return Commands.sequence( Commands.parallel( elevator @@ -154,17 +176,99 @@ public Command coralLevelOne(BooleanSupplier score) { Commands.parallel( elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), - spinnyClaw.stop()) + spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(score)), - spinnyClaw.coralHoldExtakePower().withTimeout(0.25), + spinnyClaw.coralLevelOneHoldExtakePower().withTimeout(0.25), // spits out coral + Commands.waitSeconds(1), // Wait to clear the reef + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) + .withName("Coral Level 1"); + } + + // New versions with ipScore + // if robot is in position for intermediate scoring, it can score early but if vision is dead + // manual control still works + // only used on solo controls + + public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.repeatingSequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .withTimeout(1.5) + .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + score, + ipScore), + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) + .withName("Coral Level 3"); + } + + public Command coralLevelTwo( + BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.sequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L2) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .withTimeout(1.5) + .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + score, + ipScore), + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) + .withName("Coral Level 2"); + } + + public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { + return Commands.sequence( + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), + spinnyClaw.stop()) // holds coral without wearing flywheels + .withTimeout(0.5) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + spinnyClaw + .coralLevelOneHoldExtakePower() + .withTimeout(0.5 /* this time could be shorter */), // spits out coral Commands.waitSeconds(1), // Wait to clear the reef coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) .withName("Coral Level 1"); } - public Command groundIntake(BooleanSupplier retract) { // untested + // quickGroundIntake() is used instead since it's faster and still reliable. + // (This one moves the coral intake the normal intake position, then does the normal intake. + // quickGroundIntake() instead hands the coral directly to the claw.) + public Command groundIntake(BooleanSupplier retract) { if (groundSpinny == null || groundArm == null || intakeSensor == null) { return Commands.none().withName("ground intake disabled"); } else { @@ -175,10 +279,19 @@ public Command groundIntake(BooleanSupplier retract) { // untested armPivot.moveToPosition(ArmPivot.CORAL_PRESET_GROUND_INTAKE), spinnyClaw.stop(), // just as a backup in case things are silly groundSpinny.setGroundIntakePower()) - .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), + .until( + elevator.above( + ElevatorSubsystem + .MIN_EMPTY_GROUND_INTAKE)), // waits until elevator is out of the way + // to lift ground intake groundArm .moveToPosition(GroundArm.GROUND_POSITION) - .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))), + .andThen(groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE)) + .withDeadline( + Commands.waitUntil( + intakeSensor + .inIntake() + .or(retract))), // ends ground pickup to stow coral for scoring groundArm.moveToPosition(GroundArm.STOWED_POSITION), groundSpinny.setFunnelIntakePower(), coralPreIntake()) @@ -187,11 +300,125 @@ public Command groundIntake(BooleanSupplier retract) { // untested } } + // This is the actual version in use. It moves the coral directly into the claw. + public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph + if (groundSpinny == null || groundArm == null || intakeSensor == null) { + // If anything's disabled, completely give up. (Theoretically we might be able to do some + // stuff, but it wasn't worth implementing in the time we had.) + return Commands.none().withName("quick ground intake disabled"); + } else { + // BooleanSupplier for whether the claw is full. This is a separate variable so that it gets a + // name (so it's easier to read). + BooleanSupplier clawFull = armSensor != null ? armSensor.inClaw() : () -> false; + // Make the big sequence. + return Commands.sequence( + // Initial setup- Move elevator high enough for ground arm to be clear, start moving + // arm pivot, and start spinning the ground intake + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.coralIntakePower(), + groundSpinny.setGroundIntakePower()) + // Move on even if arm isn't in position yet as long as elevator is high enough + .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), + // Core intake sequence + Commands.sequence( + // Deploy the ground arm (and wait until it reaches the position). + groundArm.moveToPosition(GroundArm.GROUND_POSITION), + // After it's deployed, apply a constant voltage to press it into the bumper + // and continue. + groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE), + // Grabbing segment + Commands.parallel( + // These three are the initial setup: Move elevator down to the handoff + // height, make sure armPivot finishes moving to the right height, and + // spin claw + elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.coralIntakePower(), + // Run the intake with occassional jitters. (This is stopped when the core + // intake sequence is stopped.) + Commands.sequence( + // Wait until the jitter condition (stalling for at least 0.25 + // seconds and we don't have a coral) + Commands.waitUntil( + groundSpinny + .stalling() + .debounce(0.25) + .and(intakeSensor.inIntake().negate())), + // Then, spin out at the jitter speed for 0.1 seconds. + groundSpinny.holdGroundIntakeJitterSpeed().withTimeout(0.1)) + // Whenever we move on, make sure to set the ground spinny back to the + // ground intake speed. The finallyDo() ensures this happens no matter + // when we move on. + .finallyDo(() -> groundSpinny.imperativeSetGroundIntakePower()) + // Repeat the wait and jitter until we move on. + .repeatedly())) + // Move on from the core intake sequence as soon as we either have something in + // ground intake or we tell it to retract. + .withDeadline(Commands.waitUntil(intakeSensor.inIntake().or(retract))), + // Retract the groundArm all the way to stowed (so that we aren't slowing down to stop + // in the middle), but move on to the next command when we're at the handoff point. + groundArm + .moveToPosition(GroundArm.STOWED_POSITION) + .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)), + // Spin groundSpinny out, but skip if we lost the coral. + groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), + // Go back to stow, but skip if we lost the coral. + coralStow().onlyIf(armSensor.inClaw()), + // If we lost the coral, reset for a quick retry. + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + spinnyClaw.stop()) + .onlyIf(armSensor.inClaw().negate())) + // Don't run the entire sequence at all if the claw is full. + .unless(clawFull) + .withName("Ground Intake"); + } + } + public Command coralStow() { - return Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_STOWED), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_STOWED), - spinnyClaw.stop()) + return Commands.defer( + () -> { + BranchHeight currentBranchHeight = branchHeight.get(); + double elevatorHeight = + switch (currentBranchHeight) { // used to stow closer to the scoring height + case CORAL_LEVEL_FOUR -> ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS; + case CORAL_LEVEL_THREE -> ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS; + case CORAL_LEVEL_TWO -> ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS; + case CORAL_LEVEL_ONE -> ElevatorSubsystem.CORAL_LEVEL_ONE_POS; + }; + if (elevatorHeight > ElevatorSubsystem.CORAL_PRE_INTAKE) { + return Commands.parallel( + elevator.setLevel(elevatorHeight), + Commands.sequence( // waits to raise the arm until elevator is above preintake + Commands.waitUntil(elevator.above(ElevatorSubsystem.CORAL_PRE_INTAKE)), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP)), + spinnyClaw.stop()); + } else { + return Commands.parallel( + Commands.sequence( + elevator.setLevel(ElevatorSubsystem.CORAL_PRE_INTAKE), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP), + elevator.setLevel(elevatorHeight)), + spinnyClaw.stop()); + } + }, + Set.of(elevator, armPivot, spinnyClaw)) + .withName("Coral Stow"); + } + + public Command autoCoralStow() { + return Commands.defer( + () -> + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS), + Commands.sequence( + Commands.waitUntil(elevator.above(ElevatorSubsystem.CORAL_PRE_INTAKE)), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP)), + spinnyClaw.stop()), + Set.of(elevator, armPivot, spinnyClaw)) .withName("Coral Stow"); } @@ -199,33 +426,43 @@ public Command coralPreIntake() { return Commands.parallel( elevator.setLevel(ElevatorSubsystem.CORAL_PRE_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN), - spinnyClaw.coralRejectPower()) + spinnyClaw.coralRejectPower()) // keeps coral out while waiting to intake .andThen(Commands.print("end of preIntake()")) .withName("PreIntake"); } - public Trigger inCoralPreIntakePosition() { + public Trigger + inCoralPreIntakePosition() { // creates a trigger for coral intake that waits until robot is + // in preintake position return new Trigger( () -> elevator.atPosition(ElevatorSubsystem.CORAL_PRE_INTAKE) && armPivot.atPosition(ArmPivot.CORAL_PRESET_DOWN)); } - public Command coralIntake() { + public Command coralIntake() { // yummy coral return Commands.sequence( - untilClawFull( - Commands.sequence( - Commands.parallel( - spinnyClaw.coralIntakePower(), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)), - elevator.setLevel(ElevatorSubsystem.CORAL_INTAKE_POS))), - spinnyClaw.stop(), - elevator.setLevel(ElevatorSubsystem.CORAL_STOWED), + Commands.sequence( + Commands.parallel( + spinnyClaw.coralIntakePower(), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)), + elevator.setLevel(ElevatorSubsystem.CORAL_INTAKE_POS)), coralStow()) .withName("Coral Intake"); } - public Command algaeLevelThreeFourIntake() { + public Command autoCoralIntake() { // yummy coral + return Commands.sequence( + Commands.sequence( + Commands.parallel( + spinnyClaw.coralIntakePower(), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)), + elevator.setLevel(ElevatorSubsystem.CORAL_INTAKE_POS)), + autoCoralStow()) + .withName("Coral Intake"); + } + + public Command algaeLevelThreeFourIntake() { // removes algae from upper reef return Commands.sequence( Commands.parallel( spinnyClaw.algaeIntakePower(), @@ -234,7 +471,7 @@ public Command algaeLevelThreeFourIntake() { .withName("Algae L3-L4 Intake"); } - public Command algaeLevelTwoThreeIntake() { + public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.sequence( Commands.parallel( spinnyClaw.algaeIntakePower(), @@ -243,7 +480,8 @@ public Command algaeLevelTwoThreeIntake() { .withName("Algae L2-L3 Intake"); } - public Command algaeGroundIntake() { + public Command + algaeGroundIntake() { // picks up algae on the ground (mostly only works against a wall) return Commands.parallel( spinnyClaw.algaeIntakePower(), Commands.sequence( @@ -252,7 +490,7 @@ public Command algaeGroundIntake() { .withName("Algae Ground Intake"); } - public Command algaeStow() { + public Command algaeStow() { // holds algae return Commands.parallel( elevator.setLevel(ElevatorSubsystem.ALGAE_STOWED), armPivot.moveToPosition(ArmPivot.ALGAE_STOWED), @@ -261,7 +499,7 @@ public Command algaeStow() { .withName("Algae Stow"); } - public Command algaeProcessorScore(BooleanSupplier score) { + public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in processor return Commands.sequence( Commands.parallel( spinnyClaw.algaeGripIntakePower(), @@ -282,7 +520,8 @@ public Command algaeNetScore(BooleanSupplier score) { Commands.waitUntil(score), spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), Commands.waitSeconds(0.7), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP), + armPivot.moveToPosition( + ArmPivot.CORAL_PRESET_UP), // added to prevent hitting the barge after scoring net elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) .withName("Algae Net Score"); } diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 978ac85d..a9394ce9 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -1,17 +1,38 @@ package frc.robot.generated; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.*; -import com.ctre.phoenix6.hardware.*; -import com.ctre.phoenix6.signals.*; -import com.ctre.phoenix6.swerve.*; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator @@ -33,7 +54,7 @@ public class CompTunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + new Slot0Configs().withKP(0.2).withKI(0).withKD(0).withKS(0.1).withKV(0.124).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -55,11 +76,18 @@ public class CompTunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(80.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(160)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(100)) + .withSupplyCurrentLimitEnable(true)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -68,7 +96,9 @@ public class CompTunerConstants { // low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(40)) + .withSupplyCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; diff --git a/src/main/java/frc/robot/sensors/ArmSensor.java b/src/main/java/frc/robot/sensors/ArmSensor.java index 2f100a33..49becd21 100644 --- a/src/main/java/frc/robot/sensors/ArmSensor.java +++ b/src/main/java/frc/robot/sensors/ArmSensor.java @@ -17,9 +17,9 @@ public class ArmSensor { private final LaserCan mainSensor; // VALUES ARE IN METERS private static final double TROUGH_LOWER_LIMIT = 0.10; - private static final double TROUGH_UPPER_LIMIT = 0.20; + private static final double TROUGH_UPPER_LIMIT = 0.25; private static final double CLAW_LOWER_LIMIT = 0.01; - private static final double CLAW_UPPER_LIMIT = 0.09; + private static final double CLAW_UPPER_LIMIT = 0.13; public ArmSensor() { mainSensor = new LaserCan(Hardware.MAIN_ARM_SENSOR); @@ -42,11 +42,17 @@ private void ConfigureSensor(LaserCan Sensor) { public Distance getSensorDistance() { LaserCan.Measurement measurement = mainSensor.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - return Millimeter.of(measurement.distance_mm); - } else { + if (measurement == null) { return Millimeter.of(-1); } + if (measurement.status != LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + if (measurement.status != LaserCan.LASERCAN_STATUS_OUT_OF_BOUNDS) { + return Millimeter.of(-2); + } else { + return Millimeter.of(-3); + } + } + return Millimeter.of(measurement.distance_mm); } public Trigger inTrough() { diff --git a/src/main/java/frc/robot/sensors/BranchSensors.java b/src/main/java/frc/robot/sensors/BranchSensors.java index b36ddf4b..a9735843 100644 --- a/src/main/java/frc/robot/sensors/BranchSensors.java +++ b/src/main/java/frc/robot/sensors/BranchSensors.java @@ -45,11 +45,17 @@ private void ConfigureSensor(LaserCan Sensor) { private Distance getSensorDistance(LaserCan sensor) { LaserCan.Measurement measurement = sensor.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - return Millimeter.of(measurement.distance_mm); - } else { + if (measurement == null) { return Millimeter.of(-1); } + if (measurement.status != LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + if (measurement.status != LaserCan.LASERCAN_STATUS_OUT_OF_BOUNDS) { + return Millimeter.of(-2); + } else { + return Millimeter.of(-3); + } + } + return Millimeter.of(measurement.distance_mm); } public Distance getLeftSensorDistance() { diff --git a/src/main/java/frc/robot/sensors/ElevatorLight.java b/src/main/java/frc/robot/sensors/ElevatorLight.java index bba94b64..f6139f74 100644 --- a/src/main/java/frc/robot/sensors/ElevatorLight.java +++ b/src/main/java/frc/robot/sensors/ElevatorLight.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; -import frc.robot.util.ScoringMode; +import frc.robot.util.SoloScoringMode; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -82,13 +82,15 @@ public Command animate(LEDPattern animation, String name) { .withName("Animate" + name); } - public Command showScoringMode(Supplier scoringMode) { + public Command showScoringMode(Supplier scoringMode) { return run(() -> { - ScoringMode currentMode = scoringMode.get(); - if (currentMode == ScoringMode.ALGAE) { + SoloScoringMode currentMode = scoringMode.get(); + if (currentMode == SoloScoringMode.ALGAE_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kTeal)); - } else { + } else if (currentMode == SoloScoringMode.CORAL_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kWhite)); + } else if (currentMode == SoloScoringMode.NO_GAME_PIECE) { + updateLEDs(LEDPattern.solid(Color.kDeepPink)); } }) .withName("Animate Scoring Mode"); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 08f93165..5d77eddc 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -15,8 +15,8 @@ public class IntakeSensor { private final LaserCan intakeSensor; // VALUES ARE IN METERS - private static final double INTAKE_LOWER_LIMIT = 0.010; - private static final double INTAKE_UPPER_LIMIT = 0.1; + private static final double INTAKE_LOWER_LIMIT = 0.005; + private static final double INTAKE_UPPER_LIMIT = 0.05; public IntakeSensor() { intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR); @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); @@ -38,11 +38,17 @@ private void ConfigureSensor(LaserCan Sensor) { public Distance getSensorDistance() { LaserCan.Measurement measurement = intakeSensor.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - return Millimeter.of(measurement.distance_mm); - } else { + if (measurement == null) { return Millimeter.of(-1); } + if (measurement.status != LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + if (measurement.status != LaserCan.LASERCAN_STATUS_OUT_OF_BOUNDS) { + return Millimeter.of(-2); + } else { + return Millimeter.of(-3); + } + } + return Millimeter.of(measurement.distance_mm); } public Trigger inIntake() { diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 8a8db88f..bdb98cae 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -1,3 +1,4 @@ +// THE MOST IMPORTANT PART OF THE CODE -- IMPORTS package frc.robot.subsystems; import static edu.wpi.first.units.Units.Seconds; @@ -18,6 +19,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -30,19 +32,26 @@ public class ArmPivot extends SubsystemBase { // Presets - private final double ARMPIVOT_KP = 38.5; + private final double ARMPIVOT_KP = 38.5; // previously 50 private final double ARMPIVOT_KI = 0; private final double ARMPIVOT_KD = 0; private final double ARMPIVOT_KS = 0.1; private final double ARMPIVOT_KV = 0.69; private final double ARMPIVOT_KG = 0.18; private final double ARMPIVOT_KA = 0.0; + // Preset positions for Arm with Coral public static final double CORAL_PRESET_L1 = 0; public static final double CORAL_PRESET_L2 = 0.13; public static final double CORAL_PRESET_L3 = 0.13; public static final double CORAL_PRESET_L4 = 0.0; public static final double CORAL_PRESET_PRE_L4 = 1.0 / 16.0; + public static final double CORAL_PRESET_STOWED = 0.125; + public static final double CORAL_PRESET_OUT = 0; + public static final double CORAL_PRESET_UP = 0.25; // Pointing directly upwards + public static final double CORAL_PRESET_DOWN = -0.25; + // Preset positions for Arm with Algae public static final double CORAL_POST_SCORE = -0.15; + public static final double CORAL_QUICK_INTAKE = -0.07; public static final double ALGAE_REMOVE_PREPOS = 0; public static final double ALGAE_REMOVE = 0; public static final double ALGAE_FLING = -0.08; @@ -50,11 +59,9 @@ public class ArmPivot extends SubsystemBase { public static final double ALGAE_PROCESSOR_SCORE = -0.05; public static final double ALGAE_GROUND_INTAKE = -0.085; public static final double ALGAE_NET_SCORE = 0.175; // untested - old value was 0.18 - public static final double CORAL_PRESET_STOWED = 0.125; - public static final double CORAL_PRESET_OUT = 0; - public static final double CORAL_PRESET_UP = 0.245; // Stop a little short of the hardstop + + // Other Presets public static final double CORAL_PRESET_GROUND_INTAKE = 0; - public static final double CORAL_PRESET_DOWN = -0.25; public static final double HARDSTOP_HIGH = 0.32; public static final double HARDSTOP_LOW = -0.26; public static final double POS_TOLERANCE = Units.degreesToRotations(5); @@ -68,7 +75,7 @@ public class ArmPivot extends SubsystemBase { // TalonFX private final TalonFX motor; - // alerts + // alerts if motor is not connected. private final Alert NotConnectedError = new Alert("ArmPivot", "Motor not connected", AlertType.kError); private final Debouncer notConnectedDebouncer = new Debouncer(.1, DebounceType.kBoth); @@ -77,6 +84,7 @@ public class ArmPivot extends SubsystemBase { private double targetPos; + // Arm Pivot Contructor public ArmPivot() { motor = new TalonFX(Hardware.ARM_PIVOT_MOTOR_ID); routine = @@ -94,15 +102,21 @@ public ArmPivot() { logTabs(); } - // commands + // ***ALL COMMANDS*** // + + // SysID quasistatic test, tests mechanism at a constant speed public Command SysIDQuasistatic(Direction direction) { return routine.quasistatic(direction); } + // SysID dynamic test, tests mechanism at a linear growing speed public Command SysIDDynamic(Direction direction) { return routine.dynamic(direction); } + /* Sets the desired position to move the motor to + - one time command using motor control + */ private Command setTargetPosition(double pos) { return runOnce( () -> { @@ -111,20 +125,31 @@ private Command setTargetPosition(double pos) { }); } + // Boolean returning whether or not the arm is at the desired position public boolean atPosition(double position) { return MathUtil.isNear(position, getCurrentPosition(), POS_TOLERANCE); } + // Returns the current target position (position the arm is to move to) private double getTargetPosition() { return targetPos; } + /* Returns the current position of the arm in a double measuring degrees + - get the current positions and conversts it to a double (number with decimal) + */ private double getCurrentPosition() { + if (RobotBase.isSimulation()) { + return targetPos; + } var curPos = motor.getPosition(); return curPos.getValueAsDouble(); } - // preset command placeholder + /* moves arm to the input position + - sets the target position to the inputted position + - shrinks the difference between the current position and the target position until they are close enough to work + */ public Command moveToPosition(double position) { return setTargetPosition(position).andThen(Commands.waitUntil(atAngle(position))); } @@ -133,12 +158,16 @@ public Trigger atAngle(double position) { return new Trigger(() -> Math.abs(getCurrentPosition() - position) < POS_TOLERANCE); } - // (+) is to move arm up, and (-) is down + // (+) is to move arm up, and (-) is down. sets a voltage to pass to motor to move public Command startMovingVoltage(Supplier speedControl) { return runEnd(() -> motor.setVoltage(speedControl.get().in(Volts)), () -> motor.stopMotor()); } - // logging + /* logging + * - working on logging information to shufflboard + * - "getTab" indicates what tab it would like to edit + * - each command adds a new trackable value with a name found in quotations marks + */ public void logTabs() { Shuffleboard.getTab("Arm Pivot") .addDouble("Pivot Speed", () -> motor.getVelocity().getValueAsDouble()); @@ -154,18 +183,21 @@ public void logTabs() { public void factoryDefaults() { TalonFXConfigurator cfg = motor.getConfigurator(); var talonFXConfiguration = new TalonFXConfiguration(); - + // specifies what the sensor is, what port its on, and what the gearing ratio for the sensor is + // relative to the motor talonFXConfiguration.Feedback.FeedbackRemoteSensorID = Hardware.ARM_PIVOT_CANDI_ID; talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANdiPWM1; talonFXConfiguration.Feedback.RotorToSensorRatio = 1 / ARM_RATIO; + // Inverting motor output direction talonFXConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + // Setting the motor to brake when not moving talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 20; // starting low for testing + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 20; // previously 40 talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 10; // starting low for testing + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 10; // previously 20 talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID @@ -181,11 +213,11 @@ public void factoryDefaults() { // set Motion Magic settings in rps not mechanism units talonFXConfiguration.MotionMagic.MotionMagicCruiseVelocity = - 160; // Target cruise velocity of 2560 rps + 320; // 160 // Target cruise velocity of 2560 rps talonFXConfiguration.MotionMagic.MotionMagicAcceleration = 320; // Target acceleration of 4960 rps/s (0.5 seconds) talonFXConfiguration.MotionMagic.MotionMagicJerk = - 1600; // Target jerk of 6400 rps/s/s (0.1 seconds) + 3200; // 1600 // Target jerk of 6400 rps/s/s (0.1 seconds) cfg.apply(talonFXConfiguration); } @@ -193,6 +225,9 @@ public void factoryDefaults() { // alert @Override public void periodic() { + // Error that ensures the motor is connected NotConnectedError.set(notConnectedDebouncer.calculate(!motor.getMotorVoltage().hasUpdated())); } } +// -Samuel "Big North" Mallick +// -Cleaned up and completed by Connor :) diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 09a33dda..260e962c 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -21,26 +21,33 @@ import java.util.function.DoubleSupplier; public class ClimbPivot extends SubsystemBase { + // variable that allows you to switch between using two motors or just one private static final boolean DUAL_MOTORS = false; + // the two motor instance variables private final TalonFX motorLeft; private final TalonFX motorRight; + // the three target positions in an enum public enum TargetPositions { STOWED, CLIMB_OUT, CLIMBED; } - private final DigitalInput sensor; + // digitalInput for the sensor (never worked lol) + public final DigitalInput sensor; + + // variable for the shuffleboard tab for consistency private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Climb"); + // various position and speed presets private final double STOWED_MAX_PRESET = -0.447; private final double STOWED_MIN_PRESET = -0.450; private final double CLIMB_OUT_MAX_PRESET = -0.150; private final double CLIMB_OUT_MIN_PRESET = -0.177; private final double CLIMBED_MAX_PRESET = -0.325; - private final double CLIMBED_MIN_PRESET = -0.333; + private final double CLIMBED_MIN_PRESET = -0.34; private final double FORWARD_SOFT_STOP = -0.07; private final double REVERSE_SOFT_STOP = -78; private final double CLIMB_OUT_SPEED = 1.0; @@ -49,25 +56,40 @@ public enum TargetPositions { private final double CLIMB_HOLD_CLIMBED = -0.0705; private final double CLIMB_IN_SPEED = -0.75; - // relative to eachother, likely not accurately zero'ed when obtained.x + private final double climbInKp = 50; + private final double climbOutKp = 50; + + // positions for relation between motor encoder and WCP encoder + // relative to eachother, likely not accurately zero'ed when obtained. private static final double MIN_ROTOR_POSITION = -50.45; private static final double MAX_ROTOR_POSITION = 14.456; private static final double MIN_ENCODER_POSITION = 0.611; private static final double MAX_ENCODER_POSITION = 0.915; - private boolean isClimbOut = false; + // two status variables + public boolean isClimbOut = false; private boolean isStowed = true; + // setting the starting target position private TargetPositions selectedPos = TargetPositions.STOWED; private double maxTargetPos = STOWED_MAX_PRESET; private double minTargetPos = STOWED_MIN_PRESET; private double holdSpeed = CLIMB_HOLD_STOWED; + + // if moveComplete is true it wont move regardless of if its in range. This is to ensure that + // when disabled, when re-enabled it doesnt start moving. private boolean moveComplete = true; + + // inTolerenace is used to make sure that we're within a ok range between our min goal and max + // goal private boolean inTolerance = true; + // the target speed private double setSpeed = 0; + private double speedToSet = 0; + private double pError = 0; - // alerts + // alerts for checking if either motor is or isnt connected private final Alert NotConnectedErrorOne = new Alert("Climb", "Motor 1 not connected", AlertType.kError); private final Alert NotConnectedErrorTwo = @@ -75,32 +97,49 @@ public enum TargetPositions { private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth); private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); + // here we go public ClimbPivot() { motorLeft = new TalonFX(Hardware.CLIMB_PIVOT_MOTOR_LEFT_ID); + // checking to instantiate the 2nd motor if two are installed. If only one is installed, the 2nd + // one is set to null. if (DUAL_MOTORS) { motorRight = new TalonFX(Hardware.CLIMB_PIVOT_MOTOR_RIGHT_ID); } else { motorRight = null; } + + // the failed climb sensor sensor = new DigitalInput(Hardware.CLIMB_SENSOR); + + // motor configuration and shuffleboard logging methods configure(); setupLogging(); + + // if motor 2 exists, setting its contorl mode as a follower to the 1st motor through CTRE if (motorRight != null) { motorRight.setControl(new Follower(motorLeft.getDeviceID(), true)); } } + // method for configuring the two motors private void configure() { + + // ctre uses configurator classes to configure the motors var talonFXConfigurator = motorLeft.getConfigurator(); + + // making the 2nd configurator null, unless the 2nd motor actually exist TalonFXConfigurator talonFXConfigurator2 = null; if (motorRight != null) { talonFXConfigurator2 = motorRight.getConfigurator(); } - + // one configuration to apply to both motors with the values that apply to both of them TalonFXConfiguration configuration = new TalonFXConfiguration(); + // remote sensor values for the WCP encoder configuration.Feedback.FeedbackRemoteSensorID = Hardware.CLIMB_PIVOT_CANCODER_ID; configuration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + + // previously mentioned ratio calculation configuration.Feedback.RotorToSensorRatio = (MAX_ROTOR_POSITION - MIN_ROTOR_POSITION) / (MAX_ENCODER_POSITION - MIN_ENCODER_POSITION); // Set and enable current limit @@ -110,10 +149,16 @@ private void configure() { configuration.CurrentLimits.SupplyCurrentLimitEnable = true; // Enable brake mode configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + // applying the configuration if the 2nd motor exists + // this was applied here before the rest of the config so the following configs arent applied if (motorRight != null) { talonFXConfigurator2.apply(configuration); } + // software limits employed after the 2nd motor configs were applied so it doesnt get applied to + // the 2nd motor + // having softlimits enabled on both motors isnt great configuration.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; configuration.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; configuration.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_STOP; @@ -123,19 +168,25 @@ private void configure() { // on opposite side } + // command to stop the motor public Command stopMotor() { return runOnce(() -> motorLeft.stopMotor()); } + // method to set the target position based on what target position the enum parameter is private void setTargetPos(TargetPositions newTargetPos) { switch (newTargetPos) { case STOWED -> { + // if stowed, it sets the pos its going to as stowed, and sets the target positions + // also makes move complete false to indicate it will move selectedPos = TargetPositions.STOWED; maxTargetPos = STOWED_MAX_PRESET; minTargetPos = STOWED_MIN_PRESET; moveComplete = false; } case CLIMB_OUT -> { + // if climb_out, it sets the pos its going to as climb_out, and sets the target positions + // also makes move complete false to indicate it will move selectedPos = TargetPositions.CLIMB_OUT; maxTargetPos = CLIMB_OUT_MAX_PRESET; minTargetPos = CLIMB_OUT_MIN_PRESET; @@ -143,6 +194,8 @@ private void setTargetPos(TargetPositions newTargetPos) { moveComplete = false; } case CLIMBED -> { + // if climbed, it sets the pos its going to as climbed, and sets the target positions + // also makes move complete false to indicate it will move selectedPos = TargetPositions.CLIMBED; maxTargetPos = CLIMBED_MAX_PRESET; minTargetPos = CLIMBED_MIN_PRESET; @@ -152,9 +205,11 @@ private void setTargetPos(TargetPositions newTargetPos) { } } + // method for setting the climb target depending on what the selectedPosition is public Command advanceClimbTarget() { return runOnce( () -> { + // depending on the switch itll run setTargetPos to a different position except stowed switch (selectedPos) { case STOWED -> { setTargetPos(TargetPositions.CLIMB_OUT); @@ -171,32 +226,43 @@ public Command advanceClimbTarget() { .withName("Climb Sequence"); } + // check the digital input climb sensor public boolean checkClimbSensor() { - return sensor.get(); + return !sensor.get(); } + // get the velocity from the motor public double getClimbVelocity() { return motorLeft.getVelocity().getValueAsDouble(); } + // get the position from the encoder (I think?) public double getClimbPosition() { return motorLeft.getPosition().getValueAsDouble(); } + // set what position its at (if you need to zero it) public void setPosition(double pos) { motorLeft.setPosition(pos); } + // manual climb movement public Command moveClimbManual(DoubleSupplier amount) { + // runs the command, and then run the next method when it ends return runEnd( () -> { + // set speed to the input amount setSpeed = amount.getAsDouble(); + // applies speed motorLeft.set(setSpeed); + // spam console (should've removed this...) System.out.println(setSpeed); }, + // when the command ends, stop the motors () -> motorLeft.stopMotor()); } + // basic logging public void setupLogging() { shuffleboardTab .addBoolean("Is Climb OUT?", () -> isClimbOut) @@ -204,6 +270,8 @@ public void setupLogging() { shuffleboardTab .addBoolean("Is Climb STOWED?", () -> isStowed) .withWidget(BuiltInWidgets.kBooleanBox); + + // structured similarly to setTargetPos, mimics that to log what state its in shuffleboardTab.addString( "Where Move next?", () -> { @@ -222,6 +290,8 @@ public void setupLogging() { } } }); + + // structured similarly to setTargetPos, mimics that to log what state its in shuffleboardTab.addDouble("targetPos", () -> maxTargetPos); shuffleboardTab .addString( @@ -257,8 +327,10 @@ public void setupLogging() { @Override public void periodic() { + // gets the current position at the time of that periodic rerun double currentPos = getClimbPosition(); + // checks what state its in position wise and returns if its in that state if (CLIMB_OUT_MIN_PRESET <= currentPos && currentPos <= CLIMB_OUT_MAX_PRESET) { isClimbOut = true; } else { @@ -269,12 +341,15 @@ public void periodic() { } else { isStowed = false; } + // checks if its in tolerance. if its in tolerance it ends the movement. otherwise, keep going if (minTargetPos <= currentPos && currentPos <= maxTargetPos) { inTolerance = true; moveComplete = true; } else { inTolerance = false; } + + // errors checking if the motor lost connection NotConnectedErrorOne.set( notConnectedDebouncerOne.calculate(!motorLeft.getMotorVoltage().hasUpdated())); if (DUAL_MOTORS) { @@ -283,6 +358,7 @@ public void periodic() { } } + // coasts the motors by changing their neutralmode public Command coastMotors() { return startEnd( () -> { @@ -301,6 +377,7 @@ public Command coastMotors() { .withName("Coast Climb"); } + // brakes the motors the same way as the previous method public void brakeMotors() { motorLeft.setNeutralMode(NeutralModeValue.Brake); if (DUAL_MOTORS) { @@ -308,6 +385,8 @@ public void brakeMotors() { } } + // checks if its in tolerance, then stops the movement. otherwise, if the target position is ahead + // of it move forward, if its behind it, move back public Command advanceClimbCheck() { return run( () -> { @@ -316,40 +395,55 @@ public Command advanceClimbCheck() { setSpeed = 0; } else { if (!moveComplete) { - if (minTargetPos > getClimbPosition()) { - motorLeft.set(CLIMB_OUT_SPEED); - setSpeed = CLIMB_OUT_SPEED; + pError = minTargetPos - getClimbPosition(); + if (pError > 0) { + speedToSet = CLIMB_OUT_SPEED * pError * climbOutKp; + motorLeft.set(speedToSet); + setSpeed = speedToSet; } else { - motorLeft.set(CLIMB_IN_SPEED); - setSpeed = CLIMB_IN_SPEED; + speedToSet = CLIMB_IN_SPEED * -pError * climbInKp; + motorLeft.set(speedToSet); + setSpeed = speedToSet; } } } }); } + // a trigger for checking if we're climbing public Trigger isClimbing() { return new Trigger(() -> !moveComplete); } + // a value that just sets moveComplete to true and stops the motors public void moveCompleteTrue() { moveComplete = true; motorLeft.stopMotor(); } + // sets move complete to false public void moveCompleteFalse() { moveComplete = false; } + // sets the target position to go to stowed positions public Command toStow() { return runOnce(() -> setTargetPos(TargetPositions.STOWED)).withName("to stow climb"); } + // sets the target position to go to climb_out position public Command toClimbOut() { return runOnce(() -> setTargetPos(TargetPositions.CLIMB_OUT)).withName("to climb out"); } + // sets the target position to go to climbed position public Command toClimbed() { return runOnce(() -> setTargetPos(TargetPositions.CLIMBED)).withName("to climbed"); } + + public Trigger cageDetected() { + return new Trigger(() -> checkClimbSensor()) + .debounce(0.1, DebounceType.kRising) + .debounce(0.05, DebounceType.kFalling); + } } diff --git a/src/main/java/frc/robot/subsystems/DrivebaseWrapper.java b/src/main/java/frc/robot/subsystems/DrivebaseWrapper.java index 11f91095..397043fe 100644 --- a/src/main/java/frc/robot/subsystems/DrivebaseWrapper.java +++ b/src/main/java/frc/robot/subsystems/DrivebaseWrapper.java @@ -13,6 +13,7 @@ import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import java.util.function.Supplier; +/// this is for vision better handling without a drive base and with one /** * Wrapper class around members of {@link SwerveDrive}, like the Field2d telemetry and the pose * estimator. diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index b72806cd..371953a9 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -36,7 +36,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_LEVEL_THREE_POS = 14; public static final double CORAL_LEVEL_TWO_PRE_POS = 6.94; public static final double CORAL_LEVEL_TWO_POS = 4.4; - public static final double CORAL_LEVEL_ONE_POS = 8.3; + public static final double CORAL_LEVEL_ONE_POS = 4.2; public static final double ALGAE_LEVEL_TWO_THREE = 11; public static final double ALGAE_LEVEL_TWO_THREE_FLING = 16; public static final double ALGAE_LEVEL_THREE_FOUR = 21; @@ -45,10 +45,11 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double ALGAE_PROCESSOR_SCORE = 2; public static final double ALGAE_NET_SCORE = 38; // untested public static final double ALGAE_GROUND_INTAKE = 0.05; - public static final double CORAL_STOWED = 3.9; + public static final double CORAL_STOWED = CORAL_LEVEL_TWO_PRE_POS; public static final double CORAL_GROUND_INTAKE_POS = 7.2; public static final double CORAL_INTAKE_POS = 1.55; - public static final double CORAL_PRE_INTAKE = 4.0; + public static final double CORAL_PRE_INTAKE = 4.7; + public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; public static final double MANUAL = 0.1; @@ -232,6 +233,13 @@ public boolean getHasBeenZeroed() { return hasBeenZeroed; } + public boolean getPositionSubZero() { + if (curPos < -0.1 && hasBeenZeroed) { + return true; + } + return false; + } + private double getTargetPosition() { return targetPos; } diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index a6513f26..56f1d3e1 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -13,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Hardware; public class GroundArm extends SubsystemBase { @@ -23,16 +25,19 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KV = 4; private final double ARMPIVOT_KG = 0.048; private final double ARMPIVOT_KA = 0; - public static final double STOWED_POSITION = 0.476; + public static final double STOWED_POSITION = 0.46; public static final double UP_POSITION = 0.27; // untested - should be somewhere in between stowed and ground - public static final double GROUND_POSITION = -0.040; + public static final double GROUND_POSITION = -0.050; + public static final double QUICK_INTAKE_POSITION = 0.31; public static final double POS_TOLERANCE = Units.degreesToRotations(5); + public static final double GROUND_HOLD_VOLTAGE = -0.40; // ratio from motor rotations to output rotations private static final double ARM_RATIO = 60; // MotionMagic voltage private final MotionMagicVoltage m_request = new MotionMagicVoltage(0); + private final VoltageOut m_voltageRequest = new VoltageOut(0); // TalonFX private final TalonFX motor; @@ -113,9 +118,16 @@ public void logTabs() { // preset command placeholder public Command moveToPosition(double position) { - return setTargetPosition(position) - .andThen( - Commands.waitUntil(() -> Math.abs(getCurrentPosition() - position) < POS_TOLERANCE)); + return setTargetPosition(position).andThen(Commands.waitUntil(atPosition(position))); + } + + public Command setVoltage(double voltage) { + return runOnce(() -> motor.setControl(m_voltageRequest.withOutput(voltage))) + .withName("Set voltage " + voltage); + } + + public Trigger atPosition(double position) { + return new Trigger(() -> Math.abs(getCurrentPosition() - position) < POS_TOLERANCE); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index edf79492..9398d571 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -8,11 +8,15 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Hardware; public class GroundSpinny extends SubsystemBase { public static final double GROUND_INTAKE_SPEED = -8; + public static final double GROUND_INTAKE_JITTER_SPEED = 1; public static final double FUNNEL_INTAKE_SPEED = -2; + public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1; + private static final double STATOR_CURRENT_STALL_THRESHOLD = 50; // TalonFX private final TalonFX motor; @@ -44,9 +48,9 @@ public void configMotors() { configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.apply(configuration); // enabling stator current limits - currentLimits.StatorCurrentLimit = 10; // subject to change + currentLimits.StatorCurrentLimit = 100; // subject to change currentLimits.StatorCurrentLimitEnable = true; - currentLimits.SupplyCurrentLimit = 10; // subject to change + currentLimits.SupplyCurrentLimit = 20; // subject to change currentLimits.SupplyCurrentLimitEnable = true; cfg.apply(currentLimits); } @@ -76,6 +80,11 @@ public Command holdFunnelIntakePower() { return holdPower(FUNNEL_INTAKE_SPEED).withName("hold funnel intake power"); } + public void imperativeSetGroundIntakePower() { + motor.setVoltage(GROUND_INTAKE_SPEED); + lastSetPower = GROUND_INTAKE_SPEED; + } + public Command setGroundIntakePower() { return setPower(GROUND_INTAKE_SPEED).withName("set ground intake power"); } @@ -84,6 +93,27 @@ public Command holdGroundIntakePower() { return holdPower(GROUND_INTAKE_SPEED).withName("hold ground intake power"); } + public Command setQuickHandoffExtakeSpeed() { + return setPower(QUICK_HANDOFF_EXTAKE_SPEED).withName("set quick handoff extake power"); + } + + public Command holdQuickHandoffExtakeSpeed() { + return holdPower(QUICK_HANDOFF_EXTAKE_SPEED).withName("hold quick handoff extake power"); + } + + public Command setGroundIntakeJitterSpeed() { + return setPower(GROUND_INTAKE_JITTER_SPEED).withName("set ground intake jitter power"); + } + + public Command holdGroundIntakeJitterSpeed() { + return holdPower(GROUND_INTAKE_JITTER_SPEED).withName("hold ground intake jitter power"); + } + + public Trigger stalling() { + return new Trigger( + () -> motor.getStatorCurrent().getValueAsDouble() > STATOR_CURRENT_STALL_THRESHOLD); + } + public Command stop() { return runOnce(() -> motor.stopMotor()).withName("Ground spinny stop"); } diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index 2f14b896..e8c15b5f 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -24,7 +24,7 @@ public class SpinnyClaw extends SubsystemBase { public static final double CORAL_INTAKE_SPEED = -6; public static final double CORAL_EXTAKE_SPEED = 5; public static final double CORAL_REJECT_SPEED = 1; - public static final double CORAL_L1_EXTAKE_SPEED = 2.5; + public static final double CORAL_L1_EXTAKE_SPEED = 1.7; public static final double ALGAE_INTAKE_SPEED = -4; // started at -3.5 public static final double ALGAE_GRIP_INTAKE_SPEED = -3; // started at -2.5 public static final double ALGAE_EXTAKE_SPEED = 14; diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index a0ce5022..43e8f322 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -12,10 +12,12 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -36,6 +38,7 @@ * station (+X is forward from blue driver station, +Y is left, +Z is up). */ public class VisionSubsystem extends SubsystemBase { + // differences from center robot camera poses private static final double CAMERA_X_POS_METERS_LEFT = 0.26; private static final double CAMERA_X_POS_METERS_RIGHT = 0.27; private static final double CAMERA_Y_POS_METERS_LEFT = 0.25; @@ -48,7 +51,7 @@ public class VisionSubsystem extends SubsystemBase { private static final double CAMERA_PITCH_RIGHT = Units.degreesToRadians(-8.3); private static final double CAMERA_YAW_LEFT = Units.degreesToRadians(-44.64); private static final double CAMERA_YAW_RIGHT = Units.degreesToRadians(46.42); - + // left camera diffrences from center robot public static final Transform3d ROBOT_TO_CAM_LEFT = new Transform3d( // Translation3d.kZero, @@ -57,7 +60,7 @@ public class VisionSubsystem extends SubsystemBase { CAMERA_Z_POS_METERS_LEFT, // Rotation3d.kZero); new Rotation3d(CAMERA_ROLL_LEFT, CAMERA_PITCH_LEFT, CAMERA_YAW_LEFT)); - + // right camera diffrences from center robot public static final Transform3d ROBOT_TO_CAM_RIGHT = new Transform3d( // Translation3d.kZero, @@ -67,12 +70,13 @@ public class VisionSubsystem extends SubsystemBase { // Rotation3d.kZero); new Rotation3d(CAMERA_ROLL_RIGHT, CAMERA_PITCH_RIGHT, CAMERA_YAW_RIGHT)); - // TODO Measure these + // Deviations private static final Vector STANDARD_DEVS = VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(20)); private static final Vector DISTANCE_SC_STANDARD_DEVS = VecBuilder.fill(1, 1, Units.degreesToRadians(50)); + // making the cameras, pose estimator, field2d, fieldObject2d, april tags helper objects private final PhotonCamera leftCamera; private final PhotonCamera rightCamera; private final PhotonPoseEstimator photonPoseEstimatorLeftCamera; @@ -84,68 +88,94 @@ public class VisionSubsystem extends SubsystemBase { // These are always set with every pipeline result private PhotonPipelineResult latestResult = null; - // These are only set when there's a valid pose + // These are only set when there's a valid pose & last timestamps, last poses and distance + // creation private double lastTimestampSeconds = 0; private double lastRawTimestampSeconds = 0; private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d()); private double Distance = 0; + // boolean to disable vision + // if you press the button on shuffleboard it disables vision + private final GenericSubscriber disableVision; + + // full field pose for logging private final StructPublisher fieldPose3dEntry = NetworkTableInstance.getDefault() .getStructTopic("vision/fieldPose3d", Pose3d.struct) .publish(); - + // left camera field pose for logging private final StructPublisher rawFieldPose3dEntryLeft = NetworkTableInstance.getDefault() .getStructTopic("vision/rawFieldPose3dLeft", Pose3d.struct) .publish(); - + // right camera field pose for logging private final StructPublisher rawFieldPose3dEntryRight = NetworkTableInstance.getDefault() .getStructTopic("vision/rawFieldPose3dRight", Pose3d.struct) .publish(); - + // field map for bot to april tag (configured for 2025) private static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + // method creation for subsystem init in subsystems.java public VisionSubsystem(DrivebaseWrapper aprilTagsHelper) { + // inits for field robotField = new Field2d(); SmartDashboard.putData(robotField); this.aprilTagsHelper = aprilTagsHelper; rawVisionFieldObject = robotField.getObject("RawVision"); + // cameras init hardware wise leftCamera = new PhotonCamera(Hardware.LEFT_CAM); rightCamera = new PhotonCamera(Hardware.RIGHT_CAM); + // pose estimator inits for cameras with full field, multi-tag april tag detection and camera + // differences from center robot + // pose estimator is used to estimate the robot's position on the field based on the cameras photonPoseEstimatorLeftCamera = new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM_LEFT); photonPoseEstimatorRightCamera = new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM_RIGHT); - + // vision shuffle board tab creation ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("AprilTags"); + // last raw timestamp shuffleboard entry shuffleboardTab .addDouble("Last raw timestamp", this::getLastRawTimestampSeconds) .withPosition(0, 0) .withSize(1, 1); + // number of april tags detected shuffleboard entry shuffleboardTab .addInteger("Num targets", this::getNumTargets) .withPosition(0, 1) .withSize(1, 1); + // last timestamp read shuffleboard entry shuffleboardTab .addDouble("Last timestamp", this::getLastTimestampSeconds) .withPosition(1, 0) .withSize(1, 1); + // closest tag distance in meters shuffleboard entry shuffleboardTab .addDouble("april tag distance meters", this::getDistanceToTarget) .withPosition(1, 1) .withSize(1, 1); + // time since last reading a tag shuffleboard entry shuffleboardTab .addDouble("time since last reading", this::getTimeSinceLastReading) .withPosition(2, 0) .withSize(1, 1); + // disable vision button if press button you disable vision in shuffleboard + disableVision = + shuffleboardTab + .add("Disable vision", false) + .withPosition(4, 0) + .withSize(3, 2) + .withWidget(BuiltInWidgets.kToggleButton) + .getEntry(); } + // method called to update results that cameras detect public void update() { for (PhotonPipelineResult result : leftCamera.getAllUnreadResults()) { process(result, photonPoseEstimatorLeftCamera, rawFieldPose3dEntryLeft); @@ -155,37 +185,59 @@ public void update() { } } + // processs current result with cameras and camera pose estimator and where swerve believes it is private void process( PhotonPipelineResult result, PhotonPoseEstimator estimator, StructPublisher rawFieldPose3dEntry) { + // makes a different timestamp to keep track of time var RawTimestampSeconds = result.getTimestampSeconds(); + // for waiting until orange pi 5 syncing and getting rid old results if (!MathUtil.isNear(Timer.getFPGATimestamp(), RawTimestampSeconds, 5.0)) { return; } + // updates estimated pose var estimatedPose = estimator.update(result); + // if there is an actual pose then it gets a timestamp and pose3d if (estimatedPose.isPresent()) { var TimestampSeconds = estimatedPose.get().timestampSeconds; var FieldPose3d = estimatedPose.get().estimatedPose; + // sets full pose3d for logging rawFieldPose3dEntry.set(FieldPose3d); + // if you disabled vision then vision doesn't do anything anymore + if (disableVision.getBoolean(false)) { + return; + } + //////// below is if a tag is a specific tag id don't read this result // if (BadAprilTagDetector(result)) { // return; // } + // tolerances for rotation if its outside of tolerances then return null if (!MathUtil.isNear(0, FieldPose3d.getZ(), 0.10) || !MathUtil.isNear(0, FieldPose3d.getRotation().getX(), Units.degreesToRadians(8)) || !MathUtil.isNear(0, FieldPose3d.getRotation().getY(), Units.degreesToRadians(8))) { return; } + // makes a field pose for logging var FieldPose = FieldPose3d.toPose2d(); + // gets distance var Distance = PhotonUtils.getDistanceToPose( FieldPose, + // gets closed tag and gets distance fieldLayout.getTagPose(result.getBestTarget().getFiducialId()).get().toPose2d()); + // makes a pose that vision sees aprilTagsHelper.addVisionMeasurement( + // field pose FieldPose, + // timestamp TimestampSeconds, + // start with STANDARD_DEVS, and for every meter of distance past 1 meter, add another + // DISTANCE_SC_STANDARD_DEVS to the standard devs DISTANCE_SC_STANDARD_DEVS.times(Math.max(0, Distance - 1)).plus(STANDARD_DEVS)); + // sets estimated current pose to estimated vision pose robotField.setRobotPose(aprilTagsHelper.getEstimatedPosition()); + // updates shuffleboard values if (RawTimestampSeconds > lastRawTimestampSeconds) { fieldPose3dEntry.set(FieldPose3d); lastRawTimestampSeconds = RawTimestampSeconds; @@ -198,31 +250,36 @@ private void process( } public int getNumTargets() { + // if latestResult == null then return -1 if not gets all the targets return latestResult == null ? -1 : latestResult.getTargets().size(); } - /** - * Returns the last time we saw an AprilTag. - * - * @return The time we last saw an AprilTag in seconds since FPGA startup. - */ + // returns lastTimestampSeconds public double getLastTimestampSeconds() { return lastTimestampSeconds; } + // returns lastRawTimestampSeconds public double getLastRawTimestampSeconds() { return lastRawTimestampSeconds; } + /** + * Returns the last time we saw an AprilTag. + * + * @return The time we last saw an AprilTag in seconds since FPGA startup. + */ public double getTimeSinceLastReading() { return Timer.getFPGATimestamp() - lastTimestampSeconds; } + // gets Distance times by 1000 which is in milimeters then gets rid of all the decimals then + // divides to get it in meters with 4 decimal places to meters and then converts it to double public double getDistanceToTarget() { return (double) Math.round(Distance * 1000) / 1000; } - // configured for 2025 reefscape + // configured for 2025 reefscape to filter out any tag besides the reef tags depending on allaince private static boolean BadAprilTagDetector(PhotonPipelineResult r) { boolean isRed = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Red)); boolean isBlue = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Blue)); diff --git a/src/main/java/frc/robot/subsystems/auto/AlgaeAlign.java b/src/main/java/frc/robot/subsystems/auto/AlgaeAlign.java new file mode 100644 index 00000000..fc1f188d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/AlgaeAlign.java @@ -0,0 +1,147 @@ +package frc.robot.subsystems.auto; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.MathUtil; +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.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controls; +import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import java.util.List; + +public class AlgaeAlign { + public static Command algaeAlign(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AlgaeAlignCommand(drivebaseSubsystem, controls).withName("Algae Align"); + } + + private static class AlgaeAlignCommand extends Command { + private static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + private static Transform2d robotOffset = + new Transform2d(Units.inchesToMeters(36.5 / 2), 0, Rotation2d.k180deg); + private static final Pose2d blueAlgaeAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(robotOffset); + + private static final Pose2d blueAlgaeCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(robotOffset); + + private static final Pose2d blueAlgaeEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(robotOffset); + + private static final Pose2d blueAlgaeGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(robotOffset); + + private static final Pose2d blueAlgaeIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(robotOffset); + + private static final Pose2d blueAlgaeKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(robotOffset); + + private static final Pose2d redAlgaeAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(robotOffset); + + private static final Pose2d redAlgaeCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(robotOffset); + + private static final Pose2d redAlgaeEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(robotOffset); + + private static final Pose2d redAlgaeGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(robotOffset); + + private static final Pose2d redAlgaeIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(robotOffset); + private static final Pose2d redAlgaeKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(robotOffset); + + private static final List blueAlgaePoses = + List.of(blueAlgaeAB, blueAlgaeCD, blueAlgaeEF, blueAlgaeGH, blueAlgaeIJ, blueAlgaeKL); + + private static final List redAlgaePoses = + List.of(redAlgaeAB, redAlgaeCD, redAlgaeEF, redAlgaeGH, redAlgaeIJ, redAlgaeKL); + + public static Pose2d getNearestAlgae(Pose2d p, boolean isBlue) { + List algaePose2ds = isBlue ? blueAlgaePoses : redAlgaePoses; + return p.nearest(algaePose2ds); + } + + private final PIDController pidX = new PIDController(4, 0, 0); + private final PIDController pidY = new PIDController(4, 0, 0); + private final PIDController pidRotate = new PIDController(8, 0, 0); + + private final CommandSwerveDrivetrain drive; + private final Controls controls; + private Pose2d algaePose; + + private final SwerveRequest.FieldCentric driveRequest = + new SwerveRequest.FieldCentric() // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); + + public AlgaeAlignCommand(CommandSwerveDrivetrain drive, Controls controls) { + this.drive = drive; + pidRotate.enableContinuousInput(-Math.PI, Math.PI); + this.controls = controls; + setName("Algae Align"); + } + + @Override + public void initialize() { + boolean redAlliance = DriverStation.getAlliance().get() == Alliance.Red; + Pose2d robotPose = drive.getState().Pose; + algaePose = getNearestAlgae(robotPose, !redAlliance); + pidX.setSetpoint(algaePose.getX()); + pidY.setSetpoint(algaePose.getY()); + pidRotate.setSetpoint(algaePose.getRotation().getRadians()); + } + + @Override + public void execute() { + Pose2d currentPose = drive.getState().Pose; + // Calculate the power for X direction and clamp it between -1 and 1 + double powerX = pidX.calculate(currentPose.getX()); + double powerY = pidY.calculate(currentPose.getY()); + powerX = MathUtil.clamp(powerX, -2, 2); + powerY = MathUtil.clamp(powerY, -2, 2); + powerX += .05 * Math.signum(powerX); + powerY += .05 * Math.signum(powerY); + double powerRotate = pidRotate.calculate(currentPose.getRotation().getRadians()); + powerRotate = MathUtil.clamp(powerRotate, -4, 4); + SwerveRequest request = + driveRequest.withVelocityX(powerX).withVelocityY(powerY).withRotationalRate(powerRotate); + // Set the drive control with the created request + drive.setControl(request); + } + + @Override + public boolean isFinished() { + Pose2d currentPose = drive.getState().Pose; + Transform2d robotToAlgae = algaePose.minus(currentPose); + if (robotToAlgae.getTranslation().getNorm() < 0.01 + && Math.abs(robotToAlgae.getRotation().getDegrees()) < 1) { + controls.vibrateDriveController(0.5); + return true; + } + return false; + } + + @Override + public void end(boolean interrupted) { + // Create a swerve request to stop all motion by setting velocities and rotational rate to 0 + SwerveRequest stop = driveRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); + // Set the drive control with the stop request to halt all movement + drive.setControl(stop); + controls.vibrateDriveController(0); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java new file mode 100644 index 00000000..4592ed7d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -0,0 +1,97 @@ +package frc.robot.subsystems.auto; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controls; +import frc.robot.subsystems.SuperStructure; +import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; +import frc.robot.util.ScoringMode; +import java.util.List; + +/** + * Command that automatically moves the elevator to the proper height for the nearest algae based on + * alliance and current robot pose. + */ +public class AutoAlgaeHeights extends Command { + + private final CommandSwerveDrivetrain drivebase; + private final SuperStructure s; + private final Controls c; + + private static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + private static final List blueAlgaePoses = + List.of( + aprilTagFieldLayout.getTagPose(17).get().toPose2d(), + aprilTagFieldLayout.getTagPose(19).get().toPose2d(), + aprilTagFieldLayout.getTagPose(21).get().toPose2d(), + aprilTagFieldLayout.getTagPose(18).get().toPose2d(), + aprilTagFieldLayout.getTagPose(20).get().toPose2d(), + aprilTagFieldLayout.getTagPose(22).get().toPose2d()); + + private static final List redAlgaePoses = + List.of( + aprilTagFieldLayout.getTagPose(6).get().toPose2d(), + aprilTagFieldLayout.getTagPose(8).get().toPose2d(), + aprilTagFieldLayout.getTagPose(10).get().toPose2d(), + aprilTagFieldLayout.getTagPose(7).get().toPose2d(), + aprilTagFieldLayout.getTagPose(9).get().toPose2d(), + aprilTagFieldLayout.getTagPose(11).get().toPose2d()); + + public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) { + this.drivebase = drivebase; + this.s = s; + this.c = c; + } + + public static Command autoAlgaeIntakeCommand( + CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) { + return new AutoAlgaeHeights(drivebase, s, c); + } + + private static Pose2d getNearestAlgae(Pose2d robotPose) { + List poses = AllianceUtils.isBlue() ? blueAlgaePoses : redAlgaePoses; + return robotPose.nearest(poses); + } + + private String aprilTagToAlgaeHeight(Pose2d algaePose) { + // Map poses to two heights only + List poses = AllianceUtils.isBlue() ? blueAlgaePoses : redAlgaePoses; + int index = poses.indexOf(algaePose); + + if (index >= 3) { + return "top"; // upper reef + } else { + return "low"; // lower reef + } + } + + private String activeCommand = ""; + + @Override + public void execute() { + Pose2d robotPose = drivebase.getState().Pose; + Pose2d nearestAlgae = getNearestAlgae(robotPose); + + String targetHeight = aprilTagToAlgaeHeight(nearestAlgae); + + if (!(activeCommand.equals(targetHeight))) { + if (targetHeight.equals("top")) { + s.algaeLevelThreeFourIntake().schedule(); + activeCommand = "top"; + } else { + s.algaeLevelTwoThreeIntake().schedule(); + activeCommand = "low"; + } + } + } + + @Override + public boolean isFinished() { + return c.intakeMode != ScoringMode.ALGAE; + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 796d83a9..93df784a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -12,30 +12,28 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import java.util.List; public class AutoAlign { - public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); + public enum AlignType { + ALLB, + LEFTB, + RIGHTB, + L1LB, + L1RB } - public static Boolean isBlue() { - boolean isBlue; - - if (!DriverStation.getAlliance().isEmpty()) { - isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); - } else { - isBlue = false; - } - return isBlue; + public static Command autoAlign( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls, AlignType type) { + return new AutoAlignCommand(drivebaseSubsystem, controls, type).withName("Auto Align"); } public static boolean readyToScore() { - return isStationary() && isLevel() && isCloseEnough(); + return isStationary() && isLevel() && isCloseEnough(AlignType.ALLB); } public static boolean isStationary() { @@ -51,140 +49,167 @@ public static boolean isLevel() { && MathUtil.isNear(0, rotation.getY(), Units.degreesToRadians(2)); } - public static boolean isCloseEnough() { + public static boolean isCloseEnough(AlignType type) { var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; - var branchPose = AutoAlignCommand.getNearestBranch(currentPose, isBlue()); + var branchPose = AutoAlignCommand.getTargetPose(currentPose, type); return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } + public static boolean poseInPlace(AlignType type) { + return isStationary() && isCloseEnough(type); + } + public static boolean oneSecondLeft() { // THIS WILL ONLY WORK ON THE REAL FIELD AND IN PRACTICE MODE! - return DriverStation.getMatchTime() <= 1; } + private static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + // left and right offsets from the april tags () + private static final Transform2d leftOfTag = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); + private static final Transform2d rightOfTag = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + private static final Transform2d middleOfReef = + new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(26 / 2), Rotation2d.k180deg); + + private static final Pose2d blueBranchA = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); + private static final Pose2d blueBranchB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueBranchC = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(leftOfTag); + private static final Pose2d blueBranchD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueBranchE = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(leftOfTag); + private static final Pose2d blueBranchF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueBranchG = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(leftOfTag); + private static final Pose2d blueBranchH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueBranchI = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(leftOfTag); + private static final Pose2d blueBranchJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(rightOfTag); + private static final Pose2d blueBranchK = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(leftOfTag); + private static final Pose2d blueBranchL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(rightOfTag); + + private static final Pose2d redBranchA = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(leftOfTag); + private static final Pose2d redBranchB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(rightOfTag); + private static final Pose2d redBranchC = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(leftOfTag); + private static final Pose2d redBranchD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(rightOfTag); + private static final Pose2d redBranchE = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(leftOfTag); + private static final Pose2d redBranchF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(rightOfTag); + private static final Pose2d redBranchG = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(leftOfTag); + private static final Pose2d redBranchH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(rightOfTag); + private static final Pose2d redBranchI = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(leftOfTag); + private static final Pose2d redBranchJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(rightOfTag); + private static final Pose2d redBranchK = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(leftOfTag); + private static final Pose2d redBranchL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); + + private static final Pose2d lBlueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(middleOfReef); + + private static final Pose2d rBlueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(l1RightOfReef); + + private static final Pose2d lRedReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(middleOfReef); + + private static final Pose2d rRedReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); + private static class AutoAlignCommand extends Command { - private static final AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - // left and right offsets from the april tags () - private static final Transform2d leftReef = - new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); - private static final Transform2d rightReef = - new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); - - private static final Pose2d blueBranchA = - aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(rightReef); - private static final Pose2d blueBranchB = - aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftReef); - private static final Pose2d blueBranchC = - aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(rightReef); - private static final Pose2d blueBranchD = - aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(leftReef); - private static final Pose2d blueBranchE = - aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(rightReef); - private static final Pose2d blueBranchF = - aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(leftReef); - private static final Pose2d blueBranchG = - aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(rightReef); - private static final Pose2d blueBranchH = - aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(leftReef); - private static final Pose2d blueBranchI = - aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(rightReef); - private static final Pose2d blueBranchJ = - aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(leftReef); - private static final Pose2d blueBranchK = - aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(rightReef); - private static final Pose2d blueBranchL = - aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(leftReef); - - private static final Pose2d redBranchA = - aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(rightReef); - private static final Pose2d redBranchB = - aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(leftReef); - private static final Pose2d redBranchC = - aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(rightReef); - private static final Pose2d redBranchD = - aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(leftReef); - private static final Pose2d redBranchE = - aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(rightReef); - private static final Pose2d redBranchF = - aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(leftReef); - private static final Pose2d redBranchG = - aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(rightReef); - private static final Pose2d redBranchH = - aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(leftReef); - private static final Pose2d redBranchI = - aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(rightReef); - private static final Pose2d redBranchJ = - aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(leftReef); - private static final Pose2d redBranchK = - aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightReef); - private static final Pose2d redBranchL = - aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(leftReef); - - private static final List blueBranchPoses = - List.of( - blueBranchA, - blueBranchB, - blueBranchC, - blueBranchD, - blueBranchE, - blueBranchF, - blueBranchG, - blueBranchH, - blueBranchI, - blueBranchJ, - blueBranchK, - blueBranchL); - ; - private static final List redBranchPoses = - List.of( - redBranchA, - redBranchB, - redBranchC, - redBranchD, - redBranchE, - redBranchF, - redBranchG, - redBranchH, - redBranchI, - redBranchJ, - redBranchK, - redBranchL); - - public static Pose2d getNearestBranch(Pose2d p, boolean isBlue) { - List branchPose2ds = isBlue ? blueBranchPoses : redBranchPoses; - return p.nearest(branchPose2ds); - } - private final PIDController pidX = new PIDController(4, 0, 0); - private final PIDController pidY = new PIDController(4, 0, 0); - private final PIDController pidRotate = new PIDController(8, 0, 0); + protected final PIDController pidX = new PIDController(4, 0, 0); + protected final PIDController pidY = new PIDController(4, 0, 0); + protected final PIDController pidRotate = new PIDController(8, 0, 0); - private final CommandSwerveDrivetrain drive; - private final Controls controls; - private Pose2d branchPose; + protected final CommandSwerveDrivetrain drive; + protected final Controls controls; + protected Pose2d branchPose; + protected AlignType type; private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric() // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls) { + public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls, AlignType type) { this.drive = drive; pidRotate.enableContinuousInput(-Math.PI, Math.PI); this.controls = controls; - setName("Auto Align Two"); + this.type = type; + setName("Auto Align"); } @Override public void initialize() { - boolean redAlliance = DriverStation.getAlliance().get() == Alliance.Red; Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestBranch(robotPose, !redAlliance); + branchPose = getTargetPose(robotPose, type); pidX.setSetpoint(branchPose.getX()); pidY.setSetpoint(branchPose.getY()); pidRotate.setSetpoint(branchPose.getRotation().getRadians()); @@ -228,5 +253,105 @@ public void end(boolean interrupted) { drive.setControl(stop); controls.vibrateDriveController(0); } + + public static Pose2d getTargetPose(Pose2d pose, AlignType type) { + return switch (type) { + case LEFTB -> getNearestLeftBranch(pose); + case RIGHTB -> getNearestRightBranch(pose); + case L1LB -> getNearestL1L(pose); + case L1RB -> getNearestL1R(pose); + case ALLB -> getNearestBranch(pose); + }; + } + + private static Pose2d getNearestBranch(Pose2d p) { + List branchPose2ds = + AllianceUtils.isBlue() + ? List.of( + blueBranchA, + blueBranchB, + blueBranchC, + blueBranchD, + blueBranchE, + blueBranchF, + blueBranchG, + blueBranchH, + blueBranchI, + blueBranchJ, + blueBranchK, + blueBranchL) + : List.of( + redBranchA, + redBranchB, + redBranchC, + redBranchD, + redBranchE, + redBranchF, + redBranchG, + redBranchH, + redBranchI, + redBranchJ, + redBranchK, + redBranchL); + return p.nearest(branchPose2ds); + } + + private static Pose2d getNearestLeftBranch(Pose2d p) { + List branchPose2ds = + AllianceUtils.isBlue() + ? List.of( + blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK) + : List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); + return p.nearest(branchPose2ds); + } + + private static Pose2d getNearestRightBranch(Pose2d p) { + List branchPose2ds = + AllianceUtils.isBlue() + ? List.of( + blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL) + : List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); + return p.nearest(branchPose2ds); + } + + private static Pose2d getNearestL1L(Pose2d p) { + List reefFacesPose2ds = + AllianceUtils.isBlue() + ? List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL) + : List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); + return p.nearest(reefFacesPose2ds); + } + + private static Pose2d getNearestL1R(Pose2d p) { + List reefFacesPose2ds = + AllianceUtils.isBlue() + ? List.of( + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL) + : List.of( + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); + return p.nearest(reefFacesPose2ds); + } } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java index 02f94323..e4eae0da 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java @@ -5,10 +5,10 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.Subsystems; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import java.io.IOException; import org.json.simple.parser.ParseException; @@ -42,12 +42,7 @@ public static void buildAuto(CommandSwerveDrivetrain drivebase) { // Boolean supplier that controls when the path will be mirrored for the red alliance // This will flip the path being followed to the red side of the field. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return !AutoAlign.isBlue(); // Checking alliance is red - } - return false; + return AllianceUtils.isRed(); // Checking alliance is red }, s.drivebaseSubsystem // Reference to this subsystem to set requirements ); diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index 3e88782c..f65e48a9 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.auto; import static frc.robot.Sensors.SensorConstants.ARMSENSOR_ENABLED; +import static frc.robot.Sensors.SensorConstants.INTAKE_SENSOR_ENABLED; import static frc.robot.Subsystems.SubsystemConstants.*; import com.pathplanner.lib.auto.AutoBuilder; @@ -17,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -24,6 +26,7 @@ import frc.robot.Robot; import frc.robot.Subsystems; import java.io.IOException; +import java.util.HashMap; import java.util.List; import java.util.Map; import org.json.simple.parser.ParseException; @@ -33,12 +36,6 @@ public class AutoLogic { public static final Subsystems s = r.subsystems; public static final Controls controls = r.controls; - public static final double FEEDER_DELAY = 0.4; - - // rpm to rev up launcher before launching - public static final double REV_RPM = 2500; - public static final double STAGE_ANGLE = 262; - public static enum StartPosition { FAR_LEFT_CAGE( "Far Left Cage", new Pose2d(7.187, 7.277, new Rotation2d(Units.degreesToRadians(-90)))), @@ -113,6 +110,7 @@ public static enum StartPosition { new AutoPath("MRSF_G-F", "MRSF_G-F"), new AutoPath("MRSF_G-F_WithWait", "MRSF_G-F_WithWait"), new AutoPath("MRSF_G-H", "MRSF_G-H"), + new AutoPath("MLSF_H-K_Cooking", "MLSF_H-K_Cooking"), new AutoPath("MLSF_H-G", "MLSF_H-G")); private static List threePiecePaths = @@ -125,7 +123,9 @@ public static enum StartPosition { new AutoPath("YSMLSF_K-L-A", "YSMLSF_K-L-A"), new AutoPath("YSWLSC_K-L-A", "YSWLSC_K-L-A"), new AutoPath("OSWRSF_D-C-B", "OSWRSF_D-C-B"), - new AutoPath("YSMLSC_K-L-A", "YSMLSC_K-L-A")); + new AutoPath("OSWRSF_E-D-C", "OSWRSF_E-D-C"), + new AutoPath("YSMLSC_K-L-A", "YSMLSC_K-L-A"), + new AutoPath("M_H-GHA-IJA", "M_H-GHA-IJA")); private static List fourPiecePaths = List.of( @@ -146,6 +146,16 @@ public static enum StartPosition { 4, fourPiecePaths); + private static final Map namesToAuto = new HashMap<>(); + + static { + for (List autoPaths : commandsMap.values()) { + for (AutoPath autoPath : autoPaths) { + namesToAuto.put(autoPath.getDisplayName(), autoPath); + } + } + } + // vars // in place of launching command cause launcher doesnt exist @@ -160,8 +170,8 @@ public static enum StartPosition { private static SendableChooser startPositionChooser = new SendableChooser(); - private static DynamicSendableChooser availableAutos = - new DynamicSendableChooser(); + private static DynamicSendableChooser availableAutos = + new DynamicSendableChooser(); private static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); @@ -175,6 +185,10 @@ public static void registerCommands() { NamedCommands.registerCommand("scoreCommand", scoreCommand()); NamedCommands.registerCommand("intake", intakeCommand()); NamedCommands.registerCommand("isCollected", isCollected()); + NamedCommands.registerCommand("readyIntake", readyIntakeCommand()); + NamedCommands.registerCommand("algaeAlign23", algaeCommand23()); + NamedCommands.registerCommand("algaeAlign34", algaeCommand34()); + NamedCommands.registerCommand("net", netCommand()); } // public Command getConditionalCommand(){} @@ -215,7 +229,7 @@ public static void initShuffleBoard() { tab.add("Available Auto Variants", availableAutos).withPosition(4, 2).withSize(2, 1); tab.addBoolean("readyToScore?", () -> AutoAlign.readyToScore()); tab.addBoolean("Level?", () -> AutoAlign.isLevel()); - tab.addBoolean("Close Enough?", () -> AutoAlign.isCloseEnough()); + tab.addBoolean("Close Enough?", () -> AutoAlign.isCloseEnough(AutoAlign.AlignType.ALLB)); tab.addBoolean("Stationary?", () -> AutoAlign.isStationary()); tab.addBoolean("Low on time?", () -> AutoAlign.oneSecondLeft()); tab.addDouble("MATCH TIME(TIMER FOR AUTO)", () -> DriverStation.getMatchTime()); @@ -235,7 +249,7 @@ public static void filterAutos(int numGameObjects) { availableAutos.clearOptions(); // filter based off gameobject count - availableAutos.setDefaultOption(defaultPath.getDisplayName(), defaultPath); + availableAutos.setDefaultOption(defaultPath.getDisplayName(), defaultPath.getDisplayName()); List autoCommandsList = commandsMap.get(numGameObjects); @@ -243,7 +257,7 @@ public static void filterAutos(int numGameObjects) { for (AutoPath auto : autoCommandsList) { if (auto.getStartPose().equals(startPositionChooser.getSelected()) && auto.isVision() == isVision.getSelected()) { - availableAutos.addOption(auto.getDisplayName(), auto); + availableAutos.addOption(auto.getDisplayName(), auto.getDisplayName()); } } } @@ -251,10 +265,7 @@ public static void filterAutos(int numGameObjects) { // get auto public static String getSelectedAutoName() { - if (availableAutos.getSelected() == null) { - return "nullAuto"; - } - return availableAutos.getSelected().getAutoName(); + return availableAutos.getSelectedName(); } public static boolean chooserHasAutoSelected() { @@ -263,7 +274,11 @@ public static boolean chooserHasAutoSelected() { public static Command getSelectedAuto() { double waitTimer = autoDelayEntry.getDouble(0); - String autoName = availableAutos.getSelected().getAutoName(); + AutoPath path = namesToAuto.get(getSelectedAutoName()); + if (path == null) { + path = defaultPath; + } + String autoName = path.getAutoName(); return Commands.waitSeconds(waitTimer) .andThen(AutoBuilder.buildAuto(autoName)) @@ -273,19 +288,56 @@ public static Command getSelectedAuto() { // commands util public static Command scoreCommand() { if (r.superStructure != null) { - return AutoAlign.autoAlign(s.drivebaseSubsystem, controls) - .repeatedly() - .withDeadline(r.superStructure.coralLevelFour(() -> AutoAlign.readyToScore())) - .withName("scoreCommand"); + return new ConditionalCommand( + // If true: + AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) + .repeatedly() + .withDeadline(r.superStructure.coralLevelFour(() -> AutoAlign.readyToScore())) + .withName("scoreCommand"), + // If false: + Commands.none().withName("scoreCommand-empty"), + // Condition: + () -> ARMSENSOR_ENABLED && r.sensors.armSensor.booleanInClaw()); } - return AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + return AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) .withName("scoreCommand-noSuperstructure"); } + public static Command algaeCommand23() { + if (r.superStructure != null) { + return AlgaeAlign.algaeAlign(s.drivebaseSubsystem, controls) + .repeatedly() + .withDeadline(r.superStructure.algaeLevelTwoThreeIntake()) + .withName("algaeCommand23"); + } + return Commands.none().withName("algaeCommand23"); + } + + public static Command algaeCommand34() { + if (r.superStructure != null) { + return AlgaeAlign.algaeAlign(s.drivebaseSubsystem, controls) + .repeatedly() + .withDeadline(r.superStructure.algaeLevelThreeFourIntake()) + .withName("algaeCommand34"); + } + return Commands.none().withName("algaeCommand34"); + } + + public static Command netCommand() { + if (r.superStructure != null) { + return BargeAlign.bargeScore( + s.drivebaseSubsystem, r.superStructure, () -> 0, () -> 0, () -> 0, () -> false) + .withName("net"); + } + return Commands.none().withName("net"); + } + public static Command intakeCommand() { if (r.superStructure != null) { - if (ARMSENSOR_ENABLED) { - return Commands.sequence(r.superStructure.coralPreIntake(), r.superStructure.coralIntake()) + if (ARMSENSOR_ENABLED && INTAKE_SENSOR_ENABLED) { + return Commands.waitUntil(r.sensors.intakeSensor.inIntake()) + .withTimeout(0.5) + .andThen(r.superStructure.autoCoralIntake()) .withName("intake"); } } @@ -300,4 +352,12 @@ public static Command isCollected() { } return Commands.none().withName("isCollected"); } + + public static Command readyIntakeCommand() { + if (r.superStructure != null) { + + return r.superStructure.coralPreIntake().withName("readyIntake"); + } + return Commands.none().withName("readyIntake"); + } } diff --git a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java index b3a0e330..d03e654c 100644 --- a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; public class BargeAlign extends Command { private static final double fieldLength = 17.548; // Welded field @@ -30,8 +32,13 @@ public static boolean atScoringXPosition(CommandSwerveDrivetrain drivebasesubsys return blueBargeLineX < robotX && robotX < redBargeLineX; } - private static Command driveToBlackLine(CommandSwerveDrivetrain drivebaseSubsystem) { - return new BargeAlign(drivebaseSubsystem).withName("Drive to Black Line"); + private static Command driveToBlackLine( + CommandSwerveDrivetrain drivebaseSubsystem, + DoubleSupplier manualXSpeed, + DoubleSupplier manualYSpeed, + DoubleSupplier manualRotateSpeed) { + return new BargeAlign(drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) + .withName("Drive to Black Line"); } private static final SwerveRequest.FieldCentric bargeDriveRequest = @@ -44,24 +51,43 @@ private static Command driveToBlackLine(CommandSwerveDrivetrain drivebaseSubsyst private static final double xBargeDriveSpeed = 0.5; public static Command bargeScore( - CommandSwerveDrivetrain drivebaseSubsystem, SuperStructure superStructure) { + CommandSwerveDrivetrain drivebaseSubsystem, + SuperStructure superStructure, + DoubleSupplier manualXSpeed, + DoubleSupplier manualYSpeed, + DoubleSupplier manualRotateSpeed, + BooleanSupplier manualScore) { return Commands.sequence( - BargeAlign.driveToBlackLine(drivebaseSubsystem), - BargeAlign.driveToBarge(drivebaseSubsystem) + BargeAlign.driveToBlackLine( + drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) + .asProxy(), + BargeAlign.driveToBarge(drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) + .asProxy() .withDeadline( superStructure.algaeNetScore( - () -> BargeAlign.atScoringXPosition(drivebaseSubsystem)))); + () -> + manualScore.getAsBoolean() + || BargeAlign.atScoringXPosition(drivebaseSubsystem)))); } - private static Command driveToBarge(CommandSwerveDrivetrain drivebaseSubsystem) { + private static Command driveToBarge( + CommandSwerveDrivetrain drivebaseSubsystem, + DoubleSupplier manualXSpeed, + DoubleSupplier manualYSpeed, + DoubleSupplier manualRotateSpeed) { return drivebaseSubsystem .applyRequest( () -> { boolean onRedSide = drivebaseSubsystem.getState().Pose.getX() > fieldLength / 2; + double xSpeed = onRedSide ? -xBargeDriveSpeed : xBargeDriveSpeed; + double manualX = manualXSpeed.getAsDouble(); + if (manualX != 0) { + xSpeed = manualX; + } return bargeDriveRequest - .withVelocityX(onRedSide ? -xBargeDriveSpeed : xBargeDriveSpeed) - .withVelocityY(0) - .withRotationalRate(0); + .withVelocityX(xSpeed) + .withVelocityY(manualYSpeed.getAsDouble()) + .withRotationalRate(manualRotateSpeed.getAsDouble()); }) .until(() -> BargeAlign.atScoringXPosition(drivebaseSubsystem)) .finallyDo( @@ -75,9 +101,19 @@ private static Command driveToBarge(CommandSwerveDrivetrain drivebaseSubsystem) private PIDController pidRotate = new PIDController(8, 0, 0); private CommandSwerveDrivetrain drive; + private DoubleSupplier manualXSpeed; + private DoubleSupplier manualYSpeed; + private DoubleSupplier manualRotateSpeed; - private BargeAlign(CommandSwerveDrivetrain drive) { + private BargeAlign( + CommandSwerveDrivetrain drive, + DoubleSupplier manualXSpeed, + DoubleSupplier manualYSpeed, + DoubleSupplier manualRotateSpeed) { this.drive = drive; + this.manualXSpeed = manualXSpeed; + this.manualYSpeed = manualYSpeed; + this.manualRotateSpeed = manualRotateSpeed; pidRotate.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drive); setName("drive to black line"); @@ -101,10 +137,18 @@ public void execute() { powerX += .05 * Math.signum(powerX); double powerRotate = pidRotate.calculate(currentPose.getRotation().getRadians()); powerRotate = MathUtil.clamp(powerRotate, -4, 4); + double manualPowerX = manualXSpeed.getAsDouble(); + if (manualPowerX != 0) { + powerX = manualPowerX; + } + double manualPowerRotate = manualRotateSpeed.getAsDouble(); + if (manualPowerRotate != 0) { + powerRotate = manualPowerRotate; + } SwerveRequest request = blackLineDriveRequest .withVelocityX(powerX) - .withVelocityY(0) + .withVelocityY(manualYSpeed.getAsDouble()) .withRotationalRate(powerRotate); // Set the drive control with the created request drive.setControl(request); @@ -112,7 +156,7 @@ public void execute() { @Override public boolean isFinished() { - return Math.abs(pidX.getError()) < 0.01 + return Math.abs(pidX.getError()) < 0.05 && Math.abs(Units.radiansToDegrees(pidRotate.getError())) < 1; } } diff --git a/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java b/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java index 5b76a132..854828f6 100644 --- a/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java +++ b/src/main/java/frc/robot/subsystems/auto/DynamicSendableChooser.java @@ -90,6 +90,19 @@ public void setDefaultOption(String name, T object) { addOption(name, object); } + public String getSelectedName() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + } + /** * Returns the selected option. If there is none selected, it will return the default. If there is * none selected and no default, then it will return {@code null}. @@ -99,7 +112,7 @@ public void setDefaultOption(String name, T object) { public T getSelected() { m_mutex.lock(); try { - if (m_selected != null) { + if (m_selected != null && m_map.containsKey(m_selected)) { return m_map.get(m_selected); } else { return m_map.get(m_defaultChoice); diff --git a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java index 1c49cffa..d4427c06 100644 --- a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java @@ -239,10 +239,12 @@ private void startSimThread() { m_simNotifier.startPeriodic(kSimLoopPeriod); } + // returns the speeds for logging purposes public ChassisSpeeds returnSpeeds() { return getState().Speeds; } + // method for on-demand coasting control public Command coastMotors() { return startEnd( () -> { @@ -255,6 +257,7 @@ public Command coastMotors() { .withName("Coast Swerve"); } + // method for braking the motors after coasting public void brakeMotors() { configNeutralMode(NeutralModeValue.Brake); } diff --git a/src/main/java/frc/robot/util/AllianceUtils.java b/src/main/java/frc/robot/util/AllianceUtils.java new file mode 100644 index 00000000..1f0ef67e --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceUtils.java @@ -0,0 +1,19 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.DriverStation; + +public final class AllianceUtils { + public static boolean isBlue() { + if (!DriverStation.getAlliance().isEmpty()) { + return DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue); + } + return false; + } + + public static boolean isRed() { + if (!DriverStation.getAlliance().isEmpty()) { + return DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red); + } + return false; + } +} diff --git a/src/main/java/frc/robot/util/MatchTab.java b/src/main/java/frc/robot/util/MatchTab.java index b68e4666..8768884c 100644 --- a/src/main/java/frc/robot/util/MatchTab.java +++ b/src/main/java/frc/robot/util/MatchTab.java @@ -13,7 +13,8 @@ public static void create(Sensors sensors, Subsystems subsystems) { tab.addBoolean( "has arm sensor", () -> - (sensors.armSensor != null && sensors.armSensor.getSensorDistance().in(Meters) > 0)) + (sensors.armSensor != null + && sensors.armSensor.getSensorDistance().in(Meters) >= 0)) .withSize(1, 1) .withPosition(0, 0) .withWidget(BuiltInWidgets.kBooleanBox); @@ -21,7 +22,7 @@ public static void create(Sensors sensors, Subsystems subsystems) { "has ground intake sensor", () -> (sensors.intakeSensor != null - && sensors.intakeSensor.getSensorDistance().in(Meters) > 0)) + && sensors.intakeSensor.getSensorDistance().in(Meters) >= 0)) .withSize(1, 1) .withPosition(1, 0) .withWidget(BuiltInWidgets.kBooleanBox); @@ -29,7 +30,7 @@ public static void create(Sensors sensors, Subsystems subsystems) { "has left branch sensor", () -> (sensors.branchSensors != null - && sensors.branchSensors.getLeftSensorDistance().in(Meters) > 0)) + && sensors.branchSensors.getLeftSensorDistance().in(Meters) >= 0)) .withSize(1, 1) .withPosition(0, 1) .withWidget(BuiltInWidgets.kBooleanBox); @@ -37,7 +38,7 @@ public static void create(Sensors sensors, Subsystems subsystems) { "has right branch sensor", () -> (sensors.branchSensors != null - && sensors.branchSensors.getRightSensorDistance().in(Meters) > 0)) + && sensors.branchSensors.getRightSensorDistance().in(Meters) >= 0)) .withSize(1, 1) .withPosition(1, 1) .withWidget(BuiltInWidgets.kBooleanBox); @@ -50,10 +51,10 @@ public static void create(Sensors sensors, Subsystems subsystems) { .withSize(2, 2) .withPosition(3, 0) .withWidget(BuiltInWidgets.kBooleanBox); - tab.addCamera("left cam", "left", "http://10.24.12.11:1184/stream.mjpg") + tab.addCamera("left cam", "left", "http://10.24.12.11:1182/stream.mjpg") .withSize(4, 4) .withPosition(2, 2); - tab.addCamera("right cam", "right", "http://10.24.12.11:1182/stream.mjpg") + tab.addCamera("right cam", "right", "http://10.24.12.11:1184/stream.mjpg") .withSize(4, 4) .withPosition(6, 2); } diff --git a/src/main/java/frc/robot/util/ScoringType.java b/src/main/java/frc/robot/util/ScoringType.java new file mode 100644 index 00000000..b99ffad9 --- /dev/null +++ b/src/main/java/frc/robot/util/ScoringType.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum ScoringType { + SOLOC_RIGHT, + SOLOC_LEFT, + DRIVER; +} diff --git a/src/main/java/frc/robot/util/SoloScoringMode.java b/src/main/java/frc/robot/util/SoloScoringMode.java new file mode 100644 index 00000000..529fc716 --- /dev/null +++ b/src/main/java/frc/robot/util/SoloScoringMode.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum SoloScoringMode { + CORAL_IN_CLAW, + ALGAE_IN_CLAW, + NO_GAME_PIECE; +} diff --git a/vendordeps/PathplannerLib-2025.2.6.json b/vendordeps/PathplannerLib-2025.2.7.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.6.json rename to vendordeps/PathplannerLib-2025.2.7.json index 95ba2033..d3f84e53 100644 --- a/vendordeps/PathplannerLib-2025.2.6.json +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.6.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2025.2.6", + "version": "2025.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.6" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.6", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 8edd60f048c8fa69500b8b91c59745453066383b Mon Sep 17 00:00:00 2001 From: Mark Peterson Date: Fri, 28 Nov 2025 20:04:18 -0800 Subject: [PATCH 3/4] Post merge clean-up and removal of dead code --- src/main/java/frc/robot/Controls.java | 32 +++++++-------- .../java/frc/robot/subsystems/ClimbPivot.java | 6 --- .../robot/subsystems/ElevatorSubsystem.java | 3 -- .../java/frc/robot/subsystems/SpinnyClaw.java | 6 --- .../frc/robot/subsystems/VisionSubsystem.java | 16 -------- .../subsystems/auto/AutoAlgaeHeights.java | 2 +- .../drivebase/CommandSwerveDrivetrain.java | 41 ------------------- 7 files changed, 16 insertions(+), 90 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 39bbf9fd..9783f9b5 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -71,8 +71,6 @@ public class Controls { RobotType.getCurrent() == RobotType.BONK ? BonkTunerConstants.kSpeedAt12Volts.in(MetersPerSecond) : CompTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private final double MAX_ACCELERATION = 50; - private final double MAX_ROTATION_ACCELERATION = 50; // kSpeedAt12Volts desired top speed public static double MaxAngularRate = RotationsPerSecond.of(0.75) @@ -455,41 +453,41 @@ private Command getCoralBranchHeightCommand(ScoringType version) { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure .coralLevelThree( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure .coralLevelTwo( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure .coralLevelOne( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); }; } else if (version == ScoringType.SOLOC_RIGHT) { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure .coralLevelThree( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure .coralLevelTwo( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure .coralLevelOne( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); }; } else { return switch (branchHeight) { @@ -721,7 +719,7 @@ private void configureSpinnyClawBindings() { if (s.spinnyClawSubsytem == null) { return; } - s.spinnyClawSubsytem.setScoringMode(() -> scoringMode); + // Claw controls bindings go here connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.leftBumper()) @@ -975,7 +973,7 @@ private void configureSoloControllerBindings() { Commands.sequence( bargeScoreCommand, Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); + () -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE)); } case NO_GAME_PIECE -> { scoreCommand = @@ -993,12 +991,12 @@ private void configureSoloControllerBindings() { })); soloController .leftTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); soloController .leftTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB)); // Processor + Auto align right + Select scoring mode Coral @@ -1022,7 +1020,7 @@ private void configureSoloControllerBindings() { soloController.rightBumper()), Commands.waitSeconds(0.7), Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + () -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE)) .withName("Processor score"); case NO_GAME_PIECE -> Commands.parallel( Commands.runOnce(() -> intakeMode = ScoringMode.CORAL) @@ -1036,12 +1034,12 @@ private void configureSoloControllerBindings() { .withName("score")); soloController .rightTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); soloController .rightTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB)); // Ground Intake diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 260e962c..64f5e875 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -51,9 +51,6 @@ public enum TargetPositions { private final double FORWARD_SOFT_STOP = -0.07; private final double REVERSE_SOFT_STOP = -78; private final double CLIMB_OUT_SPEED = 1.0; - private final double CLIMB_HOLD_STOWED = -0.001; - private final double CLIMB_HOLD_CLIMBOUT = -0.0; - private final double CLIMB_HOLD_CLIMBED = -0.0705; private final double CLIMB_IN_SPEED = -0.75; private final double climbInKp = 50; @@ -74,7 +71,6 @@ public enum TargetPositions { private TargetPositions selectedPos = TargetPositions.STOWED; private double maxTargetPos = STOWED_MAX_PRESET; private double minTargetPos = STOWED_MIN_PRESET; - private double holdSpeed = CLIMB_HOLD_STOWED; // if moveComplete is true it wont move regardless of if its in range. This is to ensure that // when disabled, when re-enabled it doesnt start moving. @@ -190,7 +186,6 @@ private void setTargetPos(TargetPositions newTargetPos) { selectedPos = TargetPositions.CLIMB_OUT; maxTargetPos = CLIMB_OUT_MAX_PRESET; minTargetPos = CLIMB_OUT_MIN_PRESET; - holdSpeed = CLIMB_HOLD_STOWED; moveComplete = false; } case CLIMBED -> { @@ -199,7 +194,6 @@ private void setTargetPos(TargetPositions newTargetPos) { selectedPos = TargetPositions.CLIMBED; maxTargetPos = CLIMBED_MAX_PRESET; minTargetPos = CLIMBED_MIN_PRESET; - holdSpeed = CLIMB_HOLD_CLIMBOUT; moveComplete = false; } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 371953a9..69f91caf 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -60,11 +60,8 @@ public class ElevatorSubsystem extends SubsystemBase { private final double ELEVATOR_KS = 0.33878; private final double ELEVATOR_KV = 0.12975; private final double ELEVATOR_KA = 0.0070325; - private final double REVERSE_SOFT_LIMIT = -0.05; - private final double FORWARD_SOFT_LIMIT = 38; public static final double UP_VOLTAGE = 5; private final double DOWN_VOLTAGE = -3; - private final double HOLD_VOLTAGE = 0.6; // create a Motion Magic request, voltage output private final MotionMagicVoltage m_request = new MotionMagicVoltage(0); diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e8c15b5f..cad04ad7 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; import frc.robot.sensors.ArmSensor; -import frc.robot.util.ScoringMode; import java.util.function.Supplier; public class SpinnyClaw extends SubsystemBase { @@ -37,7 +36,6 @@ public class SpinnyClaw extends SubsystemBase { private final TalonFX motor; // ArmSensor private final ArmSensor armSensor; - private Supplier scoringMode = () -> ScoringMode.CORAL; // alerts private final Alert NotConnectedError = @@ -52,10 +50,6 @@ public SpinnyClaw(ArmSensor armSensor) { logTabs(); } - public void setScoringMode(Supplier scoringMode) { - this.scoringMode = scoringMode; - } - // (+) is to intake out, and (-) is in public Command movingVoltage(Supplier speedControl) { return run(() -> motor.setVoltage(speedControl.get().in(Volts))) diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 43e8f322..e21696d0 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -15,7 +15,6 @@ import edu.wpi.first.networktables.GenericSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -25,7 +24,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; -import java.util.Optional; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -279,18 +277,4 @@ public double getDistanceToTarget() { return (double) Math.round(Distance * 1000) / 1000; } - // configured for 2025 reefscape to filter out any tag besides the reef tags depending on allaince - private static boolean BadAprilTagDetector(PhotonPipelineResult r) { - boolean isRed = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Red)); - boolean isBlue = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Blue)); - for (var t : r.getTargets()) { - boolean isRedReef = 6 <= t.getFiducialId() && t.getFiducialId() <= 11; - boolean isBlueReef = 17 <= t.getFiducialId() && t.getFiducialId() <= 22; - boolean isValid = isBlueReef && !isRed || isRedReef && !isBlue; - if (!isValid) { - return true; - } - } - return false; - } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java index 4592ed7d..62f25d94 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; -import frc.robot.subsystems.SuperStructure; +import frc.robot.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import frc.robot.util.AllianceUtils; import frc.robot.util.ScoringMode; diff --git a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java index d4427c06..b09245bf 100644 --- a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java @@ -41,10 +41,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = @@ -58,43 +54,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su new SysIdRoutine.Mechanism( output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); - /* The SysId routine to test */ private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; From acb87ba1486940c81e09f1cea41aa2c885e97755 Mon Sep 17 00:00:00 2001 From: Mark Peterson Date: Fri, 28 Nov 2025 20:18:53 -0800 Subject: [PATCH 4/4] Fixing Spotless issue --- src/main/java/frc/robot/subsystems/VisionSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index e21696d0..15ce47bd 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -276,5 +276,4 @@ public double getTimeSinceLastReading() { public double getDistanceToTarget() { return (double) Math.round(Distance * 1000) / 1000; } - }