From 49d9788a4f708be2a0d9416b180a1a29032e92ff Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Wed, 28 Feb 2024 17:09:17 -0800 Subject: [PATCH 01/10] Upgrades to support dynamic auto --- .../Robot_Xbot2024/config.json | 6 +++--- SeriouslyCommonLib | 2 +- .../competition/simulation/Simulator2024.java | 10 +++++----- .../subsystems/arm/ArmSubsystem.java | 19 ++++++++++++------- .../subsystems/oracle/DynamicOracle.java | 16 ++++++++++++---- .../competition/subsystems/oracle/Note.java | 1 + .../subsystems/oracle/NoteMap.java | 6 +++--- ...uperstructureAccordingToOracleCommand.java | 6 +++--- .../SwerveAccordingToOracleCommand.java | 16 ++++++++++++++-- .../subsystems/pose/PoseSubsystem.java | 10 +++++----- .../shooter/ShooterWheelSubsystem.java | 4 ++-- .../subsystems/vision/VisionSubsystem.java | 2 ++ 12 files changed, 63 insertions(+), 35 deletions(-) diff --git a/AdvantageScopeModels/Robot_Xbot2024/config.json b/AdvantageScopeModels/Robot_Xbot2024/config.json index fcc9b7b5..1632138f 100644 --- a/AdvantageScopeModels/Robot_Xbot2024/config.json +++ b/AdvantageScopeModels/Robot_Xbot2024/config.json @@ -8,7 +8,7 @@ }, { "axis": "z", - "degrees": -90 + "degrees": 90 } ], "position": [ @@ -58,7 +58,7 @@ }, { "axis": "z", - "degrees": -90 + "degrees": 90 }, { "axis": "y", @@ -66,7 +66,7 @@ } ], "zeroedPosition": [ - -0.3, + 0.3, 0, 0.435 ] diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 979e6d36..d08d05b9 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 979e6d3690d3140a7086d7ae8655dba58d561644 +Subproject commit d08d05b983bf697ddb910039681e6f9bd6061daf diff --git a/src/main/java/competition/simulation/Simulator2024.java b/src/main/java/competition/simulation/Simulator2024.java index 4e9426a6..0237617a 100644 --- a/src/main/java/competition/simulation/Simulator2024.java +++ b/src/main/java/competition/simulation/Simulator2024.java @@ -77,7 +77,7 @@ private void visualizeNotes() { private void simulateCollector() { // If we are running the collector and near a note, - var nearestNote = oracle.getNoteMap().getClosestNote(pose.getCurrentPose2d().getTranslation(), 0.1, Note.NoteAvailability.Available); + var nearestNote = oracle.getNoteMap().getClosestNote(pose.getCurrentPose2d().getTranslation(), 0.1, Note.NoteAvailability.Available, Note.NoteAvailability.AgainstObstacle); if (nearestNote != null && collector.collectorMotor.getAppliedOutput() > 0.05) { // Trigger our sensors to say we have one. ((MockDigitalInput)collector.inControlNoteSensor).setValue(true); @@ -187,18 +187,18 @@ private void initializeNewFiredNote() { // create a translation3d representing note start point. noteStartPoint = new Translation3d( currentPose.getTranslation().getX() - + Math.cos(currentPose.getRotation().getRadians() + Math.PI) * 0.33, + + Math.cos(currentPose.getRotation().getRadians()) * 0.33, currentPose.getTranslation().getY() - + Math.sin(currentPose.getRotation().getRadians() + Math.PI) * 0.33, + + Math.sin(currentPose.getRotation().getRadians()) * 0.33, 0.33 ); // create a translation3d representing note finish point. noteFinishPoint = new Translation3d( currentPose.getTranslation().getX() - + Math.cos(currentPose.getRotation().getRadians() + Math.PI) * 3, + + Math.cos(currentPose.getRotation().getRadians()) * 3, currentPose.getTranslation().getY() - + Math.sin(currentPose.getRotation().getRadians() + Math.PI) * 3, + + Math.sin(currentPose.getRotation().getRadians()) * 3, 3 ); } diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 9a1cb32c..8802cddd 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -27,6 +27,7 @@ import javax.inject.Inject; import javax.inject.Singleton; +import java.util.Arrays; @Singleton public class ArmSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { @@ -81,6 +82,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF private int totalLoops = 0; private int loopsWhereCompressorRunning = 0; + private static double[] experimentalRangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136}; + private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 20.0, 26, 41, 57, 64}; public enum ArmState { EXTENDING, @@ -202,19 +205,17 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa .append(new MechanismLigament2d("box-top", 2, 90)) .append(new MechanismLigament2d("box-left", 1, 90)); - - double[] rangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136}; - double[] rangesInMeters = new double[7]; + double[] rangesInMeters = new double[experimentalRangesInInches.length]; //PoseSubsystem.INCHES_IN_A_METER // Convert the rangesInInches array to meters - for (int i = 0; i < rangesInInches.length; i++) { - rangesInMeters[i] = rangesInInches[i] / PoseSubsystem.INCHES_IN_A_METER; + for (int i = 0; i < experimentalRangesInInches.length; i++) { + rangesInMeters[i] = experimentalRangesInInches[i] / PoseSubsystem.INCHES_IN_A_METER; } speakerDistanceToExtensionInterpolator = new DoubleInterpolator( rangesInMeters, - new double[]{0, 0, 20.0, 26, 41, 57, 64}); + experimentalArmExtensionsInMm); } public double constrainPowerIfNearLimit(double power, double actualPosition) { @@ -643,7 +644,11 @@ public double getRecommendedExtensionForSpeaker() { @Override protected boolean areTwoTargetsEquivalent(Double target1, Double target2) { - return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2); + return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2, 1); + } + + public double getMaximumRangeForAnyShot() { + return experimentalRangesInInches[experimentalRangesInInches.length - 1] / PoseSubsystem.INCHES_IN_A_METER; } public void periodic() { diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 72afa9c3..d1956512 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -1,5 +1,6 @@ package competition.subsystems.oracle; +import competition.subsystems.arm.ArmSubsystem; import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -25,6 +26,7 @@ public enum HighLevelGoal { public enum ScoringSubGoals { IngestNote, + IngestNoteAgainstObstacle, MoveToScoringRange, EarnestlyLaunchNote } @@ -39,19 +41,21 @@ public enum ScoringSubGoals { boolean firstRunInNewGoal; PoseSubsystem pose; + ArmSubsystem arm; int instructionNumber = 0; @Inject public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiringInfoSource noteFiringInfoSource, - PoseSubsystem pose) { + PoseSubsystem pose, ArmSubsystem arm) { this.noteCollectionInfoSource = noteCollectionInfoSource; this.noteFiringInfoSource = noteFiringInfoSource; this.noteMap = new NoteMap(); this.pose = pose; + this.arm = arm; this.currentHighLevelGoal = HighLevelGoal.CollectNote; - this.currentScoringSubGoal = ScoringSubGoals.IngestNote; + this.currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; firstRunInNewGoal = true; setupLowResField(); @@ -186,10 +190,11 @@ public void periodic() { // Choose a good note collection location Note suggestedNote = noteMap.getClosestNote(pose.getCurrentPose2d().getTranslation(), Note.NoteAvailability.Available, + Note.NoteAvailability.AgainstObstacle, Note.NoteAvailability.SuggestedByDriver, Note.NoteAvailability.SuggestedByVision); setTargetNote(suggestedNote); - setSpecialAimTarget(null); + setSpecialAimTarget(suggestedNote.getLocation()); if (suggestedNote == null) { // No notes on the field! Let's suggest going to the source and hope something turns up. setTerminatingPoint(new Pose2d(14, 1.2, Rotation2d.fromDegrees(0))); @@ -296,15 +301,18 @@ public ScoringSubGoals getScoringSubgoal() { private void determineScoringSubgoal() { double acceptableRangeBeforeScoringMeters = 0; + boolean inUnderstoodRange = false; if (currentHighLevelGoal == HighLevelGoal.ScoreInSpeaker) { acceptableRangeBeforeScoringMeters = 2; + inUnderstoodRange = pose.getDistanceFromSpeaker() < arm.getMaximumRangeForAnyShot(); + } else if (currentHighLevelGoal == HighLevelGoal.ScoreInAmp) { // in the future we'll do something more like "get near amp, then drive into the wall for a few moments // before scoring" acceptableRangeBeforeScoringMeters = 0.05; } - if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters)) { + if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters) && inUnderstoodRange) { currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; } else { currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; diff --git a/src/main/java/competition/subsystems/oracle/Note.java b/src/main/java/competition/subsystems/oracle/Note.java index 134851fc..ccae39e5 100644 --- a/src/main/java/competition/subsystems/oracle/Note.java +++ b/src/main/java/competition/subsystems/oracle/Note.java @@ -6,6 +6,7 @@ public class Note { public enum NoteAvailability { Available, + AgainstObstacle, ReservedByOthersInAuto, SuggestedByVision, SuggestedByDriver, diff --git a/src/main/java/competition/subsystems/oracle/NoteMap.java b/src/main/java/competition/subsystems/oracle/NoteMap.java index 6311023e..253d9c37 100644 --- a/src/main/java/competition/subsystems/oracle/NoteMap.java +++ b/src/main/java/competition/subsystems/oracle/NoteMap.java @@ -23,7 +23,7 @@ public NoteMap() { private void initializeNotes() { addNote(Note.KeyNoteNames.BlueSpikeTop, new Note(PoseSubsystem.SpikeTop)); addNote(Note.KeyNoteNames.BlueSpikeMiddle, new Note(PoseSubsystem.SpikeMiddle)); - addNote(Note.KeyNoteNames.BlueSpikeBottom, new Note(PoseSubsystem.SpikeBottom)); + addNote(Note.KeyNoteNames.BlueSpikeBottom, new Note(PoseSubsystem.SpikeBottom, 1, Note.NoteAvailability.AgainstObstacle)); addNote(Note.KeyNoteNames.CenterLine1, new Note(PoseSubsystem.CenterLine1)); addNote(Note.KeyNoteNames.CenterLine2, new Note(PoseSubsystem.CenterLine2)); addNote(Note.KeyNoteNames.CenterLine3, new Note(PoseSubsystem.CenterLine3)); @@ -31,7 +31,7 @@ private void initializeNotes() { addNote(Note.KeyNoteNames.CenterLine5, new Note(PoseSubsystem.CenterLine5)); addNote(Note.KeyNoteNames.RedSpikeTop, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeTop))); addNote(Note.KeyNoteNames.RedSpikeMiddle, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeMiddle))); - addNote(Note.KeyNoteNames.RedSpikeBottom, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeBottom))); + addNote(Note.KeyNoteNames.RedSpikeBottom, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeBottom),1, Note.NoteAvailability.AgainstObstacle)); } public void addNote(String key, Note note) { @@ -85,7 +85,7 @@ public Pose3d[] getAllKnownNotes() { for (Note note : this.internalNoteMap.values()) { double noteZ = 0.025; - if (note.getAvailability() != Note.NoteAvailability.Available) { + if (note.getAvailability() != Note.NoteAvailability.Available && note.getAvailability() != Note.NoteAvailability.AgainstObstacle) { noteZ = -3.0; } diff --git a/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java index 018f2806..3128476a 100644 --- a/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java @@ -82,15 +82,15 @@ private void fireWhenReady() { aKitLog.record("SanityChecks", sanityChecks); - if (superStructureReady && shouldCommitToFiring && sanityChecks) { + if ((superStructureReady && sanityChecks) || shouldCommitToFiring) { collector.fire(); } else { collector.stop(); } } + // Once we've started shooting, don't stop until we're sure the note is totally gone. private boolean getShouldCommitToFiring() { - return collector.getIntakeState() == CollectorSubsystem.IntakeState.FIRING - || (shooter.isMaintainerAtGoal() && arm.isMaintainerAtGoal()); + return collector.getIntakeState() == CollectorSubsystem.IntakeState.FIRING; } } diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index 77cb45bd..0e0a4fc8 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java @@ -71,8 +71,20 @@ private void setNewInstruction() { // Since we're going to grab a note, point the front end of our robot towards the goal, // since our collector is on the front side. logic.setDriveBackwards(true); - logic.setEnableSpecialAimTarget(false); + if (oracle.targetNote.getAvailability() == Note.NoteAvailability.AgainstObstacle) { + // if a note is against an obstacle, we need to always point at it to avoid rotation thrashing as we + // try to approach it + logic.setEnableSpecialAimTarget(true); + logic.setSpecialAimTarget(oracle.getSpecialAimTarget()); + logic.setAimAtIntermediateNonFinalLegs(false); + } else { + // Note is in the clear, just drive straight at it. + logic.setEnableSpecialAimTarget(false); + logic.setAimAtIntermediateNonFinalLegs(true); + } + + logic.setEnableSpecialAimDuringFinalLeg(false); // When approaching the note, make sure to aim straight at the note for the best chance of collection. logic.setAimAtGoalDuringFinalLeg(true); @@ -83,7 +95,7 @@ private void setNewInstruction() { // This "special aim" mode instructs the robot to aim at a point that isn't the goal point. For example, // we could aim directly at the Speaker aperture regardless of where the robot is, causing the robot to // "track" the speaker as it moves towards a nice firing point. - logic.setEnableSpecialAimTarget(true); + logic.setEnableSpecialAimDuringFinalLeg(true); logic.setSpecialAimTarget(oracle.getSpecialAimTarget()); // TODO: split the score in speaker and score in Amp into two independent sections. It's likely diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 08b62725..3c65efa4 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -58,11 +58,11 @@ public class PoseSubsystem extends BasePoseSubsystem { public static Pose2d SpikeTop = new Pose2d(2.8956, 7.0012, new Rotation2d()); public static Pose2d SpikeMiddle = new Pose2d(2.8956, 5.5478, new Rotation2d()); public static Pose2d SpikeBottom = new Pose2d(2.8956, 4.1056, new Rotation2d()); - public static Pose2d CenterLine1 = new Pose2d(8.2956, 7.4584, new Rotation2d()); - public static Pose2d CenterLine2 = new Pose2d(8.2956, 5.7820, new Rotation2d()); - public static Pose2d CenterLine3 = new Pose2d(8.2956, 4.1056, new Rotation2d()); - public static Pose2d CenterLine4 = new Pose2d(8.2956, 2.4292, new Rotation2d()); - public static Pose2d CenterLine5 = new Pose2d(8.2956, 0.7528, new Rotation2d()); + public static Pose2d CenterLine1 = new Pose2d(8.2705, 7.4584, new Rotation2d()); + public static Pose2d CenterLine2 = new Pose2d(8.2705, 5.7820, new Rotation2d()); + public static Pose2d CenterLine3 = new Pose2d(8.2705, 4.1056, new Rotation2d()); + public static Pose2d CenterLine4 = new Pose2d(8.2705, 2.4292, new Rotation2d()); + public static Pose2d CenterLine5 = new Pose2d(8.2705, 0.7528, new Rotation2d()); public static Pose2d NearbySource = new Pose2d(14, 1.2, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java index e7e36be3..7221283d 100644 --- a/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java +++ b/src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java @@ -231,8 +231,8 @@ public ShooterWheelTargetSpeeds getSpeedForRange(){ @Override protected boolean areTwoTargetsEquivalent(ShooterWheelTargetSpeeds target1, ShooterWheelTargetSpeeds target2) { - return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1.upperWheelsTargetRPM, target2.upperWheelsTargetRPM) - && BaseSetpointSubsystem.areTwoDoublesEquivalent(target1.lowerWheelsTargetRPM, target2.lowerWheelsTargetRPM); + return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1.upperWheelsTargetRPM, target2.upperWheelsTargetRPM, 100) + && BaseSetpointSubsystem.areTwoDoublesEquivalent(target1.lowerWheelsTargetRPM, target2.lowerWheelsTargetRPM, 100); } public void periodic() { diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java index fa0b8e10..fee07e9a 100644 --- a/src/main/java/competition/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java @@ -66,6 +66,8 @@ public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalC try { aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + double width = aprilTagFieldLayout.getFieldWidth(); + double height = aprilTagFieldLayout.getFieldLength(); aprilTagsLoaded = true; log.info("Successfully loaded AprilTagFieldLayout"); } catch (IOException e) { From 6153a481e0b30f25d31c7a4fc8bae2b389b9a27f Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Wed, 28 Feb 2024 22:10:05 -0800 Subject: [PATCH 02/10] Now E2E no major bugs, but still doesn't handle red alliance well. --- SeriouslyCommonLib | 2 +- .../subsystems/oracle/DynamicOracle.java | 27 +++++++++--- .../SwerveAccordingToOracleCommand.java | 41 +++++++++++-------- 3 files changed, 48 insertions(+), 22 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index d08d05b9..579b8e42 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit d08d05b983bf697ddb910039681e6f9bd6061daf +Subproject commit 579b8e42a7f2d8ea95f7223388125dea2cb896cb diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index d1956512..058d6940 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -194,13 +194,27 @@ public void periodic() { Note.NoteAvailability.SuggestedByDriver, Note.NoteAvailability.SuggestedByVision); setTargetNote(suggestedNote); - setSpecialAimTarget(suggestedNote.getLocation()); + if (suggestedNote == null) { // No notes on the field! Let's suggest going to the source and hope something turns up. - setTerminatingPoint(new Pose2d(14, 1.2, Rotation2d.fromDegrees(0))); - } else { + setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.NearbySource)); + } + else if (suggestedNote.getAvailability() == Note.NoteAvailability.AgainstObstacle) { + // Take the note's pose2d and extend it in to the super far distance so the robot + // will effectively aim at the "wall" of the obstacle as it approaches the note. + Pose2d superExtendedIntoTheDistancePose = new Pose2d( + new Translation2d( + suggestedNote.getLocation().getTranslation().getX() + Math.cos(suggestedNote.getLocation().getRotation().getRadians()) * 10000000, + suggestedNote.getLocation().getTranslation().getY() + Math.sin(suggestedNote.getLocation().getRotation().getRadians()) * 10000000), + suggestedNote.getLocation().getRotation() + ); + setSpecialAimTarget(superExtendedIntoTheDistancePose); + setTerminatingPoint(suggestedNote.getLocation()); + } + else { setTerminatingPoint(getTargetNote().getLocation()); } + // Publish a route from current position to that location firstRunInNewGoal = false; reevaluationRequested = false; @@ -228,8 +242,11 @@ public void periodic() { aKitLog.record("Current Goal", currentHighLevelGoal); aKitLog.record("Current Note", targetNote == null ? new Pose2d(-100, -100, new Rotation2d(0)) : getTargetNote().getLocation()); - aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose()); - aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber()); + + if (getTerminatingPoint() != null) { + aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose()); + aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber()); + } aKitLog.record("Current SubGoal", currentScoringSubGoal); // Let's show some major obstacles diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index 0e0a4fc8..4f74cc8d 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java @@ -63,33 +63,42 @@ private void setNewInstruction() { // This causes the robot to "point" along the direction of travel for all but the final leg of its travel. // This is useful for passing obstacles, as the robot will present a narrower profile and won't have its // corners collide with things. - logic.setAimAtIntermediateNonFinalLegs(true); - var goalPosition = oracle.getTerminatingPoint(); if (oracle.getHighLevelGoal() == DynamicOracle.HighLevelGoal.CollectNote) { // Since we're going to grab a note, point the front end of our robot towards the goal, // since our collector is on the front side. logic.setDriveBackwards(true); + logic.setAimAtIntermediateNonFinalLegs(true); + logic.setAimAtGoalDuringFinalLeg(true); - if (oracle.targetNote.getAvailability() == Note.NoteAvailability.AgainstObstacle) { - // if a note is against an obstacle, we need to always point at it to avoid rotation thrashing as we - // try to approach it - logic.setEnableSpecialAimTarget(true); - logic.setSpecialAimTarget(oracle.getSpecialAimTarget()); - logic.setAimAtIntermediateNonFinalLegs(false); - } else { - // Note is in the clear, just drive straight at it. + if (oracle.targetNote == null) { + // No note, just driving somewhere where we might find some. logic.setEnableSpecialAimTarget(false); - logic.setAimAtIntermediateNonFinalLegs(true); + logic.setEnableSpecialAimDuringFinalLeg(false); + logic.setAimAtGoalDuringFinalLeg(false); + } else { + if (oracle.targetNote.getAvailability() == Note.NoteAvailability.AgainstObstacle) { + // if a note is against an obstacle, we need to always point at it to avoid rotation thrashing as we + // try to approach it + logic.setEnableSpecialAimTarget(true); + logic.setSpecialAimTarget(oracle.getSpecialAimTarget()); + logic.setAimAtGoalDuringFinalLeg(false); + } else { + // Note is in the clear, just drive straight at it. + logic.setEnableSpecialAimTarget(false); + } + + logic.setEnableSpecialAimDuringFinalLeg(false); + // When approaching the note, make sure to aim straight at the note for the best chance of collection. } - logic.setEnableSpecialAimDuringFinalLeg(false); - // When approaching the note, make sure to aim straight at the note for the best chance of collection. - logic.setAimAtGoalDuringFinalLeg(true); - } else { - // We are doing some kind of score operation. We want to point the back of our robot towards the goal, + // We are doing some kind of score operation. + // Setup general "follow the path" behavior + logic.setAimAtIntermediateNonFinalLegs(true); + logic.setAimAtGoalDuringFinalLeg(true); + // We want to point the back of our robot towards the goal, // since our shooter is on the back side of the robot. logic.setDriveBackwards(false); // This "special aim" mode instructs the robot to aim at a point that isn't the goal point. For example, From f57ca848bb9322c991712c2dd8ec57d479de3d94 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Wed, 28 Feb 2024 23:17:21 -0800 Subject: [PATCH 03/10] Make reservable locations generic --- .../auto_programs/TwoNoteGriefAuto.java | 2 +- .../competition/simulation/Simulator2024.java | 3 +- .../subsystems/oracle/Availability.java | 10 +++ .../subsystems/oracle/DynamicOracle.java | 31 ++++--- .../competition/subsystems/oracle/Note.java | 21 ++--- .../subsystems/oracle/NoteMap.java | 84 +++++++------------ .../subsystems/oracle/ReservableLocation.java | 9 ++ .../oracle/ReservableLocationMap.java | 48 +++++++++++ .../subsystems/oracle/ScoringLocation.java | 36 ++++++++ .../subsystems/oracle/ScoringLocationMap.java | 48 +++++++++++ .../SwerveAccordingToOracleCommand.java | 2 +- .../subsystems/pose/PoseSubsystem.java | 5 ++ 12 files changed, 216 insertions(+), 83 deletions(-) create mode 100644 src/main/java/competition/subsystems/oracle/Availability.java create mode 100644 src/main/java/competition/subsystems/oracle/ReservableLocation.java create mode 100644 src/main/java/competition/subsystems/oracle/ReservableLocationMap.java create mode 100644 src/main/java/competition/subsystems/oracle/ScoringLocation.java create mode 100644 src/main/java/competition/subsystems/oracle/ScoringLocationMap.java diff --git a/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java b/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java index 86e21fd1..87fb8a77 100644 --- a/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java +++ b/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java @@ -30,7 +30,7 @@ import javax.inject.Provider; import java.util.ArrayList; -//am i cookin with this?? 🤫🧏‍♂️🤑 +//am i cookin with this?? //this auto starts in front of the subwoofer //scores the note we are holding, then goes to the center and steals the notes to our side via both the scoocher and the collector //at the end, it grabs the last centerline note and scores it at the subwoofer. diff --git a/src/main/java/competition/simulation/Simulator2024.java b/src/main/java/competition/simulation/Simulator2024.java index 0237617a..c1c461a2 100644 --- a/src/main/java/competition/simulation/Simulator2024.java +++ b/src/main/java/competition/simulation/Simulator2024.java @@ -4,6 +4,7 @@ import competition.subsystems.arm.ArmSubsystem; import competition.subsystems.collector.CollectorSubsystem; import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.oracle.Availability; import competition.subsystems.oracle.DynamicOracle; import competition.subsystems.oracle.Note; import competition.subsystems.pose.PoseSubsystem; @@ -77,7 +78,7 @@ private void visualizeNotes() { private void simulateCollector() { // If we are running the collector and near a note, - var nearestNote = oracle.getNoteMap().getClosestNote(pose.getCurrentPose2d().getTranslation(), 0.1, Note.NoteAvailability.Available, Note.NoteAvailability.AgainstObstacle); + var nearestNote = oracle.getNoteMap().getClosest(pose.getCurrentPose2d().getTranslation(), 0.1, Availability.Available, Availability.AgainstObstacle); if (nearestNote != null && collector.collectorMotor.getAppliedOutput() > 0.05) { // Trigger our sensors to say we have one. ((MockDigitalInput)collector.inControlNoteSensor).setValue(true); diff --git a/src/main/java/competition/subsystems/oracle/Availability.java b/src/main/java/competition/subsystems/oracle/Availability.java new file mode 100644 index 00000000..6e8c7b66 --- /dev/null +++ b/src/main/java/competition/subsystems/oracle/Availability.java @@ -0,0 +1,10 @@ +package competition.subsystems.oracle; + +public enum Availability { + Available, + AgainstObstacle, + ReservedByOthersInAuto, + SuggestedByVision, + SuggestedByDriver, + Unavailable +} diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 058d6940..1923102e 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -75,8 +75,8 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri int noteCount = 1; private void reserveNote(Note.KeyNoteNames specificNote) { - Note reserved = noteMap.getNote(specificNote); - reserved.setAvailability(Note.NoteAvailability.ReservedByOthersInAuto); + Note reserved = noteMap.get(specificNote); + reserved.setAvailability(Availability.ReservedByOthersInAuto); // create an obstacle at the same location. field.addObstacle(new Obstacle(reserved.getLocation().getTranslation().getX(), reserved.getLocation().getTranslation().getY(), 1.25, 1.25, "ReservedNote" + noteCount)); @@ -159,9 +159,20 @@ public void requestReevaluation() { reevaluationRequested = true; } + /** + * Should be called in AutonomousInit + */ + public void freezeConfigurationForAutonomous() { + // + } + @Override public void periodic() { + // Need to spend a moment figuring out what alliance we are on and what state of the game we are in. + // For example, + + switch (currentHighLevelGoal) { case ScoreInAmp: // For now keeping things simple case ScoreInSpeaker: @@ -188,18 +199,18 @@ public void periodic() { case CollectNote: if (firstRunInNewGoal || reevaluationRequested) { // Choose a good note collection location - Note suggestedNote = noteMap.getClosestNote(pose.getCurrentPose2d().getTranslation(), - Note.NoteAvailability.Available, - Note.NoteAvailability.AgainstObstacle, - Note.NoteAvailability.SuggestedByDriver, - Note.NoteAvailability.SuggestedByVision); + Note suggestedNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), + Availability.Available, + Availability.AgainstObstacle, + Availability.SuggestedByDriver, + Availability.SuggestedByVision); setTargetNote(suggestedNote); if (suggestedNote == null) { // No notes on the field! Let's suggest going to the source and hope something turns up. setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.NearbySource)); } - else if (suggestedNote.getAvailability() == Note.NoteAvailability.AgainstObstacle) { + else if (suggestedNote.getAvailability() == Availability.AgainstObstacle) { // Take the note's pose2d and extend it in to the super far distance so the robot // will effectively aim at the "wall" of the obstacle as it approaches the note. Pose2d superExtendedIntoTheDistancePose = new Pose2d( @@ -224,9 +235,9 @@ else if (suggestedNote.getAvailability() == Note.NoteAvailability.AgainstObstacl if (noteCollectionInfoSource.confidentlyHasControlOfNote()) { // Mark the nearest note as being unavailable, if we are anywhere near it - Note nearestNote = noteMap.getClosestNote(pose.getCurrentPose2d().getTranslation(), 1.0); + Note nearestNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), 1.0); if (nearestNote != null) { - nearestNote.setAvailability(Note.NoteAvailability.Unavailable); + nearestNote.setAvailability(Availability.Unavailable); } // Since we have a note, let's go score it. diff --git a/src/main/java/competition/subsystems/oracle/Note.java b/src/main/java/competition/subsystems/oracle/Note.java index ccae39e5..c5a63681 100644 --- a/src/main/java/competition/subsystems/oracle/Note.java +++ b/src/main/java/competition/subsystems/oracle/Note.java @@ -2,16 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; -public class Note { - - public enum NoteAvailability { - Available, - AgainstObstacle, - ReservedByOthersInAuto, - SuggestedByVision, - SuggestedByDriver, - Unavailable - } +public class Note implements ReservableLocation { public enum KeyNoteNames{ BlueSpikeTop, @@ -29,17 +20,17 @@ public enum KeyNoteNames{ private int priority; - private NoteAvailability availability; + private Availability availability; private Pose2d location; public Note(Pose2d location) { this.priority = -1; - this.availability = NoteAvailability.Available; + this.availability = Availability.Available; this.location = location; } - public Note(Pose2d location, int priority, NoteAvailability availability) { + public Note(Pose2d location, int priority, Availability availability) { this.priority = priority; this.availability = availability; this.location = location; @@ -49,7 +40,7 @@ public int getPriority() { return priority; } - public NoteAvailability getAvailability() { + public Availability getAvailability() { return availability; } @@ -61,7 +52,7 @@ public void setPriority(int priority) { this.priority = priority; } - public void setAvailability(NoteAvailability availability) { + public void setAvailability(Availability availability) { this.availability = availability; } diff --git a/src/main/java/competition/subsystems/oracle/NoteMap.java b/src/main/java/competition/subsystems/oracle/NoteMap.java index 253d9c37..936775dd 100644 --- a/src/main/java/competition/subsystems/oracle/NoteMap.java +++ b/src/main/java/competition/subsystems/oracle/NoteMap.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.DriverStation; import xbot.common.subsystems.pose.BasePoseSubsystem; import javax.inject.Singleton; @@ -12,80 +13,53 @@ import java.util.HashMap; @Singleton -public class NoteMap { - private HashMap internalNoteMap; +public class NoteMap extends ReservableLocationMap { public NoteMap() { - this.internalNoteMap = new HashMap<>(); initializeNotes(); } private void initializeNotes() { - addNote(Note.KeyNoteNames.BlueSpikeTop, new Note(PoseSubsystem.SpikeTop)); - addNote(Note.KeyNoteNames.BlueSpikeMiddle, new Note(PoseSubsystem.SpikeMiddle)); - addNote(Note.KeyNoteNames.BlueSpikeBottom, new Note(PoseSubsystem.SpikeBottom, 1, Note.NoteAvailability.AgainstObstacle)); - addNote(Note.KeyNoteNames.CenterLine1, new Note(PoseSubsystem.CenterLine1)); - addNote(Note.KeyNoteNames.CenterLine2, new Note(PoseSubsystem.CenterLine2)); - addNote(Note.KeyNoteNames.CenterLine3, new Note(PoseSubsystem.CenterLine3)); - addNote(Note.KeyNoteNames.CenterLine4, new Note(PoseSubsystem.CenterLine4)); - addNote(Note.KeyNoteNames.CenterLine5, new Note(PoseSubsystem.CenterLine5)); - addNote(Note.KeyNoteNames.RedSpikeTop, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeTop))); - addNote(Note.KeyNoteNames.RedSpikeMiddle, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeMiddle))); - addNote(Note.KeyNoteNames.RedSpikeBottom, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeBottom),1, Note.NoteAvailability.AgainstObstacle)); + add(Note.KeyNoteNames.BlueSpikeTop, new Note(PoseSubsystem.SpikeTop)); + add(Note.KeyNoteNames.BlueSpikeMiddle, new Note(PoseSubsystem.SpikeMiddle)); + add(Note.KeyNoteNames.BlueSpikeBottom, new Note(PoseSubsystem.SpikeBottom, 1, Availability.AgainstObstacle)); + add(Note.KeyNoteNames.CenterLine1, new Note(PoseSubsystem.CenterLine1)); + add(Note.KeyNoteNames.CenterLine2, new Note(PoseSubsystem.CenterLine2)); + add(Note.KeyNoteNames.CenterLine3, new Note(PoseSubsystem.CenterLine3)); + add(Note.KeyNoteNames.CenterLine4, new Note(PoseSubsystem.CenterLine4)); + add(Note.KeyNoteNames.CenterLine5, new Note(PoseSubsystem.CenterLine5)); + add(Note.KeyNoteNames.RedSpikeTop, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeTop))); + add(Note.KeyNoteNames.RedSpikeMiddle, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeMiddle))); + add(Note.KeyNoteNames.RedSpikeBottom, new Note(BasePoseSubsystem.convertBluetoRed(PoseSubsystem.SpikeBottom),1, Availability.AgainstObstacle)); } - public void addNote(String key, Note note) { - this.internalNoteMap.put(key.toString(), note); - } - - private void addNote(Note.KeyNoteNames key, Note note) { - this.internalNoteMap.put(key.toString(), note); - } - - public Note getNote(String key) { - return this.internalNoteMap.get(key); - } - - public Note getNote(Note.KeyNoteNames key) { - return this.internalNoteMap.get(key.toString()); - } - - public boolean removeNote(String key) { - if (this.internalNoteMap.containsKey(key)) { - this.internalNoteMap.remove(key); - return true; + public void markAllianceNotesAsUnavailable(DriverStation.Alliance alliance) { + if (alliance == DriverStation.Alliance.Red) { + get(Note.KeyNoteNames.RedSpikeTop).setAvailability(Availability.Unavailable); + get(Note.KeyNoteNames.RedSpikeMiddle).setAvailability(Availability.Unavailable); + get(Note.KeyNoteNames.RedSpikeBottom).setAvailability(Availability.Unavailable); + } else { + get(Note.KeyNoteNames.BlueSpikeTop).setAvailability(Availability.Unavailable); + get(Note.KeyNoteNames.BlueSpikeMiddle).setAvailability(Availability.Unavailable); + get(Note.KeyNoteNames.BlueSpikeBottom).setAvailability(Availability.Unavailable); } - return false; } - public Note getClosestNote(Translation2d point, double withinDistanceMeters, Note.NoteAvailability... availabilities) { - Note closestNote = null; - double closestDistance = Double.MAX_VALUE; - - for (Note note : this.internalNoteMap.values()) { - if (availabilities.length == 0 || Arrays.asList(availabilities).contains(note.getAvailability())) { - double distance = note.getLocation().getTranslation().getDistance(point); - if (distance < closestDistance && distance < withinDistanceMeters) { - closestDistance = distance; - closestNote = note; - } - } - } - - return closestNote; + private void add(Note.KeyNoteNames key, Note note) { + add(key.toString(), note); } - public Note getClosestNote(Translation2d point, Note.NoteAvailability... availabilities) { - return getClosestNote(point, Double.MAX_VALUE, availabilities); + public Note get(Note.KeyNoteNames key) { + return get(key.toString()); } public Pose3d[] getAllKnownNotes() { - Pose3d[] notes = new Pose3d[this.internalNoteMap.size()]; + Pose3d[] notes = new Pose3d[internalMap.size()]; int i = 0; - for (Note note : this.internalNoteMap.values()) { + for (Note note : this.internalMap.values()) { double noteZ = 0.025; - if (note.getAvailability() != Note.NoteAvailability.Available && note.getAvailability() != Note.NoteAvailability.AgainstObstacle) { + if (note.getAvailability() != Availability.Available && note.getAvailability() != Availability.AgainstObstacle) { noteZ = -3.0; } diff --git a/src/main/java/competition/subsystems/oracle/ReservableLocation.java b/src/main/java/competition/subsystems/oracle/ReservableLocation.java new file mode 100644 index 00000000..8499b2ab --- /dev/null +++ b/src/main/java/competition/subsystems/oracle/ReservableLocation.java @@ -0,0 +1,9 @@ +package competition.subsystems.oracle; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface ReservableLocation { + public Pose2d getLocation(); + public Availability getAvailability(); + public void setAvailability(Availability availability); +} diff --git a/src/main/java/competition/subsystems/oracle/ReservableLocationMap.java b/src/main/java/competition/subsystems/oracle/ReservableLocationMap.java new file mode 100644 index 00000000..b6f36903 --- /dev/null +++ b/src/main/java/competition/subsystems/oracle/ReservableLocationMap.java @@ -0,0 +1,48 @@ +package competition.subsystems.oracle; + +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.Arrays; +import java.util.HashMap; + +public class ReservableLocationMap { + + protected HashMap internalMap; + + public ReservableLocationMap() { + this.internalMap = new HashMap<>(); + } + + public void add(String key, T location) { + this.internalMap.put(key, location); + } + + public T get(String key) { + return this.internalMap.get(key); + } + + public void remove(String key) { + this.internalMap.remove(key); + } + + public T getClosest(Translation2d point, double withinDistanceMeters, Availability... availabilities) { + T closestLocation = null; + double closestDistance = Double.MAX_VALUE; + + for (T reservableLocation : this.internalMap.values()) { + if (availabilities.length == 0 || Arrays.asList(availabilities).contains(reservableLocation.getAvailability())) { + double distance = reservableLocation.getLocation().getTranslation().getDistance(point); + if (distance < closestDistance && distance < withinDistanceMeters) { + closestDistance = distance; + closestLocation = reservableLocation; + } + } + } + + return closestLocation; + } + + public T getClosest(Translation2d point, Availability... availabilities) { + return getClosest(point, Double.MAX_VALUE, availabilities); + } +} diff --git a/src/main/java/competition/subsystems/oracle/ScoringLocation.java b/src/main/java/competition/subsystems/oracle/ScoringLocation.java new file mode 100644 index 00000000..539ef681 --- /dev/null +++ b/src/main/java/competition/subsystems/oracle/ScoringLocation.java @@ -0,0 +1,36 @@ +package competition.subsystems.oracle; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; + +public class ScoringLocation implements ReservableLocation { + + public enum WellKnownScoringLocations { + SubwooferTopBlue, + SubwooferMiddleBlue, + SubwooferBottomBlue, + SubwooferTopRed, + SubwooferMiddleRed, + SubwooferBottomRed + } + + private Pose2d location; + private Availability availability; + + public ScoringLocation(Pose2d location, Availability availability) { + this.location = location; + this.availability = availability; + } + + public Pose2d getLocation() { + return location; + } + + public Availability getAvailability() { + return availability; + } + + public void setAvailability(Availability availability) { + this.availability = availability; + } +} diff --git a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java new file mode 100644 index 00000000..29f47610 --- /dev/null +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -0,0 +1,48 @@ +package competition.subsystems.oracle; + +import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.wpilibj.DriverStation; + +public class ScoringLocationMap extends ReservableLocationMap { + + public ScoringLocationMap() { + initializeScoringLocations(); + } + + private void initializeScoringLocations() { + initializeBlueScoringLocations(); + initializeRedScoringLocations(); + } + + private void initializeBlueScoringLocations() { + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue, new ScoringLocation(PoseSubsystem.SubwooferTopScoringLocation, Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, Availability.AgainstObstacle)); + } + + private void initializeRedScoringLocations() { + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferCentralScoringLocation), Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferTopRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferTopScoringLocation), Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.AgainstObstacle)); + } + + public void markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance alliance) { + if (alliance == DriverStation.Alliance.Red) { + get(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed).setAvailability(Availability.Unavailable); + get(ScoringLocation.WellKnownScoringLocations.SubwooferTopRed).setAvailability(Availability.Unavailable); + get(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed).setAvailability(Availability.Unavailable); + } else { + get(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue).setAvailability(Availability.Unavailable); + get(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue).setAvailability(Availability.Unavailable); + get(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue).setAvailability(Availability.Unavailable); + } + } + + private void add(ScoringLocation.WellKnownScoringLocations key, ScoringLocation location) { + add(key.toString(), location); + } + + public ScoringLocation get(ScoringLocation.WellKnownScoringLocations key) { + return get(key.toString()); + } +} diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index 4f74cc8d..887504e7 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java @@ -78,7 +78,7 @@ private void setNewInstruction() { logic.setEnableSpecialAimDuringFinalLeg(false); logic.setAimAtGoalDuringFinalLeg(false); } else { - if (oracle.targetNote.getAvailability() == Note.NoteAvailability.AgainstObstacle) { + if (oracle.targetNote.getAvailability() == Availability.AgainstObstacle) { // if a note is against an obstacle, we need to always point at it to avoid rotation thrashing as we // try to approach it logic.setEnableSpecialAimTarget(true); diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 3c65efa4..75df1b11 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -77,7 +77,12 @@ public class PoseSubsystem extends BasePoseSubsystem { public static Translation2d BlueSubwoofer = new Translation2d(0.415, 5.57); public static Pose2d AmpScoringLocation = new Pose2d(1.83, 7.71, Rotation2d.fromDegrees(90)); + + // TODO: get good positions + public static Pose2d SubwooferTopScoringLocation = new Pose2d(1.3, 6.9, Rotation2d.fromDegrees(180)); public static Pose2d SubwooferCentralScoringLocation = new Pose2d(1.41, 5.54, Rotation2d.fromDegrees(180)); + public static Pose2d SubwooferBottomScoringLocation = new Pose2d(0.9, 4.3, Rotation2d.fromDegrees(180)); + private DoubleProperty matchTime; From 9937bf0e323b12936a9c4e451c5bc2af61d466cc Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Wed, 28 Feb 2024 23:39:16 -0800 Subject: [PATCH 04/10] Bugfixes, position cleanup --- .../subsystems/oracle/DynamicOracle.java | 20 ++++++++++--------- .../subsystems/oracle/ScoringLocationMap.java | 4 ++-- .../subsystems/pose/PoseSubsystem.java | 4 ++-- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 1923102e..863e9dad 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj.DriverStation; import xbot.common.command.BaseSubsystem; import xbot.common.subsystems.pose.BasePoseSubsystem; import xbot.common.trajectory.LowResField; @@ -34,6 +35,7 @@ public enum ScoringSubGoals { NoteCollectionInfoSource noteCollectionInfoSource; NoteFiringInfoSource noteFiringInfoSource; NoteMap noteMap; + ScoringLocationMap scoringLocationMap; LowResField field; HighLevelGoal currentHighLevelGoal; @@ -51,6 +53,12 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri this.noteCollectionInfoSource = noteCollectionInfoSource; this.noteFiringInfoSource = noteFiringInfoSource; this.noteMap = new NoteMap(); + this.scoringLocationMap = new ScoringLocationMap(); + + // TODO: adjust this during autonomous init + noteMap.markAllianceNotesAsUnavailable(DriverStation.Alliance.Red); + scoringLocationMap.markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance.Red); + this.pose = pose; this.arm = arm; @@ -61,14 +69,6 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri //reserveNote(Note.KeyNoteNames.BlueSpikeMiddle); //reserveNote(Note.KeyNoteNames.BlueSpikeBottom); - - Pose2d scoringPositionTop = new Pose2d(1.3, 6.9, Rotation2d.fromDegrees(180)); - Pose2d scoringPositionMiddle = new Pose2d(1.5, 5.5, Rotation2d.fromDegrees(180)); - Pose2d scoringPositionBottom = new Pose2d(0.9, 4.3, Rotation2d.fromDegrees(180)); - - activeScoringPosition = scoringPositionMiddle; - //createRobotObstacle(scoringPositionMiddle.getTranslation(), 1.75, "PartnerA"); - //createRobotObstacle(scoringPositionBottom.getTranslation(), 1.75, "PartnerB"); } Pose2d activeScoringPosition; @@ -178,7 +178,9 @@ public void periodic() { case ScoreInSpeaker: if (firstRunInNewGoal || reevaluationRequested) { setTargetNote(null); - setTerminatingPoint(activeScoringPosition); + setTerminatingPoint(scoringLocationMap.getClosest(pose.getCurrentPose2d().getTranslation(), + Availability.Available).getLocation()); + currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; setSpecialAimTarget(new Pose2d(0, 5.5, Rotation2d.fromDegrees(0))); // Choose a good speaker scoring location diff --git a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java index 29f47610..bdae644e 100644 --- a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -17,13 +17,13 @@ private void initializeScoringLocations() { private void initializeBlueScoringLocations() { add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue, new ScoringLocation(PoseSubsystem.SubwooferTopScoringLocation, Availability.Available)); - add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, Availability.AgainstObstacle)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, Availability.Available)); } private void initializeRedScoringLocations() { add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferCentralScoringLocation), Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferTopRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferTopScoringLocation), Availability.Available)); - add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.AgainstObstacle)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.Available)); } public void markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance alliance) { diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 75df1b11..29976228 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -79,9 +79,9 @@ public class PoseSubsystem extends BasePoseSubsystem { public static Pose2d AmpScoringLocation = new Pose2d(1.83, 7.71, Rotation2d.fromDegrees(90)); // TODO: get good positions - public static Pose2d SubwooferTopScoringLocation = new Pose2d(1.3, 6.9, Rotation2d.fromDegrees(180)); + public static Pose2d SubwooferTopScoringLocation = new Pose2d(0.77, 6.8, Rotation2d.fromDegrees(180)); public static Pose2d SubwooferCentralScoringLocation = new Pose2d(1.41, 5.54, Rotation2d.fromDegrees(180)); - public static Pose2d SubwooferBottomScoringLocation = new Pose2d(0.9, 4.3, Rotation2d.fromDegrees(180)); + public static Pose2d SubwooferBottomScoringLocation = new Pose2d(0.77, 4.32, Rotation2d.fromDegrees(180)); private DoubleProperty matchTime; From f30cda0706ca5928f511c4e0f7d891012f235f82 Mon Sep 17 00:00:00 2001 From: Stephen Just Date: Thu, 29 Feb 2024 19:16:20 -0800 Subject: [PATCH 05/10] Appease checkstyle --- .../subsystems/oracle/DynamicOracle.java | 6 ++++-- .../subsystems/oracle/ScoringLocationMap.java | 18 ++++++++++++------ 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 863e9dad..ddd4ca61 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -217,8 +217,10 @@ else if (suggestedNote.getAvailability() == Availability.AgainstObstacle) { // will effectively aim at the "wall" of the obstacle as it approaches the note. Pose2d superExtendedIntoTheDistancePose = new Pose2d( new Translation2d( - suggestedNote.getLocation().getTranslation().getX() + Math.cos(suggestedNote.getLocation().getRotation().getRadians()) * 10000000, - suggestedNote.getLocation().getTranslation().getY() + Math.sin(suggestedNote.getLocation().getRotation().getRadians()) * 10000000), + suggestedNote.getLocation().getTranslation().getX() + + Math.cos(suggestedNote.getLocation().getRotation().getRadians()) * 10000000, + suggestedNote.getLocation().getTranslation().getY() + + Math.sin(suggestedNote.getLocation().getRotation().getRadians()) * 10000000), suggestedNote.getLocation().getRotation() ); setSpecialAimTarget(superExtendedIntoTheDistancePose); diff --git a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java index bdae644e..3394e0e1 100644 --- a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -15,15 +15,21 @@ private void initializeScoringLocations() { } private void initializeBlueScoringLocations() { - add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); - add(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue, new ScoringLocation(PoseSubsystem.SubwooferTopScoringLocation, Availability.Available)); - add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, + new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue, + new ScoringLocation(PoseSubsystem.SubwooferTopScoringLocation, Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, + new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, Availability.Available)); } private void initializeRedScoringLocations() { - add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferCentralScoringLocation), Availability.Available)); - add(ScoringLocation.WellKnownScoringLocations.SubwooferTopRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferTopScoringLocation), Availability.Available)); - add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed, + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferCentralScoringLocation), Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferTopRed, + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferTopScoringLocation), Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.Available)); } public void markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance alliance) { From 5f0f48a1201a4d544134e52eec3b97570f1d4324 Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Thu, 29 Feb 2024 21:11:07 -0800 Subject: [PATCH 06/10] Attempt to get some consistency --- src/main/java/competition/subsystems/arm/ArmSubsystem.java | 3 ++- src/main/java/competition/subsystems/oracle/DynamicOracle.java | 2 +- src/main/java/competition/subsystems/pose/PoseSubsystem.java | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 8802cddd..cc500312 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -83,7 +83,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF private int loopsWhereCompressorRunning = 0; private static double[] experimentalRangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136}; - private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 20.0, 26, 41, 57, 64}; + //private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 20.0, 26, 41, 57, 64}; + private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 0.0, 0, 0, 0, 0}; public enum ArmState { EXTENDING, diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index ddd4ca61..4cf98691 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -335,7 +335,7 @@ private void determineScoringSubgoal() { double acceptableRangeBeforeScoringMeters = 0; boolean inUnderstoodRange = false; if (currentHighLevelGoal == HighLevelGoal.ScoreInSpeaker) { - acceptableRangeBeforeScoringMeters = 2; + acceptableRangeBeforeScoringMeters = 0.2; inUnderstoodRange = pose.getDistanceFromSpeaker() < arm.getMaximumRangeForAnyShot(); } else if (currentHighLevelGoal == HighLevelGoal.ScoreInAmp) { diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 29976228..d6ab6b06 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -185,7 +185,7 @@ protected void updateOdometry() { ); if (isUsingVisionAssistedPose()) { - improveFusedOdometryUsingPhotonLib(updatedPosition); + //improveFusedOdometryUsingPhotonLib(updatedPosition); } aKitLog.record("VisionEstimate", fusedSwerveOdometry.getEstimatedPosition()); From c0656f2382c428a6157600f3f89e7e3c18b7979c Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 16:18:45 -0800 Subject: [PATCH 07/10] Newer CL --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 579b8e42..3e64aa20 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 579b8e42a7f2d8ea95f7223388125dea2cb896cb +Subproject commit 3e64aa2052572228d85af58e4467197b3d84aaa2 From 855b0301ae82afb22f480b24a09a4d67220bae02 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 16:31:00 -0800 Subject: [PATCH 08/10] Added comments, small cleanup --- .../java/competition/subsystems/arm/ArmSubsystem.java | 10 ++++++++-- .../competition/subsystems/oracle/DynamicOracle.java | 2 +- .../competition/subsystems/vision/VisionSubsystem.java | 2 -- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index cc500312..de912f2c 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -27,7 +27,6 @@ import javax.inject.Inject; import javax.inject.Singleton; -import java.util.Arrays; @Singleton public class ArmSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { @@ -84,6 +83,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF private static double[] experimentalRangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136}; //private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 20.0, 26, 41, 57, 64}; + // TODO: For now, this was a very ugly and quick way to force the arm low, given that all our scoring is coming + // from the subwoofer. This will need to be revisited. private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 0.0, 0, 0, 0, 0}; public enum ArmState { @@ -648,7 +649,12 @@ protected boolean areTwoTargetsEquivalent(Double target1, Double target2) { return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2, 1); } - public double getMaximumRangeForAnyShot() { + /** + * Returns our maximum scoring range. Is useful if the arm is "at target" + * but we know there's no way it will actually score. + * @return the maximum scoring range in meters + */ + public double getMaximumRangeForAnyShotMeters() { return experimentalRangesInInches[experimentalRangesInInches.length - 1] / PoseSubsystem.INCHES_IN_A_METER; } diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 4cf98691..142406a3 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -336,7 +336,7 @@ private void determineScoringSubgoal() { boolean inUnderstoodRange = false; if (currentHighLevelGoal == HighLevelGoal.ScoreInSpeaker) { acceptableRangeBeforeScoringMeters = 0.2; - inUnderstoodRange = pose.getDistanceFromSpeaker() < arm.getMaximumRangeForAnyShot(); + inUnderstoodRange = pose.getDistanceFromSpeaker() < arm.getMaximumRangeForAnyShotMeters(); } else if (currentHighLevelGoal == HighLevelGoal.ScoreInAmp) { // in the future we'll do something more like "get near amp, then drive into the wall for a few moments diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java index fee07e9a..fa0b8e10 100644 --- a/src/main/java/competition/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java @@ -66,8 +66,6 @@ public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalC try { aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - double width = aprilTagFieldLayout.getFieldWidth(); - double height = aprilTagFieldLayout.getFieldLength(); aprilTagsLoaded = true; log.info("Successfully loaded AprilTagFieldLayout"); } catch (IOException e) { From 36ff4c5fe307fb7633465886dab9ad6d6d2551d5 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 16:49:25 -0800 Subject: [PATCH 09/10] Move "commit to firing" to the Collector, where it makes more sense. --- .../collector/CollectorSubsystem.java | 25 +++++++++++++++++++ ...uperstructureAccordingToOracleCommand.java | 11 ++------ 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java index 0ef8afd9..2c1233e7 100644 --- a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java +++ b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java @@ -69,6 +69,10 @@ public IntakeState getIntakeState(){ return intakeState; } public void intake(){ + if (shouldCommitToFiring()){ + return; + } + double power = intakePower.get(); if (getGamePieceInControl()) { power *= intakePowerInControlMultiplier.get(); @@ -80,10 +84,22 @@ public void intake(){ intakeState = IntakeState.INTAKING; } public void eject(){ + if (shouldCommitToFiring()){ + return; + } + setPower(ejectPower.get()); intakeState = IntakeState.EJECTING; } public void stop(){ + if (shouldCommitToFiring()){ + return; + } + setPower(0); + intakeState = IntakeState.STOPPED; + } + + public void emergencyStopBypassingJammingPrevention() { setPower(0); intakeState = IntakeState.STOPPED; } @@ -134,6 +150,15 @@ public boolean confidentlyHasFiredNote() { return getSecondsSinceFiringBegan() > waitTimeAfterFiring.get(); } + /** + * If we start firing for any reason, we have to commit to running the intake at full power + * for a few moments to avoid jamming the system. + * @return Returns true if we have started, but not finished, the firing process. + */ + public boolean shouldCommitToFiring() { + return intakeState == IntakeState.FIRING && !confidentlyHasFiredNote(); + } + @Override public void periodic() { if (contract.isCollectorReady()) { diff --git a/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java index 3128476a..c5bf13d6 100644 --- a/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java @@ -58,6 +58,7 @@ public void execute() { } if (tryToScore) { + // If it's time to score, or if we're already started firing, commit! (Or should this live in the collector?) if (oracle.getScoringSubgoal() == DynamicOracle.ScoringSubGoals.EarnestlyLaunchNote) { fireWhenReady(); } else { @@ -73,24 +74,16 @@ private void fireWhenReady() { && shooter.isMaintainerAtGoal() && shooter.getTargetValue().upperWheelsTargetRPM > 500; - boolean shouldCommitToFiring = getShouldCommitToFiring(); - boolean sanityChecks = oracle.getHighLevelGoal() != DynamicOracle.HighLevelGoal.CollectNote; aKitLog.record("SuperstructureReady", superStructureReady); - aKitLog.record("ShouldCommitToFiring", shouldCommitToFiring); aKitLog.record("SanityChecks", sanityChecks); - if ((superStructureReady && sanityChecks) || shouldCommitToFiring) { + if (superStructureReady && sanityChecks) { collector.fire(); } else { collector.stop(); } } - - // Once we've started shooting, don't stop until we're sure the note is totally gone. - private boolean getShouldCommitToFiring() { - return collector.getIntakeState() == CollectorSubsystem.IntakeState.FIRING; - } } From 2c093560f9dcff485bd3eaf990738dbe9c19ea7a Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 17:18:02 -0800 Subject: [PATCH 10/10] PR feedback --- SeriouslyCommonLib | 2 +- .../java/competition/subsystems/arm/ArmSubsystem.java | 7 +++++-- .../competition/subsystems/oracle/DynamicOracle.java | 5 +++-- .../competition/subsystems/pose/PoseSubsystem.java | 10 +++++----- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 3e64aa20..fd5d2a28 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 3e64aa2052572228d85af58e4467197b3d84aaa2 +Subproject commit fd5d2a286b21633db0dc12b72020d00ff76a1df8 diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index de912f2c..9e286bdf 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -81,11 +81,14 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF private int totalLoops = 0; private int loopsWhereCompressorRunning = 0; - private static double[] experimentalRangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136}; + //private static double[] experimentalRangesInInches = new double[]{0, 36, 49.5, 63, 80, 111, 136}; //private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 20.0, 26, 41, 57, 64}; // TODO: For now, this was a very ugly and quick way to force the arm low, given that all our scoring is coming // from the subwoofer. This will need to be revisited. - private static double[] experimentalArmExtensionsInMm = new double[]{0, 0, 0.0, 0, 0, 0, 0}; + private static double[] experimentalRangesInInches = new double[]{0, 63}; + private static double[] experimentalArmExtensionsInMm = new double[]{0, 0}; + + public enum ArmState { EXTENDING, diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 142406a3..6ab4a6ac 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -160,10 +160,11 @@ public void requestReevaluation() { } /** - * Should be called in AutonomousInit + * Should be called in AutonomousInit. + * Idea: the operator will use the NeoTrellis to lay out a plan, and then either press a button to + * lock in that plan (or make updates to the plan), or the plan will automatically lock in at the start of auto. */ public void freezeConfigurationForAutonomous() { - // } @Override diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index d6ab6b06..c727acb5 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -58,11 +58,11 @@ public class PoseSubsystem extends BasePoseSubsystem { public static Pose2d SpikeTop = new Pose2d(2.8956, 7.0012, new Rotation2d()); public static Pose2d SpikeMiddle = new Pose2d(2.8956, 5.5478, new Rotation2d()); public static Pose2d SpikeBottom = new Pose2d(2.8956, 4.1056, new Rotation2d()); - public static Pose2d CenterLine1 = new Pose2d(8.2705, 7.4584, new Rotation2d()); - public static Pose2d CenterLine2 = new Pose2d(8.2705, 5.7820, new Rotation2d()); - public static Pose2d CenterLine3 = new Pose2d(8.2705, 4.1056, new Rotation2d()); - public static Pose2d CenterLine4 = new Pose2d(8.2705, 2.4292, new Rotation2d()); - public static Pose2d CenterLine5 = new Pose2d(8.2705, 0.7528, new Rotation2d()); + public static Pose2d CenterLine1 = new Pose2d(fieldXMidpointInMeters, 7.4584, new Rotation2d()); + public static Pose2d CenterLine2 = new Pose2d(fieldXMidpointInMeters, 5.7820, new Rotation2d()); + public static Pose2d CenterLine3 = new Pose2d(fieldXMidpointInMeters, 4.1056, new Rotation2d()); + public static Pose2d CenterLine4 = new Pose2d(fieldXMidpointInMeters, 2.4292, new Rotation2d()); + public static Pose2d CenterLine5 = new Pose2d(fieldXMidpointInMeters, 0.7528, new Rotation2d()); public static Pose2d NearbySource = new Pose2d(14, 1.2, Rotation2d.fromDegrees(0));