Adds seeking behavior to oracle and Grief Auto (#278)

* GriefMiddle on "third" auto button

* Actually bind grief to button, improve drive path slightly

* Tentative oracle upgrades for searching

* Last stuff from Sammamssh

* Use newer SCL

src/main/java/competition/auto_programs/GriefMiddle.java

@@ -0,0 +1,122 @@
+package competition.auto_programs;
+
+import competition.commandgroups.FireFromSubwooferCommandGroup;
+import competition.subsystems.collector.commands.EjectCollectorCommand;
+import competition.subsystems.drive.DriveSubsystem;
+import competition.subsystems.pose.PoseSubsystem;
+import competition.subsystems.schoocher.commands.EjectScoocherCommand;
+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.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand;
+import xbot.common.subsystems.pose.BasePoseSubsystem;
+import xbot.common.trajectory.XbotSwervePoint;
+
+import javax.inject.Inject;
+import javax.inject.Provider;
+import java.util.ArrayList;
+import java.util.List;
+
+public class GriefMiddle extends SequentialCommandGroup {
+
+    @Inject
+    public GriefMiddle(
+            Provider<SwerveSimpleTrajectoryCommand> swerveSimpleTrajectoryCommandProvider,
+            Provider<FireFromSubwooferCommandGroup> fireFromSubwooferCommandGroup,
+            EjectCollectorCommand ejectCollector,
+            EjectScoocherCommand ejectScoocher,
+            DriveSubsystem drive,
+            PoseSubsystem pose)
+    {
+        // Start at the bot, maybe?
+
+        var startSpeakerBot = pose.createSetPositionCommand(
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation));
+        this.addCommands(startSpeakerBot);
+
+        // Shoot 1st note
+        var shoot = fireFromSubwooferCommandGroup.get();
+        this.addCommands(shoot);
+
+        // Drive to lower blue line waypoint
+        var driveToLowerWing = swerveSimpleTrajectoryCommandProvider.get();
+        driveToLowerWing.logic.setKeyPointsProvider(this::goToBottomWing);
+        driveToLowerWing.logic.setEnableConstantVelocity(true);
+        driveToLowerWing.logic.setConstantVelocity(drive.getSuggestedAutonomousExtremeSpeed());
+        // Keep driving!
+        driveToLowerWing.logic.setStopWhenFinished(false);
+        driveToLowerWing.logic.setDriveBackwards(true);
+        driveToLowerWing.logic.setAimAtGoalDuringFinalLeg(true);
+
+        this.addCommands(driveToLowerWing);
+
+        // Drive to lowest note pointing north
+        var justDriveToCenter5 = swerveSimpleTrajectoryCommandProvider.get();
+        justDriveToCenter5.logic.setKeyPointsProvider(this::goToCenter5);
+        justDriveToCenter5.logic.setEnableConstantVelocity(true);
+        justDriveToCenter5.logic.setStopWhenFinished(false);
+        justDriveToCenter5.logic.setConstantVelocity(drive.getSuggestedAutonomousExtremeSpeed());
+        justDriveToCenter5.logic.setEnableSpecialAimTarget(true);
+        justDriveToCenter5.logic.setSpecialAimTarget(
+                new Pose2d(
+                        new Translation2d(
+                            BasePoseSubsystem.fieldXMidpointInMeters,
+                                3000000),
+                        Rotation2d.fromDegrees(0)));
+
+        this.addCommands(justDriveToCenter5);
+
+        var driveToCenter1 = swerveSimpleTrajectoryCommandProvider.get();
+        driveToCenter1.logic.setKeyPointsProvider(this::goUpLineToCenter1);
+        driveToCenter1.setConstantRotationPowerSupplier(this::getRotationPower);
+        driveToCenter1.logic.setEnableConstantVelocity(true);
+        driveToCenter1.logic.setConstantVelocity(drive.getSuggestedAutonomousMaximumSpeed());
+
+        this.addCommands(driveToCenter1.alongWith(ejectCollector, ejectScoocher));
+    }
+
+    public List<XbotSwervePoint> goToBottomWing() {
+        var points = new ArrayList<XbotSwervePoint>();
+        points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(PoseSubsystem.BlueBottomWing, 10));
+        return points;
+    }
+
+    double globalXAdjustment = 0.25;
+
+    private Pose2d shiftCenterlineX(Pose2d poseToAdjust) {
+        return new Pose2d(new Translation2d(
+                poseToAdjust.getX() + globalXAdjustment,
+                poseToAdjust.getY()),
+                poseToAdjust.getRotation());
+    }
+
+    public List<XbotSwervePoint> goToCenter5() {
+        var points = new ArrayList<XbotSwervePoint>();
+        var adjusted5 = shiftCenterlineX(PoseSubsystem.CenterLine5);
+        points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(adjusted5, 10));
+        return points;
+    }
+
+    public List<XbotSwervePoint> goUpLineToCenter1() {
+        var points = new ArrayList<XbotSwervePoint>();
+        var adjusted1 = shiftCenterlineX(PoseSubsystem.CenterLine1);
+        points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(adjusted1, 10));
+        return points;
+    }
+
+    public double getRotationPower() {
+        double rotationPower = 0.5;
+        if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue) {
+            // on blue alliance, spin positive
+            return rotationPower;
+        } else{
+            // on red alliance, spin negative
+            return -rotationPower;
+        }
+
+    }
+
+
+}

