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..fd5d2a28 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 979e6d3690d3140a7086d7ae8655dba58d561644 +Subproject commit fd5d2a286b21633db0dc12b72020d00ff76a1df8 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 4e9426a6..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); + 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); @@ -187,18 +188,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..9e286bdf 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -81,6 +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[] 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[] experimentalRangesInInches = new double[]{0, 63}; + private static double[] experimentalArmExtensionsInMm = new double[]{0, 0}; + + public enum ArmState { EXTENDING, @@ -202,19 +210,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 +649,16 @@ public double getRecommendedExtensionForSpeaker() { @Override protected boolean areTwoTargetsEquivalent(Double target1, Double target2) { - return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2); + return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2, 1); + } + + /** + * 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; } public void periodic() { 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/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 72afa9c3..6ab4a6ac 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -1,10 +1,12 @@ 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; 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; @@ -25,6 +27,7 @@ public enum HighLevelGoal { public enum ScoringSubGoals { IngestNote, + IngestNoteAgainstObstacle, MoveToScoringRange, EarnestlyLaunchNote } @@ -32,6 +35,7 @@ public enum ScoringSubGoals { NoteCollectionInfoSource noteCollectionInfoSource; NoteFiringInfoSource noteFiringInfoSource; NoteMap noteMap; + ScoringLocationMap scoringLocationMap; LowResField field; HighLevelGoal currentHighLevelGoal; @@ -39,40 +43,40 @@ 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.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; this.currentHighLevelGoal = HighLevelGoal.CollectNote; - this.currentScoringSubGoal = ScoringSubGoals.IngestNote; + this.currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; firstRunInNewGoal = true; setupLowResField(); //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; 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)); @@ -155,15 +159,29 @@ public void requestReevaluation() { reevaluationRequested = true; } + /** + * 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 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: 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 @@ -184,18 +202,35 @@ 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.SuggestedByDriver, - Note.NoteAvailability.SuggestedByVision); + Note suggestedNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), + Availability.Available, + Availability.AgainstObstacle, + Availability.SuggestedByDriver, + Availability.SuggestedByVision); setTargetNote(suggestedNote); - setSpecialAimTarget(null); + 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() == 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( + 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; @@ -205,9 +240,9 @@ public void periodic() { 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. @@ -223,8 +258,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 @@ -296,15 +334,18 @@ public ScoringSubGoals getScoringSubgoal() { private void determineScoringSubgoal() { double acceptableRangeBeforeScoringMeters = 0; + boolean inUnderstoodRange = false; if (currentHighLevelGoal == HighLevelGoal.ScoreInSpeaker) { - acceptableRangeBeforeScoringMeters = 2; + acceptableRangeBeforeScoringMeters = 0.2; + 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 // 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..c5a63681 100644 --- a/src/main/java/competition/subsystems/oracle/Note.java +++ b/src/main/java/competition/subsystems/oracle/Note.java @@ -2,15 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; -public class Note { - - public enum NoteAvailability { - Available, - ReservedByOthersInAuto, - SuggestedByVision, - SuggestedByDriver, - Unavailable - } +public class Note implements ReservableLocation { public enum KeyNoteNames{ BlueSpikeTop, @@ -28,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; @@ -48,7 +40,7 @@ public int getPriority() { return priority; } - public NoteAvailability getAvailability() { + public Availability getAvailability() { return availability; } @@ -60,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 6311023e..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)); - 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))); + 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) { + 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..3394e0e1 --- /dev/null +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -0,0 +1,54 @@ +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.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)); + } + + 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/SuperstructureAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SuperstructureAccordingToOracleCommand.java index 018f2806..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 && shouldCommitToFiring && sanityChecks) { + if (superStructureReady && sanityChecks) { collector.fire(); } else { collector.stop(); } } - - private boolean getShouldCommitToFiring() { - return collector.getIntakeState() == CollectorSubsystem.IntakeState.FIRING - || (shooter.isMaintainerAtGoal() && arm.isMaintainerAtGoal()); - } } diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index 77cb45bd..887504e7 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java @@ -63,27 +63,48 @@ 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.setEnableSpecialAimTarget(false); - - // When approaching the note, make sure to aim straight at the note for the best chance of collection. + logic.setAimAtIntermediateNonFinalLegs(true); logic.setAimAtGoalDuringFinalLeg(true); + if (oracle.targetNote == null) { + // No note, just driving somewhere where we might find some. + logic.setEnableSpecialAimTarget(false); + logic.setEnableSpecialAimDuringFinalLeg(false); + logic.setAimAtGoalDuringFinalLeg(false); + } else { + 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); + 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. + } + } 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, // 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..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.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(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)); @@ -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(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.77, 4.32, Rotation2d.fromDegrees(180)); + private DoubleProperty matchTime; @@ -180,7 +185,7 @@ protected void updateOdometry() { ); if (isUsingVisionAssistedPose()) { - improveFusedOdometryUsingPhotonLib(updatedPosition); + //improveFusedOdometryUsingPhotonLib(updatedPosition); } aKitLog.record("VisionEstimate", fusedSwerveOdometry.getEstimatedPosition()); 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() {