diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java index d63c7f26..03f9925a 100644 --- a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java +++ b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java @@ -7,6 +7,7 @@ import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand; import competition.subsystems.drive.commands.DriveToListOfPointsCommand; import competition.subsystems.pose.PoseSubsystem; +import competition.subsystems.vision.VisionSubsystem; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -29,7 +30,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector Provider driveToGivenNoteAndCollectCommandGroupProvider, Provider fireFromSubwooferCommandGroup, Provider driveToCentralSubwooferCommandProvider, - PoseSubsystem pose, DriveSubsystem drive) { + PoseSubsystem pose, DriveSubsystem drive, VisionSubsystem vision) { this.autoSelector = autoSelector; // Force our location @@ -50,6 +51,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector }) ); var driveToMiddleSpikeNoteAndCollect = driveToGivenNoteAndCollectCommandGroupProvider.get(); + driveToMiddleSpikeNoteAndCollect.setMaximumSpeedOverride(vision.getSpeedForAuto()); this.addCommands(driveToMiddleSpikeNoteAndCollect.withTimeout(interstageTimeout)); // Drive back to subwoofer @@ -68,6 +70,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector }) ); var driveToTopSpikeNoteAndCollect = driveToGivenNoteAndCollectCommandGroupProvider.get(); + driveToTopSpikeNoteAndCollect.setMaximumSpeedOverride(vision.getSpeedForAuto()); this.addCommands(driveToTopSpikeNoteAndCollect.withTimeout(interstageTimeout)); // Drive back to subwoofer @@ -94,6 +97,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector // Now, go get the bottom spike note var driveToBottomSpikeNoteAndCollect = driveToGivenNoteAndCollectCommandGroupProvider.get(); + driveToBottomSpikeNoteAndCollect.setMaximumSpeedOverride(vision.getSpeedForAuto()); this.addCommands(driveToBottomSpikeNoteAndCollect.withTimeout(interstageTimeout)); // Drive back to subwoofer diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 84129c73..df311063 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -38,6 +38,7 @@ public class DriveSubsystem extends BaseSwerveDriveSubsystem implements DataFram private Translation2d specialPointAtPositionTarget = new Translation2d(); private final DoubleProperty suggestedAutonomousMaximumSpeed; private final DoubleProperty suggestedAutonomousExtremeSpeed; + private final DoubleProperty suggestedAutonomousSpeedIfNotVisionAssisted; private final PIDManager aggressiveGoalHeadingPidManager; @Inject @@ -54,6 +55,8 @@ public DriveSubsystem(PIDManagerFactory pidFactory, PropertyFactory pf, pf.createPersistentProperty("Suggested Autonomous Maximum Speed", 3.0); suggestedAutonomousExtremeSpeed = pf.createPersistentProperty("Suggested Autonomous EXTREME Speed", 5.0); + suggestedAutonomousSpeedIfNotVisionAssisted = + pf.createPersistentProperty("Suggested Autonomous NO VISION Speed", 1.5); aggressiveGoalHeadingPidManager = aggressiveGoalHeadingPidFactory.create( this.getPrefix() + "AggressiveGoalHeadingPID", @@ -77,6 +80,9 @@ public double getSuggestedAutonomousMaximumSpeed() { } public double getSuggestedAutonomousExtremeSpeed() { return suggestedAutonomousExtremeSpeed.get(); } + public double getSuggestedAutonomousNoAutoSpeed() { + return suggestedAutonomousSpeedIfNotVisionAssisted.get(); + } @Override protected PIDDefaults getPositionalPIDDefaults() { diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionAndVisionAssistanceConsiderationCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionAndVisionAssistanceConsiderationCommand.java new file mode 100644 index 00000000..ec107a5b --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionAndVisionAssistanceConsiderationCommand.java @@ -0,0 +1,32 @@ +package competition.subsystems.drive.commands; + +import competition.subsystems.collector.CollectorSubsystem; +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.oracle.DynamicOracle; +import competition.subsystems.pose.PoseSubsystem; +import competition.subsystems.vision.NoteSeekLogic; +import competition.subsystems.vision.VisionSubsystem; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.drive.control_logic.HeadingModule; + +import javax.inject.Inject; + +public class DriveToGivenNoteWithBearingVisionAndVisionAssistanceConsiderationCommand extends DriveToGivenNoteWithBearingVisionCommand{ + + VisionSubsystem vision; + + @Inject + DriveToGivenNoteWithBearingVisionAndVisionAssistanceConsiderationCommand( + PoseSubsystem pose, DriveSubsystem drive, DynamicOracle oracle, + PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, + VisionSubsystem vision, CollectorSubsystem collector, NoteSeekLogic noteSeekLogic) { + super(pose, drive, oracle, pf, headingModuleFactory, vision, collector, noteSeekLogic); + this.vision = vision; + } + + @Override + public void execute() { + super.setMaximumSpeedOverride(vision.getSpeedForAuto()); + super.execute(); + } +} diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java index 3d653ce5..b7ed5644 100644 --- a/src/main/java/competition/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.vision; import competition.electrical_contract.CompetitionContract; +import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -39,6 +40,7 @@ @Singleton public class VisionSubsystem extends BaseSubsystem implements DataFrameRefreshable { + DriveSubsystem drive; final RobotAssertionManager assertionManager; final BooleanProperty isInverted; final DoubleProperty yawOffset; @@ -73,11 +75,14 @@ public class VisionSubsystem extends BaseSubsystem implements DataFrameRefreshab public final double terminalNotePitch = 0.0; public final DoubleProperty terminalNoteYawRange; + BooleanProperty usingVisionForAuto; @Inject - public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalContract, RobotAssertionManager assertionManager) { + public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalContract, RobotAssertionManager assertionManager, + DriveSubsystem drive) { this.assertionManager = assertionManager; + this.drive = drive; pf.setPrefix(this); isInverted = pf.createPersistentProperty("Yaw inverted", true); @@ -89,6 +94,8 @@ public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalC minNoteConfidence = pf.createPersistentProperty("Min note confidence", 0.8); minNoteArea = pf.createPersistentProperty("Minimum note area", 0.5); + usingVisionForAuto = pf.createPersistentProperty("Using vision for auto", true); + terminalNoteYawRange = pf.createPersistentProperty("Terminal Note Yaw Range", 5.0); bestRangeFromStaticNoteToSearchForNote = pf.createPersistentProperty("BestRangeFromStaticNoteToSearchForNote", 1.2); @@ -487,4 +494,12 @@ else if (allCameras.stream().allMatch(state -> !state.isCameraWorking())) { // If some of the cameras are working, return 2 return 2; } + + public double getSpeedForAuto() { + if (usingVisionForAuto.get()) { + return drive.getSuggestedAutonomousExtremeSpeed(); + } else { + return drive.getSuggestedAutonomousNoAutoSpeed(); + } + } }