diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index c7ccd4c3..134e6b37 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit c7ccd4c3b02ec6b35ff2bdaa4a6589b475e87f36 +Subproject commit 134e6b375b42b8d50f7476584a9f926002afdd2d diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 2b46431f..2397d792 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -20,6 +20,7 @@ import xbot.common.math.FieldPose; import xbot.common.math.MovingAverageForDouble; import xbot.common.math.MovingAverageForTranslation2d; +import xbot.common.subsystems.drive.swerve.SwerveDriveSubsystem; import xbot.common.subsystems.pose.BasePoseSubsystem; public class Robot extends BaseRobot { @@ -31,6 +32,7 @@ public Robot() { DynamicOracle oracle; PoseSubsystem poseSubsystem; OperatorInterface oi; + DriveSubsystem drive; @Override protected void initializeSystems() { @@ -45,8 +47,9 @@ protected void initializeSystems() { } oracle = getInjectorComponent().dynamicOracle(); oi = getInjectorComponent().operatorInterface(); + drive = (DriveSubsystem)getInjectorComponent().driveSubsystem(); - dataFrameRefreshables.add((DriveSubsystem)getInjectorComponent().driveSubsystem()); + dataFrameRefreshables.add(drive); poseSubsystem = (PoseSubsystem) getInjectorComponent().poseSubsystem(); dataFrameRefreshables.add(poseSubsystem); dataFrameRefreshables.add(getInjectorComponent().visionSubsystem()); @@ -105,6 +108,7 @@ public void teleopInit() { super.teleopInit(); oracle.clearNoteMapForTeleop(); oracle.clearScoringLocationsForTeleop(); + drive.setDriveModuleCurrentLimits(SwerveDriveSubsystem.CurrentLimitMode.Teleop); } @Override diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java b/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java index d074139c..04687b54 100644 --- a/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java +++ b/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; +import xbot.common.subsystems.drive.swerve.SwerveDriveSubsystem; import xbot.common.trajectory.XbotSwervePoint; import javax.inject.Inject; @@ -27,7 +28,13 @@ //when we have the time to tune a ranged shot will update to that public class SubwooferShotFromBotThenTwoCenterline extends SequentialCommandGroup { AutonomousCommandSelector autoSelector; - double centerlineTimeout = 8; + double centerlineTimeout = 999; + + double meterThreshold = 0.3048; + double velocityThreshold = 0.05; + final PoseSubsystem pose; + final DriveSubsystem drive; + @Inject public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelector, PoseSubsystem pose, DriveSubsystem drive, @@ -37,8 +44,14 @@ public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelec Provider driveToListOfPointsCommandProvider, Provider intakeCollectorCommandProvider ){ + this.pose = pose; + this.drive = drive; + this.autoSelector = autoSelector; + var limitCurrentCommand = drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto); + this.addCommands(limitCurrentCommand); + var startInFrontOfSpeaker = pose.createSetPositionCommand( () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation)); this.addCommands(startInFrontOfSpeaker); @@ -52,29 +65,32 @@ public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelec queueMessageToAutoSelector("Drive to bottom spike note, collect, drive back to sub(middle) and shoot"); this.addCommands( new InstantCommand(() -> { - drive.setTargetNote(PoseSubsystem.CenterLine5); + drive.setTargetNote(PoseSubsystem.CenterLine4); }) ); - var driveToCenterline5 = driveToNoteProvider.get(); + var driveToCenterline4 = driveToNoteProvider.get(); this.addCommands( new InstantCommand(()->{ - driveToCenterline5.setWaypoints(new Translation2d( + driveToCenterline4.setWaypoints(new Translation2d( PoseSubsystem.BlueSpikeBottom.getX() + 2.06, PoseSubsystem.BlueSpikeBottom.getY() - 2.3951 )); }) ); - driveToCenterline5.logic.setEnableConstantVelocity(true); - driveToCenterline5.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); - driveToCenterline5.setVisionRangeOverride(VisionRange.Far); + driveToCenterline4.logic.setEnableConstantVelocity(true); + driveToCenterline4.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); + driveToCenterline4.setVisionRangeOverride(VisionRange.Far); var collect1 = collectSequenceCommandGroupProvider.get(); //swap collect and drive for testing - this.addCommands(Commands.deadline(collect1,driveToCenterline5).withTimeout(centerlineTimeout)); + this.addCommands(Commands.deadline(collect1,driveToCenterline4).withTimeout(centerlineTimeout)); + + addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Teleop)); var driveBackToBottomSubwooferFirst = driveToListOfPointsCommandProvider.get(); driveBackToBottomSubwooferFirst.addPointsSupplier(this::goBackToBotSubwoofer); driveBackToBottomSubwooferFirst.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); + driveBackToBottomSubwooferFirst.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer); var collect3 = intakeCollectorCommandProvider.get(); this.addCommands(Commands.deadline(driveBackToBottomSubwooferFirst.withTimeout(centerlineTimeout),collect3)); @@ -85,30 +101,35 @@ public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelec this.addCommands( new InstantCommand(() -> { - drive.setTargetNote(PoseSubsystem.CenterLine4); + drive.setTargetNote(PoseSubsystem.CenterLine5); }) ); - var driveToCenterline4 = driveToNoteProvider.get(); + var driveToCenterline5 = driveToNoteProvider.get(); this.addCommands( new InstantCommand(()->{ - driveToCenterline4.setWaypoints(new Translation2d( + driveToCenterline5.setWaypoints(new Translation2d( PoseSubsystem.BlueSpikeBottom.getX() + 2.06, PoseSubsystem.BlueSpikeBottom.getY() - 2.3951 )); }) ); - driveToCenterline4.logic.setEnableConstantVelocity(true); - driveToCenterline4.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); - driveToCenterline4.setVisionRangeOverride(VisionRange.Far); + addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto)); + + driveToCenterline5.logic.setEnableConstantVelocity(true); + driveToCenterline5.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); + driveToCenterline5.setVisionRangeOverride(VisionRange.Far); var collect2 = collectSequenceCommandGroupProvider.get(); //swap collect and drive for testing - this.addCommands(Commands.deadline(collect2,driveToCenterline4).withTimeout(centerlineTimeout)); + this.addCommands(Commands.deadline(collect2,driveToCenterline5).withTimeout(centerlineTimeout)); + + addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto)); var driveBackToBottomSubwooferSecond = driveToListOfPointsCommandProvider.get(); driveBackToBottomSubwooferSecond.addPointsSupplier(this::goBackToBotSubwoofer); driveBackToBottomSubwooferSecond.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); + driveBackToBottomSubwooferSecond.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer); var collect4 = intakeCollectorCommandProvider.get(); this.addCommands(Commands.deadline(driveBackToBottomSubwooferSecond.withTimeout(centerlineTimeout),collect4)); @@ -130,4 +151,19 @@ private ArrayList goBackToBotSubwoofer(){ points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(PoseSubsystem.BlueSubwooferBottomScoringLocation,10)); return points; } + + private boolean alternativeIsFinishedForSubwoofer() { + double speed = pose.getRobotCurrentSpeed(); + + Translation2d robotLocation = pose.getCurrentPose2d().getTranslation(); + + // Returns finished if both position and velocity are under threshold + boolean nearPositionThreshold = PoseSubsystem.convertBlueToRedIfNeeded( + PoseSubsystem.BlueSubwooferBottomScoringLocation) + .getTranslation().getDistance(robotLocation) < meterThreshold; + + boolean nearVelocityThreshold = speed < velocityThreshold; + + return (nearPositionThreshold && nearVelocityThreshold); + } } diff --git a/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java b/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java index ca59ad5d..4498ec53 100644 --- a/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java +++ b/src/main/java/competition/commandgroups/DriveToGivenNoteWithVisionCommand.java @@ -66,7 +66,7 @@ public void execute() { } } break; - case VisionApproach: + case CenterCameraVisionApproach: if (super.isFinished()) { // we've hit our target, but no note! Back away and try again. log.info("Switching to back away mode"); @@ -80,7 +80,7 @@ public void execute() { if (getNearestVisionNote() != null) { log.info("Switching to vision mode"); assignClosestVisionNoteToDriveSubsystemIfYouSeeANoteAndReplanPath(); - noteAcquisitionMode = NoteAcquisitionMode.VisionApproach; + noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach; } else { log.info("Switching to give up mode"); noteAcquisitionMode = NoteAcquisitionMode.GiveUp; @@ -152,7 +152,7 @@ private void assignClosestVisionNoteToDriveSubsystemIfYouSeeANoteAndReplanPath() log.info("Found note at " + noteLocation.getTranslation()); log.info("Assigning note to drive subsystem"); drive.setTargetNote(noteLocation); - noteAcquisitionMode = NoteAcquisitionMode.VisionApproach; + noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach; prepareToDriveAtGivenNote(); } diff --git a/src/main/java/competition/subsystems/collector/commands/WaitForNoteCollectedCommand.java b/src/main/java/competition/subsystems/collector/commands/WaitForNoteCollectedCommand.java index 8ef1139e..d6256ce8 100644 --- a/src/main/java/competition/subsystems/collector/commands/WaitForNoteCollectedCommand.java +++ b/src/main/java/competition/subsystems/collector/commands/WaitForNoteCollectedCommand.java @@ -25,9 +25,7 @@ public void execute() { @Override public boolean isFinished() { - return collector.confidentlyHasControlOfNote() - || collector.getGamePieceInControl() - || collector.getGamePieceReady(); + return collector.confidentlyHasControlOfNote(); } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java index 1a05e1f0..a9cf7b10 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java @@ -92,9 +92,9 @@ public void execute() { case BackAwayToTryAgain: super.execute(); break; - // If we are using - case VisionApproach: - case VisionTerminalApproach: + // If we are using the cameras, listen to the suggested powers + case CenterCameraVisionApproach: + case CenterCameraTerminalApproach: case SearchViaRotation: if (currentAdvice.suggestedDrivePercentages.isPresent()) { var driveValues = currentAdvice.suggestedDrivePercentages.get(); diff --git a/src/main/java/competition/subsystems/drive/commands/TryDriveToBearingNote.java b/src/main/java/competition/subsystems/drive/commands/TryDriveToBearingNote.java index dc611307..048182f8 100644 --- a/src/main/java/competition/subsystems/drive/commands/TryDriveToBearingNote.java +++ b/src/main/java/competition/subsystems/drive/commands/TryDriveToBearingNote.java @@ -26,7 +26,6 @@ public TryDriveToBearingNote( @Override public void initialize() { log.info("Initializing"); - noteSeekLogic.setInitialMode(NoteAcquisitionMode.SearchViaRotation); noteSeekLogic.setAllowRotationSearch(true); super.initialize(); } diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index aaa495c1..c13b614e 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -286,109 +286,117 @@ public void reserveNoteBasedOnNeoTrellis(PointOfInterest pointOfInterest, Driver */ @Override public void periodic() { - checkForMaskedShotsBecomingAvailable(); + try { + checkForMaskedShotsBecomingAvailable(); - // Populate the field with notes from vision - noteMap.clearStaleVisionNotes(this.maxVisionNoteAge.get()); - if (this.includeVisionNotes.get()) { - handleVisionDetectedNotes(); - } + // Populate the field with notes from vision + noteMap.clearStaleVisionNotes(this.maxVisionNoteAge.get()); + if (this.includeVisionNotes.get()) { + handleVisionDetectedNotes(); + } - aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); - // TODO: move this visualization into Simulator2024. This is a lot of data for network tables. - // We can always set the global log level to debug and replay the inputs to regenerate this data. - aKitLog.record("NoteMap", noteMap.getAllAvailableNotes().stream().map(Note::get3dLocation).toArray(Pose3d[]::new)); - aKitLog.record("UnavailableNoteMap", noteMap.getAllUnavailableNotes().stream().map(Note::get3dLocation).toArray(Pose3d[]::new)); - aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); - - switch (currentHighLevelGoal) { - case ScoreInAmp: // For now keeping things simple - case ScoreInSpeaker: - if (firstRunInNewGoal || reevaluationRequested) { - setTargetNote(null); - var closestScoringLocation = scoringLocationMap.getClosest(pose.getCurrentPose2d().getTranslation(), - Availability.Available); - setTerminatingPoint(closestScoringLocation.getLocation()); - setChosenScoringLocation(closestScoringLocation.getPointOfInterest()); - - currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; - setSpecialAimTarget(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SPEAKER_AIM_TARGET)); - // Choose a good speaker scoring location - // Publish a route from current position to that location - firstRunInNewGoal = false; - reevaluationRequested = false; - } + aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); + // TODO: move this visualization into Simulator2024. This is a lot of data for network tables. + // We can always set the global log level to debug and replay the inputs to regenerate this data. + aKitLog.record("NoteMap", noteMap.getAllAvailableNotes().stream().map(Note::get3dLocation).toArray(Pose3d[]::new)); + aKitLog.record("UnavailableNoteMap", noteMap.getAllUnavailableNotes().stream().map(Note::get3dLocation).toArray(Pose3d[]::new)); + aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); + + switch (currentHighLevelGoal) { + case ScoreInAmp: // For now keeping things simple + case ScoreInSpeaker: + if (firstRunInNewGoal || reevaluationRequested) { + setTargetNote(null); + var closestScoringLocation = scoringLocationMap.getClosest(pose.getCurrentPose2d().getTranslation(), + Availability.Available); + + if (closestScoringLocation != null) { + setTerminatingPoint(closestScoringLocation.getLocation()); + setChosenScoringLocation(closestScoringLocation.getPointOfInterest()); + } + + currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; + setSpecialAimTarget(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SPEAKER_AIM_TARGET)); + // Choose a good speaker scoring location + // Publish a route from current position to that location + firstRunInNewGoal = false; + reevaluationRequested = false; + } - determineScoringSubgoal(); + determineScoringSubgoal(); - // If we've launched our note, time to get another one - if (noteFiringInfoSource.confidentlyHasFiredNote()) { - currentHighLevelGoal = HighLevelGoal.CollectNote; - firstRunInNewGoal = true; + // If we've launched our note, time to get another one + if (noteFiringInfoSource.confidentlyHasFiredNote()) { + currentHighLevelGoal = HighLevelGoal.CollectNote; + firstRunInNewGoal = true; + break; + } break; - } - break; - case CollectNote: - if (firstRunInNewGoal || reevaluationRequested) { - // Choose a good note collection location - Note suggestedNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), - Availability.Available); - setTargetNote(suggestedNote); - - if (suggestedNote == null) { - // No notes on the field! Let's suggest going to the source and hope something turns up. - // However, if we are in autonomous, we should instead just go to the line. - if (DriverStation.isAutonomous()) { - setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.CenterLine5)); + case CollectNote: + if (firstRunInNewGoal || reevaluationRequested) { + // Choose a good note collection location + Note suggestedNote = noteMap.getClosest(pose.getCurrentPose2d().getTranslation(), + Availability.Available); + setTargetNote(suggestedNote); + + if (suggestedNote == null) { + // No notes on the field! Let's suggest going to the source and hope something turns up. + // However, if we are in autonomous, we should instead just go to the line. + if (DriverStation.isAutonomous()) { + setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.CenterLine5)); + } else { + setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSourceMiddle)); + } } else { - setTerminatingPoint(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSourceMiddle)); + setTerminatingPoint(getTargetNote().getLocation()); + setSpecialAimTarget(getTargetNote().getLocation()); } - } - else { - setTerminatingPoint(getTargetNote().getLocation()); - setSpecialAimTarget(getTargetNote().getLocation()); - } - // Publish a route from current position to that location - firstRunInNewGoal = false; - reevaluationRequested = false; + // Publish a route from current position to that location + firstRunInNewGoal = false; + reevaluationRequested = false; - currentScoringSubGoal = ScoringSubGoals.IngestNoteBlindly; - } + currentScoringSubGoal = ScoringSubGoals.IngestNoteBlindly; + } - // This will have any special logic about how to collect a note - // or update our approach to getting notes - determineCollectionSubgoal(); + // 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); - if (nearestNote != null) { - nearestNote.setUnavailable(UnavailableReason.Gone); - } + // 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); + if (nearestNote != null) { + nearestNote.setUnavailable(UnavailableReason.Gone); + } - // Since we have a note, let's go score it. - currentHighLevelGoal = HighLevelGoal.ScoreInSpeaker; - firstRunInNewGoal = true; + // Since we have a note, let's go score it. + currentHighLevelGoal = HighLevelGoal.ScoreInSpeaker; + firstRunInNewGoal = true; + break; + } break; - } - break; - default: - break; - } + default: + break; + } - aKitLog.record("Current Goal", currentHighLevelGoal); - aKitLog.record("Current Note", - targetNote == null ? new Pose2d(-100, -100, new Rotation2d(0)) : getTargetNote().getLocation()); + aKitLog.record("Current Goal", currentHighLevelGoal); + aKitLog.record("Current Note", + targetNote == null ? new Pose2d(-100, -100, new Rotation2d(0)) : getTargetNote().getLocation()); - aKitLog.setLogLevel(AKitLogger.LogLevel.DEBUG); - if (getTerminatingPoint() != null) { - aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose()); - aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber()); + aKitLog.setLogLevel(AKitLogger.LogLevel.DEBUG); + if (getTerminatingPoint() != null) { + aKitLog.record("Terminating Point", getTerminatingPoint().getTerminatingPose()); + aKitLog.record("MessageCount", getTerminatingPoint().getPoseMessageNumber()); + } + aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); + aKitLog.record("Current SubGoal", currentScoringSubGoal); + } + catch (Exception e) + { + log.info("Crash!"); } - aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); - aKitLog.record("Current SubGoal", currentScoringSubGoal); } private void handleVisionDetectedNotes() { @@ -619,6 +627,10 @@ private void determineScoringSubgoal() { } public boolean isTerminatingPointWithinDistance(double distance) { + if (terminatingPoint == null) { + return false; + } + return pose.getCurrentPose2d().getTranslation().getDistance( getTerminatingPoint().getTerminatingPose().getTranslation()) < distance; diff --git a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java index 1c7ecf60..395d9f72 100644 --- a/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java +++ b/src/main/java/competition/subsystems/oracle/SwerveAccordingToOracleCommand.java @@ -72,7 +72,7 @@ private void setNewInstruction() { double maxVelocity = drive.getMaxTargetSpeedMetersPerSecond(); if (DriverStation.isAutonomous()) { - maxVelocity = drive.getSuggestedAutonomousExtremeSpeed(); + maxVelocity = drive.getSuggestedAutonomousMaximumSpeed(); } if (oracle.getTargetNote() != null) { @@ -149,13 +149,15 @@ private void setNewInstruction() { // Now that our swerve logic is configured to satisfaction, give it the goal point and reset it to start // the new path. - lastSeenInstructionNumber = goalPosition.getPoseMessageNumber(); - logic.setKeyPoints(List.of(new XbotSwervePoint( - goalPosition.getTerminatingPose().getTranslation(), - goalPosition.getTerminatingPose().getRotation(), - 10))); - - logic.reset(pose.getCurrentPose2d()); + if (goalPosition != null) { + lastSeenInstructionNumber = goalPosition.getPoseMessageNumber(); + logic.setKeyPoints(List.of(new XbotSwervePoint( + goalPosition.getTerminatingPose().getTranslation(), + goalPosition.getTerminatingPose().getRotation(), + 10))); + + logic.reset(pose.getCurrentPose2d()); + } } @Override diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index e0186370..15fd7d91 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -468,9 +468,12 @@ public double getAngularErrorToSpeakerInDegrees() { } public double getAngularErrorToTranslation2dInDegrees(Translation2d targetPosition, Rotation2d offset) { - var angleFromChassisToTarget = WrappedRotation2d.fromDegrees( + return getCurrentHeading().minus(getAngleFromChassisToTarget(targetPosition, offset)).getDegrees(); + } + + public WrappedRotation2d getAngleFromChassisToTarget(Translation2d targetPosition, Rotation2d offset) { + return WrappedRotation2d.fromDegrees( targetPosition.minus(getCurrentPose2d().getTranslation()).getAngle().rotateBy(offset).getDegrees()); - return getCurrentHeading().minus(angleFromChassisToTarget).getDegrees(); } public Translation2d transformRobotCoordinateToFieldCoordinate(Translation2d robotCoordinates) { diff --git a/src/main/java/competition/subsystems/vision/NoteAcquisitionMode.java b/src/main/java/competition/subsystems/vision/NoteAcquisitionMode.java index f3f689b8..c852dc84 100644 --- a/src/main/java/competition/subsystems/vision/NoteAcquisitionMode.java +++ b/src/main/java/competition/subsystems/vision/NoteAcquisitionMode.java @@ -2,8 +2,9 @@ public enum NoteAcquisitionMode { BlindApproach, - VisionApproach, - VisionTerminalApproach, + RotateToNoteDetectedByCornerCameras, + CenterCameraVisionApproach, + CenterCameraTerminalApproach, BackAwayToTryAgain, SearchViaRotation, GiveUp diff --git a/src/main/java/competition/subsystems/vision/NoteDetectionSource.java b/src/main/java/competition/subsystems/vision/NoteDetectionSource.java new file mode 100644 index 00000000..81aa5d71 --- /dev/null +++ b/src/main/java/competition/subsystems/vision/NoteDetectionSource.java @@ -0,0 +1,6 @@ +package competition.subsystems.vision; + +public enum NoteDetectionSource { + CenterCamera, + PeripheralCameras +} diff --git a/src/main/java/competition/subsystems/vision/NoteScanResult.java b/src/main/java/competition/subsystems/vision/NoteScanResult.java new file mode 100644 index 00000000..022055ab --- /dev/null +++ b/src/main/java/competition/subsystems/vision/NoteScanResult.java @@ -0,0 +1,19 @@ +package competition.subsystems.vision; + +public class NoteScanResult { + private NoteDetectionSource source; + private SimpleNote note; + + public NoteScanResult(NoteDetectionSource source, SimpleNote note) { + this.source = source; + this.note = note; + } + + public NoteDetectionSource getSource() { + return source; + } + + public SimpleNote getNote() { + return note; + } +} diff --git a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 353a4ed0..20bdd719 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java @@ -4,6 +4,8 @@ import competition.subsystems.oracle.DynamicOracle; 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.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import org.apache.logging.log4j.LogManager; @@ -31,16 +33,22 @@ public class NoteSeekLogic { NoteAcquisitionMode noteAcquisitionMode = NoteAcquisitionMode.BlindApproach; double frozenHeading = 0; - double timeWhenVisionModeEntered = Double.MAX_VALUE; - double timeWhenTerminalVisionModeEntered = Double.MAX_VALUE; - double timeWhenRotationSearchModeEntered = Double.MAX_VALUE; - double timeWhenBackUpModeEntered = Double.MAX_VALUE; + Translation2d frozenNoteTarget; + + final TimeoutTracker visionModeTimeoutTracker; + final TimeoutTracker terminalVisionModeTimeoutTracker; + final TimeoutTracker rotationSearchModeTimeoutTracker; + final TimeoutTracker backupModeTimeoutTracker; + final TimeoutTracker rotateToNoteModeTimeoutTracker; + + final DoubleProperty visionModeDuration; final DoubleProperty terminalVisionModeDuration; final DoubleProperty rotationSearchDuration; final DoubleProperty rotationSearchPower; final DoubleProperty terminalVisionModePowerFactor; final DoubleProperty backUpDuration; + final DoubleProperty rotateToNoteDuration; boolean hasDoneVisionCheckYet = false; protected final AKitLogger aKitLog; @@ -64,12 +72,21 @@ public NoteSeekLogic(VisionSubsystem vision, DynamicOracle oracle, PoseSubsystem pf.setPrefix("NoteSeekLogic"); pf.setDefaultLevel(Property.PropertyLevel.Important); + + visionModeDuration = pf.createPersistentProperty("VisionModeDuration", 5); terminalVisionModeDuration = pf.createPersistentProperty("TerminalVisionModeDuration", 1.0); rotationSearchDuration = pf.createPersistentProperty("RotationSearchDuration", 3.0); rotationSearchPower = pf.createPersistentProperty("RotationSearchPower", 0.17); terminalVisionModePowerFactor = pf.createPersistentProperty("TerminalVisionModePowerFactor", 0.5); backUpDuration = pf.createPersistentProperty("BackUpDuration", 1.0); - + rotateToNoteDuration = pf.createPersistentProperty("RotateToNoteDuration", 1.0); + + visionModeTimeoutTracker = new TimeoutTracker(() -> visionModeDuration.get()); + rotateToNoteModeTimeoutTracker = new TimeoutTracker(() -> rotateToNoteDuration.get()); + terminalVisionModeTimeoutTracker = new TimeoutTracker(() -> terminalVisionModeDuration.get()); + rotationSearchModeTimeoutTracker = new TimeoutTracker(() -> rotationSearchDuration.get()); + backupModeTimeoutTracker = new TimeoutTracker(() -> backUpDuration.get()); + headingModule = headingModuleFactory.create(drive.getAggressiveGoalHeadingPid()); aKitLog = new AKitLogger(akitPrefix); @@ -90,69 +107,67 @@ public void setVisionRange(VisionRange range) { public void reset() { noteAcquisitionMode = initialMode; resetVisionModeTimers(); - if (initialMode == NoteAcquisitionMode.VisionApproach) { - timeWhenVisionModeEntered = XTimer.getFPGATimestamp(); + if (initialMode == NoteAcquisitionMode.CenterCameraVisionApproach) { + visionModeTimeoutTracker.start(); } hasDoneVisionCheckYet = false; } private void resetVisionModeTimers() { - timeWhenVisionModeEntered = Double.MAX_VALUE; - timeWhenTerminalVisionModeEntered = Double.MAX_VALUE; - timeWhenRotationSearchModeEntered = Double.MAX_VALUE; - timeWhenBackUpModeEntered = Double.MAX_VALUE; + visionModeTimeoutTracker.reset(); + terminalVisionModeTimeoutTracker.reset(); + rotationSearchModeTimeoutTracker.reset(); + backupModeTimeoutTracker.reset(); + rotateToNoteModeTimeoutTracker.reset(); } private void checkForModeChanges(boolean atTargetPose) { switch (noteAcquisitionMode) { case BlindApproach: - if (!hasDoneVisionCheckYet) { - - if (drive.getTargetNote() == null) { - log.info("No target note set."); - if (allowRotationSearch) { - log.info("Attempting to find one via rotation."); - noteAcquisitionMode = NoteAcquisitionMode.SearchViaRotation; - timeWhenRotationSearchModeEntered = XTimer.getFPGATimestamp(); - } else { - log.info("Giving up."); - noteAcquisitionMode = NoteAcquisitionMode.GiveUp; - } - break; - } + // If no note has been set, then we either need to give up + // or try searching via rotation or other methods. + if (drive.getTargetNote() == null) { + log.info("No target note set."); + decideWhetherToGiveUpOrRotate(); + break; + } - double rangeToStaticNote = pose.getCurrentPose2d().getTranslation().getDistance( - drive.getTargetNote().getTranslation()); - aKitLog.record("RangeToStaticNote", rangeToStaticNote); - if (rangeToStaticNote < vision.getBestRangeFromStaticNoteToSearchForNote(visionRange)) { - hasDoneVisionCheckYet = true; + double rangeToStaticNote = pose.getCurrentPose2d().getTranslation().getDistance( + drive.getTargetNote().getTranslation()); + aKitLog.record("RangeToStaticNote", rangeToStaticNote); + + if (rangeToStaticNote < vision.getBestRangeFromStaticNoteToSearchForNote()) { + if (!hasDoneVisionCheckYet) { log.info("Close to static note - attempting vision update."); - if (vision.getCenterCamLargestNoteTarget().isPresent()) { - log.info("Found with central camera. Advancing using vision"); - noteAcquisitionMode = NoteAcquisitionMode.VisionApproach; - timeWhenVisionModeEntered = XTimer.getFPGATimestamp(); - } else { - log.info("No note found with central camera. Staying in blind approach."); - } + hasDoneVisionCheckYet = true; } + evaluateIfShouldMoveToVisionBasedCollection(false); + } + break; + case RotateToNoteDetectedByCornerCameras: + // Rotate until either the center camera sees the note nice and solidly or we have been in this + // mode too long. + if (hasSolidLockOnNoteWithCenterCamera()) { + noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach; + visionModeTimeoutTracker.start(); + } + if (rotateToNoteModeTimeoutTracker.getTimedOut()) { + decideWhetherToGiveUpOrRotate(); } break; - case VisionApproach: + case CenterCameraVisionApproach: if (shouldEnterTerminalVisionApproach()) { log.info("Switching to terminal vision approach"); - noteAcquisitionMode = NoteAcquisitionMode.VisionTerminalApproach; - timeWhenTerminalVisionModeEntered = XTimer.getFPGATimestamp(); + noteAcquisitionMode = NoteAcquisitionMode.CenterCameraTerminalApproach; + terminalVisionModeTimeoutTracker.start(); frozenHeading = pose.getCurrentHeading().getDegrees(); } break; - case VisionTerminalApproach: + case CenterCameraTerminalApproach: if (shouldExitTerminalVisionApproach()) { - log.info("Switching to back away to try again"); - timeWhenBackUpModeEntered = XTimer.getFPGATimestamp(); - noteAcquisitionMode = NoteAcquisitionMode.BackAwayToTryAgain; - suggestedLocation = new Pose2d( - pose.transformRobotCoordinateToFieldCoordinate(new Translation2d(1,0)), - pose.getCurrentHeading()); + log.info("Going to rotation search"); + rotationSearchModeTimeoutTracker.start(); + noteAcquisitionMode = NoteAcquisitionMode.SearchViaRotation; } break; case BackAwayToTryAgain: @@ -161,28 +176,17 @@ private void checkForModeChanges(boolean atTargetPose) { if (vision.getCenterCamLargestNoteTarget().isPresent()) { log.info("Found a note. Switching to vision mode."); resetVisionModeTimers(); - timeWhenVisionModeEntered = XTimer.getFPGATimestamp(); - noteAcquisitionMode = NoteAcquisitionMode.VisionApproach; + visionModeTimeoutTracker.start(); + noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach; } else { log.info("Can't see a note."); - if (allowRotationSearch) { - log.info("Attempting to find one via rotation."); - noteAcquisitionMode = NoteAcquisitionMode.SearchViaRotation; - timeWhenRotationSearchModeEntered = XTimer.getFPGATimestamp(); - } else { - log.info("Giving up."); - noteAcquisitionMode = NoteAcquisitionMode.GiveUp; - } + decideWhetherToGiveUpOrRotate(); } } break; case SearchViaRotation: - if (vision.getCenterCamLargestNoteTarget().isPresent()) { - log.info("Found a note. Switching to vision mode."); - resetVisionModeTimers(); - timeWhenVisionModeEntered = XTimer.getFPGATimestamp(); - noteAcquisitionMode = NoteAcquisitionMode.VisionApproach; - } else if (shouldExitRotationSearch()) { + evaluateIfShouldMoveToVisionBasedCollection(true); + if (shouldExitRotationSearch()) { log.info("Giving up."); noteAcquisitionMode = NoteAcquisitionMode.GiveUp; } @@ -198,27 +202,48 @@ private void checkForModeChanges(boolean atTargetPose) { aKitLog.record("NoteAcquisitionMode", noteAcquisitionMode); } + private void evaluateIfShouldMoveToVisionBasedCollection(boolean resetTimersIfFound) { + var scannedNote = scanForNote(); + if (scannedNote.isPresent()) { + if (scannedNote.get().getSource() == NoteDetectionSource.CenterCamera) { + log.info("Found with central camera. Advancing using vision"); + noteAcquisitionMode = NoteAcquisitionMode.CenterCameraVisionApproach; + visionModeTimeoutTracker.start(); + } else { + log.info("Found with a corner camera. Advancing using vision"); + noteAcquisitionMode = NoteAcquisitionMode.RotateToNoteDetectedByCornerCameras; + frozenHeading = scannedNote.get().getNote().yaw; + rotateToNoteModeTimeoutTracker.start(); + } + if (resetTimersIfFound) { + resetVisionModeTimers(); + } + } + } + public NoteSeekAdvice getAdvice(boolean atTargetPose) { checkForModeChanges(atTargetPose); double approachPower = -drive.getSuggestedAutonomousMaximumSpeed() / drive.getMaxTargetSpeedMetersPerSecond(); double terminalPower = approachPower * terminalVisionModePowerFactor.get(); - + double rotationPower = 0; switch (noteAcquisitionMode) { case BlindApproach: // Only return the state - calling class will figure out how to approach. return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.empty()); - case VisionApproach: + case RotateToNoteDetectedByCornerCameras: + rotationPower = headingModule.calculateHeadingPower(frozenHeading); + return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.of(new Twist2d(0, 0, rotationPower))); + case CenterCameraVisionApproach: var target = vision.getCenterCamLargestNoteTarget(); if (target.isPresent()) { - double rotationPower = - this.drive.getAggressiveGoalHeadingPid().calculate(0, target.get().getYaw()); + rotationPower = this.drive.getAggressiveGoalHeadingPid().calculate(0, target.get().getYaw()); suggestedPowers = new Twist2d(approachPower, 0, rotationPower); } return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.of(suggestedPowers)); - case VisionTerminalApproach: - double rotationPower = headingModule.calculateHeadingPower(frozenHeading); + case CenterCameraTerminalApproach: + rotationPower = headingModule.calculateHeadingPower(frozenHeading); suggestedPowers = new Twist2d(terminalPower, 0, rotationPower); return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.of(suggestedPowers)); case BackAwayToTryAgain: @@ -247,24 +272,97 @@ private boolean shouldEnterTerminalVisionApproach() { } private boolean shouldExitTerminalVisionApproach() { - if (XTimer.getFPGATimestamp() > timeWhenTerminalVisionModeEntered + terminalVisionModeDuration.get()) { - return true; - } - return false; + return terminalVisionModeTimeoutTracker.getTimedOut(); } private boolean shouldExitRotationSearch() { - if (XTimer.getFPGATimestamp() > timeWhenRotationSearchModeEntered + rotationSearchDuration.get()) { - return true; - } - return false; + return rotationSearchModeTimeoutTracker.getTimedOut(); } private boolean shouldExitBackUp(boolean atTargetPosition) { return atTargetPosition - || XTimer.getFPGATimestamp() > timeWhenBackUpModeEntered + backUpDuration.get() + || backupModeTimeoutTracker.getTimedOut() || vision.getCenterCamLargestNoteTarget().isPresent(); } + private Optional getCenterCamNote() { + if (vision.getCenterCamLargestNoteTarget().isPresent()) { + return Optional.of(new NoteScanResult( + NoteDetectionSource.CenterCamera, + vision.getCenterCamLargestNoteTarget().get()) + ); + } + return Optional.empty(); + } + + private Optional getCornerCameraNote() { + var notePosition = getClosestAvailableVisionNote(); + + if (notePosition.isPresent()) { + // transform the X/Y coordinate to a bearing + double bearing = this.pose.getAngularErrorToTranslation2dInDegrees( + notePosition.get().getTranslation(), + Rotation2d.fromDegrees(180)); // point rear of robot + SimpleNote fakeNote = new SimpleNote(100, bearing, 100); + + return Optional.of(new NoteScanResult( + NoteDetectionSource.PeripheralCameras, + fakeNote) + ); + } + return Optional.empty(); + } + + private Optional scanForNote() { + + var centerNote = getCenterCamNote(); + if (centerNote.isPresent()) { + return centerNote; + } + + var cornerNote = getCornerCameraNote(); + if (cornerNote.isPresent()) { + return cornerNote; + } + + // Nothing found. + return Optional.empty(); + } + + + private Optional getClosestAvailableVisionNote() { + var virtualPoint = getProjectedPoint(); + var notePosition = this.oracle.getNoteMap().getClosestAvailableNote(virtualPoint, false); + if(notePosition != null) { + return Optional.of(notePosition.toPose2d()); + } else { + return Optional.empty(); + } + } + + private Pose2d getProjectedPoint() { + return this.pose.getCurrentPose2d().plus(new Transform2d(-0.4, 0, new Rotation2d())); + } + + private void decideWhetherToGiveUpOrRotate() { + if (allowRotationSearch) { + log.info("Attempting to find one via rotation."); + noteAcquisitionMode = NoteAcquisitionMode.SearchViaRotation; + rotationSearchModeTimeoutTracker.start(); + } else { + log.info("Giving up."); + noteAcquisitionMode = NoteAcquisitionMode.GiveUp; + } + } + + private boolean hasSolidLockOnNoteWithCenterCamera() { + // If the note is roughly centered on the center camera, we can try driving to it. + var target = vision.getCenterCamLargestNoteTarget(); + if (target.isPresent()) { + return Math.abs(target.get().getYaw()) < 15; + } + return false; + } + } diff --git a/src/main/java/competition/subsystems/vision/TimeoutTracker.java b/src/main/java/competition/subsystems/vision/TimeoutTracker.java new file mode 100644 index 00000000..0a5371ab --- /dev/null +++ b/src/main/java/competition/subsystems/vision/TimeoutTracker.java @@ -0,0 +1,31 @@ +package competition.subsystems.vision; + +import xbot.common.controls.sensors.XTimer; + +import java.util.function.Supplier; + +public class TimeoutTracker { + + private Supplier durationSupplier; + private double startTime = Double.MAX_VALUE; + + public TimeoutTracker(Supplier durationSupplier) { + this.durationSupplier = durationSupplier; + } + + public void reset() { + startTime = Double.MAX_VALUE; + } + + public void start() { + startTime = XTimer.getFPGATimestamp(); + } + + public double getTimeRemaining() { + return startTime + durationSupplier.get() - XTimer.getFPGATimestamp(); + } + + public boolean getTimedOut() { + return getTimeRemaining() < 0; + } +}