src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java

@@ -62,8 +62,10 @@ public void execute() {
         switch (noteAcquisitionMode) {
             case BlindApproach:
                 if (!hasDoneVisionCheckYet) {
-                    if (pose.getCurrentPose2d().getTranslation().getDistance(
-                            drive.getTargetNote().getTranslation()) < vision.getBestRangeFromStaticNoteToSearchForNote()) {
+                    double rangeToStaticNote = pose.getCurrentPose2d().getTranslation().getDistance(
+                            drive.getTargetNote().getTranslation());
+                    aKitLog.record("RangeToStaticNote", rangeToStaticNote);
+                    if (rangeToStaticNote < vision.getBestRangeFromStaticNoteToSearchForNote()) {
                         hasDoneVisionCheckYet = true;
                         log.info("Close to static note - attempting vision update.");
                         assignClosestVisionNoteToDriveSubsystemIfYouSeeANoteAndReplanPath();

src/main/java/competition/injection/components/BaseRobotComponent.java

@@ -1,5 +1,6 @@
 package competition.injection.components;
 
+import competition.auto_programs.GriefMiddle;
 import competition.auto_programs.SubwooferShotFromMidShootThenShootNearestThree;
 import competition.auto_programs.TestVisionAuto;
 import competition.simulation.Simulator2024;
@@ -69,4 +70,5 @@ public abstract class BaseRobotComponent extends BaseComponent {
     public abstract SubwooferShotFromMidShootThenShootNearestThree subwooferShotFromMidShootThenShootNearestThree();
     public abstract ListenToOracleCommandGroup listenToOracleCommandGroup();
     public abstract TestVisionAuto testVisionAuto();
+    public abstract GriefMiddle griefMiddle();
 }

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -1,13 +1,15 @@
 package competition.operator_interface;
 
 import competition.auto_programs.DoNothingAuto;
+import competition.auto_programs.GriefMiddle;
 import competition.auto_programs.SixNoteBnbExtended;
 import competition.auto_programs.SubwooferShotFromBotShootThenShootBotSpikeThenShootBotCenter;
 import competition.auto_programs.SubwooferShotFromBotShootThenShootSpikes;
 import competition.auto_programs.SubwooferShotFromBotThenTwoCenterline;
 import competition.auto_programs.SubwooferShotFromMidShootThenShootNearestThree;
 import competition.auto_programs.SubwooferShotFromTopShootThenShootSpikes;
 import competition.auto_programs.SubwooferShotFromTopShootThenShootTopSpikeThenShootTwoCenter;
+import competition.auto_programs.TwoNoteGriefAuto;
 import competition.commandgroups.PrepareToFireAtSpeakerFromPodiumCommand;
 import competition.commandgroups.PrepareToFireNearestGoodScoringPositionCommand;
 import competition.commandgroups.PrepareToLobShotCommand;
@@ -234,6 +236,7 @@ public void setupAutonomousCommandSelection(OperatorInterface oi,
                                                 SubwooferShotFromBotShootThenShootBotSpikeThenShootBotCenter botThenBotSpikeBotCenter,
                                                 SixNoteBnbExtended bnbExtended,
                                                 DoNothingAuto doNothing,
+                                                GriefMiddle grief,
                                                 SubwooferShotFromBotThenTwoCenterline botThenTwoCenter) {
         var setOracleAuto = setAutonomousCommandProvider.get();
         setOracleAuto.setAutoCommand(listenToOracleCommandGroup);
@@ -244,9 +247,9 @@ public void setupAutonomousCommandSelection(OperatorInterface oi,
         oi.neoTrellis.getifAvailable(23).onTrue(setMidThenThree);
         setMidThenThree.includeOnSmartDashboard("Standard 4 Note Auto");
 
-        var setTopThenThree = setAutonomousCommandProvider.get();
-        setTopThenThree.setAutoCommand(topThenThree);
-        oi.neoTrellis.getifAvailable(15).onTrue(setTopThenThree);
+        var setGrief = setAutonomousCommandProvider.get();
+        setGrief.setAutoCommand(grief);
+        oi.neoTrellis.getifAvailable(15).onTrue(setGrief);
 
         var setBotThenThree = setAutonomousCommandProvider.get();
         setBotThenThree.setAutoCommand(botThenThree);

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -498,7 +498,7 @@ public double getUsefulArmPositionExtensionInMm(UsefulArmPosition usefulArmPosit
                 extension = 55;
                 break;
             case HANG_APPROACH:
-                extension = 100;
+                extension = 120;
                 break;
             default:
                 return 0;

src/main/java/competition/subsystems/oracle/DynamicOracle.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import xbot.common.advantage.AKitLogger;
 import xbot.common.command.BaseSubsystem;
+import xbot.common.controls.sensors.XTimer;
 import xbot.common.properties.BooleanProperty;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
@@ -41,6 +42,7 @@ public enum ScoringSubGoals {
         IngestNoteBlindly,
         IngestNoteVisionTerminalApproach,
         IngestNoteBlindTerminalApproach,
+        MissedNoteAndSearchingForAnother,
         IngestFromSource,
         MoveToScoringRange,
         EarnestlyLaunchNote
@@ -68,6 +70,12 @@ public enum ScoringSubGoals {
     double robotWidth = 0.914+0.5;
     double scoringZoneOffset = 0.93;
 
+    double withinNoteCriticalDistanceStartTime = Double.MAX_VALUE;
+    double noteTerminalDistanceMeters = 0.2;
+    double withinNoteCriticalDistanceDurationBeforeSearching = 1.5;
+    boolean wasInCriticalNoteRange = false;
+    double validDistanceforNotesWhenSearchingMeters = 2;
+
     @Inject
     public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiringInfoSource noteFiringInfoSource,
                          PoseSubsystem pose, VisionSubsystem vision, ArmSubsystem arm, OperatorInterface oi,
@@ -346,8 +354,11 @@ public void periodic() {
                     currentScoringSubGoal = ScoringSubGoals.IngestNoteBlindly;
                 }
 
+                // This will have any special logic about how to collect a note
+                // or update our approach to getting notes
                 determineCollectionSubgoal();
 
+                // This contains the logic for exiting this state once we have a note.
                 if (noteCollectionInfoSource.confidentlyHasControlOfNote()) {
                     // Mark the nearest note as being unavailable, if we are anywhere near it
                     Note nearestNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), 1.5);
@@ -401,27 +412,74 @@ private static Pose2d transformRelativeNotePoseToFieldPose(Pose3d note, Rotation
                 new Rotation2d());
     }
 
+    private Pose2d getVisionSuggestedNotePoseWithinDistance(double searchDistance) {
+        var potentialNote = noteMap.getClosestAvailableNote(pose.getCurrentPose2d(), false);
+        if (potentialNote == null) {
+            // No note
+            return null;
+        }
+
+        double distanceToNote = pose.getCurrentPose2d().getTranslation().getDistance(
+                potentialNote.toPose2d().getTranslation());
+
+        if (distanceToNote > searchDistance) {
+            // Too far
+            return null;
+        }
+
+        return potentialNote.toPose2d();
+    }
+
+    private void applyVisionNotePoseToStateMachine(Pose2d visionNotePose) {
+        currentScoringSubGoal = ScoringSubGoals.IngestNoteVisionTerminalApproach;
+        setTerminatingPoint(visionNotePose);
+        setSpecialAimTarget(visionNotePose);
+    }
+
     private void determineCollectionSubgoal() {
+
+        boolean inCriticalNoteRange = isTerminatingPointWithinDistance(noteTerminalDistanceMeters);
+
         if (currentScoringSubGoal == ScoringSubGoals.IngestNoteBlindly) {
             if (isTerminatingPointWithinDistance(vision.getBestRangeFromStaticNoteToSearchForNote())) {
                 // look for a vision note.
-                var visionNote = noteMap.getClosestAvailableNote(pose.getCurrentPose2d(), false);
-                if (visionNote == null) {
-                    currentScoringSubGoal = ScoringSubGoals.IngestNoteBlindTerminalApproach;
-                    return;
-                }
-                if (pose.getCurrentPose2d().getTranslation().getDistance(
-                        visionNote.toPose2d().getTranslation()) > vision.getMaxNoteSearchingDistanceForSpikeNotes()) {
+                var visionNotePose = getVisionSuggestedNotePoseWithinDistance(vision.getMaxNoteSearchingDistanceForSpikeNotes());
+                // If no note or too far, blind approach.
+                if (visionNotePose == null) {
                     currentScoringSubGoal = ScoringSubGoals.IngestNoteBlindTerminalApproach;
                     return;
                 }
+                applyVisionNotePoseToStateMachine(visionNotePose);
+            }
+        }
+
+        if (currentScoringSubGoal == ScoringSubGoals.IngestNoteBlindTerminalApproach
+        || currentScoringSubGoal == ScoringSubGoals.IngestNoteVisionTerminalApproach) {
+            // We are in the final approach. However, things can go wrong here, so we need to fallback
+            // to a general search if things aren't working.
 
-                // We found a vision note nearby.
-                currentScoringSubGoal = ScoringSubGoals.IngestNoteVisionTerminalApproach;
-                setTerminatingPoint(visionNote.toPose2d());
-                setSpecialAimTarget(visionNote.toPose2d());
+            if (inCriticalNoteRange && !wasInCriticalNoteRange) {
+                // We've just entered the critical note range.
+                withinNoteCriticalDistanceStartTime = XTimer.getFPGATimestamp();
+            }
+
+            boolean failedToCollectNoteInTime =
+                    XTimer.getFPGATimestamp() > withinNoteCriticalDistanceStartTime + withinNoteCriticalDistanceDurationBeforeSearching;
+            if (inCriticalNoteRange && (failedToCollectNoteInTime)) {
+                currentScoringSubGoal = ScoringSubGoals.MissedNoteAndSearchingForAnother;
+            }
+        }
+
+        if (currentScoringSubGoal == ScoringSubGoals.MissedNoteAndSearchingForAnother) {
+            // The drive will spin in a circle looking for notes. We need to check the note map for a target.
+            var visionNotePose = getVisionSuggestedNotePoseWithinDistance(validDistanceforNotesWhenSearchingMeters);
+            if (visionNotePose != null) {
+                // we found a nearby note!
+                applyVisionNotePoseToStateMachine(visionNotePose);
             }
         }
+
+        wasInCriticalNoteRange = inCriticalNoteRange;
     }
 
     private void checkForMaskedShotsBecomingAvailable() {

src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java

@@ -161,17 +161,24 @@ private void setNewInstruction() {
     @Override
     public void execute() {
 
-        if (oracle.getTerminatingPoint().getPoseMessageNumber() != lastSeenInstructionNumber) {
-            setNewInstruction();
-        }
+        if (oracle.getHighLevelGoal() == DynamicOracle.HighLevelGoal.CollectNote
+            && oracle.getScoringSubgoal() == DynamicOracle.ScoringSubGoals.MissedNoteAndSearchingForAnother) {
+            // Spin slowly, looking for a candidate note
+            drive.move(new XYPair(0,0), 0.25);
+        } else {
+            // Go to specific points the oracle told us to go to
+            if (oracle.getTerminatingPoint().getPoseMessageNumber() != lastSeenInstructionNumber) {
+                setNewInstruction();
+            }
 
-        Twist2d powers = logic.calculatePowers(pose.getCurrentPose2d(), drive.getPositionalPid(), headingModule, drive.getMaxTargetSpeedMetersPerSecond());
+            Twist2d powers = logic.calculatePowers(pose.getCurrentPose2d(), drive.getPositionalPid(), headingModule, drive.getMaxTargetSpeedMetersPerSecond());
 
-        aKitLog.record("Powers", powers);
+            aKitLog.record("Powers", powers);
 
-        drive.fieldOrientedDrive(
-                new XYPair(powers.dx, powers.dy),
-                powers.dtheta, pose.getCurrentHeading().getDegrees(), false);
+            drive.fieldOrientedDrive(
+                    new XYPair(powers.dx, powers.dy),
+                    powers.dtheta, pose.getCurrentHeading().getDegrees(), false);
+        }
     }
 
     @Override