From 9bd079864dd5dcbc277bae067bb9dea593fdf2dc Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Sat, 30 Mar 2024 11:05:08 -0700 Subject: [PATCH] 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 --- SeriouslyCommonLib | 2 +- .../auto_programs/GriefMiddle.java | 122 ++++++++++++++++++ .../DriveToGivenNoteWithVisionCommand.java | 6 +- .../components/BaseRobotComponent.java | 2 + .../OperatorCommandMap.java | 9 +- .../subsystems/arm/ArmSubsystem.java | 2 +- .../subsystems/oracle/DynamicOracle.java | 80 ++++++++++-- .../SwerveAccordingToOracleCommand.java | 23 ++-- 8 files changed, 220 insertions(+), 26 deletions(-) create mode 100644 src/main/java/competition/auto_programs/GriefMiddle.java diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 0618e983..e2a7d30d 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 0618e983f522e4b873307b680a7e6ccf0e36fdda +Subproject commit e2a7d30deb95f25b18df2b8cc8cbe5e8714e54ef diff --git a/src/main/java/competition/auto_programs/GriefMiddle.java b/src/main/java/competition/auto_programs/GriefMiddle.java new file mode 100644 index 00000000..115cc341 --- /dev/null +++ b/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 swerveSimpleTrajectoryCommandProvider, + Provider 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 goToBottomWing() { + var points = new ArrayList(); + 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 goToCenter5() { + var points = new ArrayList(); + var adjusted5 = shiftCenterlineX(PoseSubsystem.CenterLine5); + points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(adjusted5, 10)); + return points; + } + + public List goUpLineToCenter1() { + var points = new ArrayList(); + 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; + } + + } + + +} diff --git a/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java b/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java index 1030f517..c0bb54b5 100644 --- a/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java +++ b/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(); diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 8e0f6c79..227bedb5 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/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(); } \ No newline at end of file diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 6e5178eb..6691ca79 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -1,6 +1,7 @@ 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; @@ -8,6 +9,7 @@ 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); diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index c9267b32..8c2d9549 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/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; diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 56580c82..ac66d04a 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/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() { diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index e8fe0796..1c7ecf60 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/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