From 479956eab6ea058e0a34ea0b902948e084dc6f5d Mon Sep 17 00:00:00 2001 From: kujo27 Date: Thu, 27 Mar 2025 13:26:06 -0700 Subject: [PATCH 1/7] working --- .../OperatorCommandMap.java | 17 ++++--- .../AlignCameraToAprilTagCalculator.java | 44 ++++++++++++++++++- 2 files changed, 52 insertions(+), 9 deletions(-) diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index ad1c8b39..dda43336 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -41,8 +41,6 @@ import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.vision.CoprocessorCommunicationSubsystem; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.controls.sensors.XXboxController; import xbot.common.subsystems.autonomous.SetAutonomousCommand; @@ -113,11 +111,11 @@ public void setupDriverCommands( ); SequentialCommandGroup driveToClosestCoralStation = driveToClosestStationCommandGroupFactory.createDriveOnly(true); - operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( - pathDriveToClosestCoralStation, - driveToClosestCoralStation, - () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) - )); +// operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( +// pathDriveToClosestCoralStation, +// driveToClosestCoralStation, +// () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) +// )); // operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); // operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); @@ -125,6 +123,11 @@ public void setupDriverCommands( var aprilTagCalculator = aprilTagCalculatorFactory.create(); + operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A) + .onTrue(aprilTagCalculator.createSetHorizontalBranchOffsetMeters(1)); + operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.B) + .onTrue(aprilTagCalculator.createSetHorizontalBranchOffsetMeters(0)); + operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(aprilTagCalculator.createDecreaseOffsetByOneInchCommand()); operatorInterface.driverGamepad.getPovIfAvailable(270).onTrue(aprilTagCalculator.createIncreaseOffsetByOneInchCommand()); } diff --git a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java index def055f4..fe8cf1db 100644 --- a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java +++ b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java @@ -29,6 +29,8 @@ import xbot.common.subsystems.drive.control_logic.HeadingModule; import xbot.common.subsystems.vision.AprilTagVisionIO; +import java.util.HashMap; +import java.util.Map; import java.util.Optional; import static edu.wpi.first.units.Units.Degrees; @@ -113,6 +115,9 @@ public enum ApproachMode { private Translation2d coralStationPreShovePoint; boolean retryActive; + Map branchAOffsetHashMap = new HashMap<>(); + Map branchBOffsetHashMap = new HashMap<>(); + public static Translation2d generateAlignmentPointOffset(Distance robotCenterToOuterBumperX, CameraInfo cameraInfo, Distance offset, boolean isCameraBackwards) { return new Translation2d( @@ -162,6 +167,8 @@ public AlignCameraToAprilTagCalculator(AprilTagVisionSubsystemExtended vision, D globalHorizontalOffsetInches = pf.createPersistentProperty("GlobalHorizontalOffset-Inches", 0.0); + initializeBranchOffsets(pf); + reset(); } @@ -219,9 +226,15 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance isCameraBackwards ); + + double branchOffsetMeters = 0; + branchOffsetMeters = (targetCameraID == 0 ? branchBOffsetHashMap : branchAOffsetHashMap).get(targetAprilTagID).get(); + + akitLog.record("lastBranchOffset-M", branchOffsetMeters); + this.alignmentPointOffset = new Translation2d( alignmentPointOffset.getX(), - -getHorizontalTrimAdjustmentMeters() + (-getHorizontalTrimAdjustmentMeters() + branchOffsetMeters) ); // Now for some other one-time calculations about the tag itself @@ -280,7 +293,7 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance ); } - + akitLog.record("alignmentPointOffset", alignmentPointOffset); akitLog.record("InterstitialPoint", interstitialPoint); } @@ -511,4 +524,31 @@ private Translation2d getCoralStationPreShovePoint() { var projectedDelta = new Translation2d(0.25, stationLocation.getRotation()); return stationLocation.getTranslation().plus(projectedDelta); } + + public void initializeBranchOffsets(PropertyFactory pf) { + int[] tagIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; + for (int tagId : tagIds) { + branchAOffsetHashMap.put(tagId, pf.createPersistentProperty("BranchAOffset-Tag" + tagId, 0.0)); + branchBOffsetHashMap.put(tagId, pf.createPersistentProperty("BranchBOffset-Tag" + tagId, 0.0)); + } + } + + private void setBranchOffsetMeters(int targetCameraID) { + int tagID = aprilTagVisionSubsystem.getTargetAprilTagID(pose.getClosestReefFacePose()); + Optional aprilTagData = aprilTagVisionSubsystem.getRobotRelativeLocationOfAprilTag(targetCameraID, tagID); + + aprilTagData.ifPresent(data -> { + double offset = data.getY(); + if (targetCameraID == 1) { + branchAOffsetHashMap.get(tagID).set(offset); + } else { + branchBOffsetHashMap.get(tagID).set(offset); + } + }); + } + + public Command createSetHorizontalBranchOffsetMeters(int targetCameraID) { + return new InstantCommand(() -> setBranchOffsetMeters(targetCameraID)).ignoringDisable(true); + } + } From e8b342927b31f69e890d48b9d6802257a2202f3e Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 07:26:11 -0700 Subject: [PATCH 2/7] clean --- .../drive/logic/AlignCameraToAprilTagCalculator.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java index fe8cf1db..136a989c 100644 --- a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java +++ b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java @@ -99,6 +99,7 @@ public enum ApproachMode { final DoubleProperty globalHorizontalOffsetInches; private double globalTemporaryHorizontalOffsetInches; + private double branchOffsetMeters; double lastKnownHorizontalErrorMeters = 999999; double shoveStartTime = 0; @@ -226,12 +227,8 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance isCameraBackwards ); - - double branchOffsetMeters = 0; branchOffsetMeters = (targetCameraID == 0 ? branchBOffsetHashMap : branchAOffsetHashMap).get(targetAprilTagID).get(); - akitLog.record("lastBranchOffset-M", branchOffsetMeters); - this.alignmentPointOffset = new Translation2d( alignmentPointOffset.getX(), (-getHorizontalTrimAdjustmentMeters() + branchOffsetMeters) @@ -486,7 +483,7 @@ private void updateFinalTargetState(Pose2d currentPose) { Optional aprilTagData = aprilTagVisionSubsystem.getRobotRelativeLocationOfAprilTag(targetCameraID, targetAprilTagID); if (aprilTagData.isPresent()) { - lastKnownHorizontalErrorMeters = aprilTagData.get().getY() + getHorizontalTrimAdjustmentMeters(); + lastKnownHorizontalErrorMeters = aprilTagData.get().getY() + getHorizontalTrimAdjustmentMeters() + branchOffsetMeters; } akitLog.record("AprilTagData", aprilTagData.orElse(null)); @@ -525,7 +522,7 @@ private Translation2d getCoralStationPreShovePoint() { return stationLocation.getTranslation().plus(projectedDelta); } - public void initializeBranchOffsets(PropertyFactory pf) { + private void initializeBranchOffsets(PropertyFactory pf) { int[] tagIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; for (int tagId : tagIds) { branchAOffsetHashMap.put(tagId, pf.createPersistentProperty("BranchAOffset-Tag" + tagId, 0.0)); From 40af3b9c497532249a44b611ffc3ad7ac8f4a102 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 07:34:28 -0700 Subject: [PATCH 3/7] elastic layout --- Elastic/auto-setup.json | 352 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 352 insertions(+) diff --git a/Elastic/auto-setup.json b/Elastic/auto-setup.json index 902954b7..ef3b39c9 100644 --- a/Elastic/auto-setup.json +++ b/Elastic/auto-setup.json @@ -116,6 +116,358 @@ } ] } + }, + { + "name": "BranchAOffsets", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "BranchAOffset-Tag10", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag10", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag11", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag11", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag18", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag18", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag6", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag6", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag7", + "x": 0.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag7", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag8", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag8", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag17", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag17", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag9", + "x": 0.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag9", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag19", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag19", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag20", + "x": 512.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag20", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag21", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag21", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchAOffset-Tag22", + "x": 1024.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchAOffset-Tag22", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "BranchBOffsets", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "BranchBOffset-Tag21", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag21", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag22", + "x": 1024.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag22", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag10", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag10", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag9", + "x": 0.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag9", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag8", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag8", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag7", + "x": 0.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag7", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag6", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag6", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag11", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag11", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag17", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag17", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag18", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag18", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag19", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag19", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "BranchBOffset-Tag20", + "x": 512.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/AlignCameraToAprilTagCalculator/BranchBOffset-Tag20", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } } ] } \ No newline at end of file From 4c2c49fe4844d110326510c3dfb1c09ef089161e Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 10:31:53 -0700 Subject: [PATCH 4/7] on smartdashboard --- Elastic/auto-setup.json | 66 +++++++++++++++++++ .../OperatorCommandMap.java | 12 ++-- 2 files changed, 74 insertions(+), 4 deletions(-) diff --git a/Elastic/auto-setup.json b/Elastic/auto-setup.json index ef3b39c9..3c4557c2 100644 --- a/Elastic/auto-setup.json +++ b/Elastic/auto-setup.json @@ -113,6 +113,46 @@ "period": 0.06, "show_type": true } + }, + { + "title": "Left Vision Auto", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Left Vision Auto", + "period": 0.06, + "show_type": true + } + }, + { + "title": "Right Vision Auto", + "x": 1152.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Right Vision Auto", + "period": 0.06, + "show_type": true + } + }, + { + "title": "GlobalHorizontalOffset-Inches", + "x": 768.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/AlignCameraToAprilTagCalculator//GlobalHorizontalOffset-Inches", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } } ] } @@ -289,6 +329,19 @@ "data_type": "double", "show_submit_button": false } + }, + { + "title": "SetHorizontalBranchAOffset", + "x": 896.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/SetHorizontalBranchAOffset", + "period": 0.06, + "show_type": true + } } ] } @@ -465,6 +518,19 @@ "data_type": "double", "show_submit_button": false } + }, + { + "title": "SetHorizontalBranchBOffset", + "x": 896.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/SetHorizontalBranchBOffset", + "period": 0.06, + "show_type": true + } } ] } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index dda43336..6df6995f 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -40,6 +40,7 @@ import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.vision.CoprocessorCommunicationSubsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.controls.sensors.XXboxController; @@ -123,10 +124,13 @@ public void setupDriverCommands( var aprilTagCalculator = aprilTagCalculatorFactory.create(); - operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A) - .onTrue(aprilTagCalculator.createSetHorizontalBranchOffsetMeters(1)); - operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.B) - .onTrue(aprilTagCalculator.createSetHorizontalBranchOffsetMeters(0)); + var setHorizontalBranchAOffsetMeters = aprilTagCalculator.createSetHorizontalBranchOffsetMeters(1); + setHorizontalBranchAOffsetMeters.setName("SetHorizontalBranchAOffset"); + SmartDashboard.putData(setHorizontalBranchAOffsetMeters); + + var setHorizontalBranchBOffsetMeters = aprilTagCalculator.createSetHorizontalBranchOffsetMeters(0); + setHorizontalBranchBOffsetMeters.setName("SetHorizontalBranchBOffset"); + SmartDashboard.putData(setHorizontalBranchBOffsetMeters); operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(aprilTagCalculator.createDecreaseOffsetByOneInchCommand()); operatorInterface.driverGamepad.getPovIfAvailable(270).onTrue(aprilTagCalculator.createIncreaseOffsetByOneInchCommand()); From 59d923e6f92ddbb3f3a3af1878ad857f09f2cee0 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 10:36:03 -0700 Subject: [PATCH 5/7] uncommented code --- .../operator_interface/OperatorCommandMap.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 6df6995f..23646e4a 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -42,6 +42,7 @@ import competition.subsystems.vision.CoprocessorCommunicationSubsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.controls.sensors.XXboxController; import xbot.common.subsystems.autonomous.SetAutonomousCommand; @@ -112,11 +113,11 @@ public void setupDriverCommands( ); SequentialCommandGroup driveToClosestCoralStation = driveToClosestStationCommandGroupFactory.createDriveOnly(true); -// operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( -// pathDriveToClosestCoralStation, -// driveToClosestCoralStation, -// () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) -// )); + operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( + pathDriveToClosestCoralStation, + driveToClosestCoralStation, + () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) + )); // operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); // operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); From 70cf60574910f478965bce9028ffa6b1e3744878 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 15:14:27 -0700 Subject: [PATCH 6/7] using one hashmap insted of two --- .../logic/AlignCameraToAprilTagCalculator.java | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java index 136a989c..8c44dd69 100644 --- a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java +++ b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java @@ -116,8 +116,8 @@ public enum ApproachMode { private Translation2d coralStationPreShovePoint; boolean retryActive; - Map branchAOffsetHashMap = new HashMap<>(); - Map branchBOffsetHashMap = new HashMap<>(); + record BranchOffsetKey(Landmarks.Branch branch, Integer tagID) {} + Map branchOffsetHashMap = new HashMap<>(); public static Translation2d generateAlignmentPointOffset(Distance robotCenterToOuterBumperX, CameraInfo cameraInfo, Distance offset, boolean isCameraBackwards) { @@ -227,7 +227,8 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance isCameraBackwards ); - branchOffsetMeters = (targetCameraID == 0 ? branchBOffsetHashMap : branchAOffsetHashMap).get(targetAprilTagID).get(); + + branchOffsetMeters = branchOffsetHashMap.get(new BranchOffsetKey(targetCameraID == 0 ? Landmarks.Branch.B : Landmarks.Branch.A, targetAprilTagID)).get(); this.alignmentPointOffset = new Translation2d( alignmentPointOffset.getX(), @@ -525,8 +526,8 @@ private Translation2d getCoralStationPreShovePoint() { private void initializeBranchOffsets(PropertyFactory pf) { int[] tagIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; for (int tagId : tagIds) { - branchAOffsetHashMap.put(tagId, pf.createPersistentProperty("BranchAOffset-Tag" + tagId, 0.0)); - branchBOffsetHashMap.put(tagId, pf.createPersistentProperty("BranchBOffset-Tag" + tagId, 0.0)); + branchOffsetHashMap.put(new BranchOffsetKey(Landmarks.Branch.A, tagId), pf.createPersistentProperty("BranchAOffset-Tag" + tagId, 0.0)); + branchOffsetHashMap.put(new BranchOffsetKey(Landmarks.Branch.B, tagId), pf.createPersistentProperty("BranchBOffset-Tag" + tagId, 0.0)); } } @@ -536,11 +537,8 @@ private void setBranchOffsetMeters(int targetCameraID) { aprilTagData.ifPresent(data -> { double offset = data.getY(); - if (targetCameraID == 1) { - branchAOffsetHashMap.get(tagID).set(offset); - } else { - branchBOffsetHashMap.get(tagID).set(offset); - } + + branchOffsetHashMap.get(new BranchOffsetKey(targetCameraID == 1 ? Landmarks.Branch.A : Landmarks.Branch.B, tagID)).set(offset); }); } From c9835a2d96ce204e6b81121973de087fd3eac476 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Tue, 1 Apr 2025 18:44:17 -0700 Subject: [PATCH 7/7] removed inaccurate way to measuring offset, updated auto elastic layout --- Elastic/auto-setup.json | 26 ------------------- .../OperatorCommandMap.java | 8 ------ .../AlignCameraToAprilTagCalculator.java | 25 +++++------------- 3 files changed, 6 insertions(+), 53 deletions(-) diff --git a/Elastic/auto-setup.json b/Elastic/auto-setup.json index 3c4557c2..acf62dba 100644 --- a/Elastic/auto-setup.json +++ b/Elastic/auto-setup.json @@ -329,19 +329,6 @@ "data_type": "double", "show_submit_button": false } - }, - { - "title": "SetHorizontalBranchAOffset", - "x": 896.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/SetHorizontalBranchAOffset", - "period": 0.06, - "show_type": true - } } ] } @@ -518,19 +505,6 @@ "data_type": "double", "show_submit_button": false } - }, - { - "title": "SetHorizontalBranchBOffset", - "x": 896.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/SetHorizontalBranchBOffset", - "period": 0.06, - "show_type": true - } } ] } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index b46313cd..1eb91679 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -136,14 +136,6 @@ public void setupDriverCommands( var aprilTagCalculator = aprilTagCalculatorFactory.create(); - var setHorizontalBranchAOffsetMeters = aprilTagCalculator.createSetHorizontalBranchOffsetMeters(1); - setHorizontalBranchAOffsetMeters.setName("SetHorizontalBranchAOffset"); - SmartDashboard.putData(setHorizontalBranchAOffsetMeters); - - var setHorizontalBranchBOffsetMeters = aprilTagCalculator.createSetHorizontalBranchOffsetMeters(0); - setHorizontalBranchBOffsetMeters.setName("SetHorizontalBranchBOffset"); - SmartDashboard.putData(setHorizontalBranchBOffsetMeters); - operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(aprilTagCalculator.createDecreaseOffsetByOneInchCommand()); operatorInterface.driverGamepad.getPovIfAvailable(270).onTrue(aprilTagCalculator.createIncreaseOffsetByOneInchCommand()); } diff --git a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java index c42c2e17..084f4f79 100644 --- a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java +++ b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java @@ -228,7 +228,8 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance ); - branchOffsetMeters = branchOffsetHashMap.get(new BranchOffsetKey(targetCameraID == 0 ? Landmarks.Branch.B : Landmarks.Branch.A, targetAprilTagID)).get(); + branchOffsetMeters = branchOffsetHashMap.get( + new BranchOffsetKey(targetCameraID == 0 ? Landmarks.Branch.B : Landmarks.Branch.A, targetAprilTagID)).get(); this.alignmentPointOffset = new Translation2d( alignmentPointOffset.getX(), @@ -530,24 +531,10 @@ private Translation2d getCoralStationPreShovePoint() { private void initializeBranchOffsets(PropertyFactory pf) { int[] tagIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; for (int tagId : tagIds) { - branchOffsetHashMap.put(new BranchOffsetKey(Landmarks.Branch.A, tagId), pf.createPersistentProperty("BranchAOffset-Tag" + tagId, 0.0)); - branchOffsetHashMap.put(new BranchOffsetKey(Landmarks.Branch.B, tagId), pf.createPersistentProperty("BranchBOffset-Tag" + tagId, 0.0)); + branchOffsetHashMap.put(new BranchOffsetKey(Landmarks.Branch.A, tagId), + pf.createPersistentProperty("BranchAOffset-Tag" + tagId, 0.0)); + branchOffsetHashMap.put(new BranchOffsetKey(Landmarks.Branch.B, tagId), + pf.createPersistentProperty("BranchBOffset-Tag" + tagId, 0.0)); } } - - private void setBranchOffsetMeters(int targetCameraID) { - int tagID = aprilTagVisionSubsystem.getTargetAprilTagID(pose.getClosestReefFacePose()); - Optional aprilTagData = aprilTagVisionSubsystem.getRobotRelativeLocationOfAprilTag(targetCameraID, tagID); - - aprilTagData.ifPresent(data -> { - double offset = data.getY(); - - branchOffsetHashMap.get(new BranchOffsetKey(targetCameraID == 1 ? Landmarks.Branch.A : Landmarks.Branch.B, tagID)).set(offset); - }); - } - - public Command createSetHorizontalBranchOffsetMeters(int targetCameraID) { - return new InstantCommand(() -> setBranchOffsetMeters(targetCameraID)).ignoringDisable(true); - } - }