diff --git a/Elastic/auto-setup.json b/Elastic/auto-setup.json index 902954b7c..acf62dba2 100644 --- a/Elastic/auto-setup.json +++ b/Elastic/auto-setup.json @@ -113,6 +113,398 @@ "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 + } + } + ] + } + }, + { + "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 + } } ] } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 3508170cb..952bc9ad1 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -43,6 +43,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.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java index a82854a5f..084f4f799 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; @@ -97,6 +99,7 @@ public enum ApproachMode { final DoubleProperty globalHorizontalOffsetInches; private double globalTemporaryHorizontalOffsetInches; + private double branchOffsetMeters; double lastKnownHorizontalErrorMeters = 999999; double shoveStartTime = 0; @@ -113,6 +116,9 @@ public enum ApproachMode { private Translation2d coralStationPreShovePoint; boolean retryActive; + record BranchOffsetKey(Landmarks.Branch branch, Integer tagID) {} + Map branchOffsetHashMap = new HashMap<>(); + public static Translation2d generateAlignmentPointOffset(Distance robotCenterToOuterBumperX, CameraInfo cameraInfo, Distance offset, boolean isCameraBackwards) { return new Translation2d( @@ -162,6 +168,8 @@ public AlignCameraToAprilTagCalculator(AprilTagVisionSubsystemExtended vision, D globalHorizontalOffsetInches = pf.createPersistentProperty("GlobalHorizontalOffset-Inches", 0.0); + initializeBranchOffsets(pf); + reset(); } @@ -219,9 +227,13 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance isCameraBackwards ); + + branchOffsetMeters = branchOffsetHashMap.get( + new BranchOffsetKey(targetCameraID == 0 ? Landmarks.Branch.B : Landmarks.Branch.A, targetAprilTagID)).get(); + this.alignmentPointOffset = new Translation2d( alignmentPointOffset.getX(), - -getHorizontalTrimAdjustmentMeters() + (-getHorizontalTrimAdjustmentMeters() + branchOffsetMeters) ); // Now for some other one-time calculations about the tag itself @@ -280,7 +292,7 @@ public void configureAndReset(int targetAprilTagID, int targetCameraID, Distance ); } - + akitLog.record("alignmentPointOffset", alignmentPointOffset); akitLog.record("InterstitialPoint", interstitialPoint); } @@ -477,7 +489,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)); @@ -515,4 +527,14 @@ private Translation2d getCoralStationPreShovePoint() { var projectedDelta = new Translation2d(0.25, stationLocation.getRotation()); return stationLocation.getTranslation().plus(projectedDelta); } + + 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)); + } + } }