diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index fd5d2a28..4bd97383 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit fd5d2a286b21633db0dc12b72020d00ff76a1df8 +Subproject commit 4bd97383e5715c44a5ae6230657befb6b51246fa diff --git a/src/main/java/competition/auto_programs/DistanceShotFromMidShootThenShootNearestThree.java b/src/main/java/competition/auto_programs/DistanceShotFromMidShootThenShootNearestThree.java index e39c35dc..885a9e3a 100644 --- a/src/main/java/competition/auto_programs/DistanceShotFromMidShootThenShootNearestThree.java +++ b/src/main/java/competition/auto_programs/DistanceShotFromMidShootThenShootNearestThree.java @@ -27,7 +27,7 @@ public DistanceShotFromMidShootThenShootNearestThree(AutonomousCommandSelector a // Force our location var startInFrontOfSpeaker = pose.createSetPositionCommand( - () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation)); + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation)); this.addCommands(startInFrontOfSpeaker); // Fire note into the speaker from starting position diff --git a/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java b/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java index 616fc8c3..c76e9a8e 100644 --- a/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java +++ b/src/main/java/competition/auto_programs/FromMidShootCollectShoot.java @@ -9,7 +9,6 @@ import competition.subsystems.shooter.commands.FireWhenReadyCommand; import competition.subsystems.shooter.commands.WarmUpShooterCommand; import edu.wpi.first.wpilibj2.command.Commands; -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.SwerveSimpleTrajectoryCommand; @@ -38,7 +37,7 @@ public FromMidShootCollectShoot( this.autoSelector = autoSelector; // Force our location var startInFrontOfSpeaker = pose.createSetPositionCommand( - () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation)); + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation)); this.addCommands(startInFrontOfSpeaker); // Shoot the pre-loaded note from the subwoofer @@ -75,7 +74,7 @@ public FromMidShootCollectShoot( driveToSubwoofer.logic.setConstantVelocity(1); driveToSubwoofer.logic.setKeyPointsProvider(() -> { ArrayList points = new ArrayList<>(); - var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation); + var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation); points.add(new XbotSwervePoint(target.getTranslation(), target.getRotation(), 10)); return points; }); diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java index f1b0eb17..ee1a3e3b 100644 --- a/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java +++ b/src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java @@ -2,7 +2,6 @@ import competition.commandgroups.DriveToGivenNoteAndCollectCommandGroup; import competition.commandgroups.FireFromSubwooferCommandGroup; -import competition.commandgroups.FireNoteCommandGroup; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand; import competition.subsystems.pose.PoseSubsystem; @@ -28,7 +27,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector // Force our location var startInFrontOfSpeaker = pose.createSetPositionCommand( - () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation)); + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation)); this.addCommands(startInFrontOfSpeaker); // Fire preload note into the speaker from starting position diff --git a/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java b/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java index 87fb8a77..037ab6fb 100644 --- a/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java +++ b/src/main/java/competition/auto_programs/TwoNoteGriefAuto.java @@ -1,11 +1,8 @@ package competition.auto_programs; -import competition.commandgroups.FireNoteCommandGroup; import competition.subsystems.arm.ArmSubsystem; import competition.subsystems.arm.commands.SetArmAngleCommand; -import competition.subsystems.collector.commands.EjectCollectorCommand; import competition.subsystems.collector.commands.IntakeUntilNoteCollectedCommand; -import competition.subsystems.collector.commands.StopCollectorCommand; import competition.subsystems.oracle.DynamicOracle; import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.schoocher.commands.EjectScoocherCommand; @@ -13,17 +10,12 @@ import competition.subsystems.shooter.ShooterWheelSubsystem; import competition.subsystems.shooter.commands.FireWhenReadyCommand; import competition.subsystems.shooter.commands.WarmUpShooterCommand; -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.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; -import xbot.common.subsystems.pose.BasePoseSubsystem; import xbot.common.trajectory.XbotSwervePoint; import javax.inject.Inject; @@ -59,7 +51,7 @@ public TwoNoteGriefAuto(Provider swerveProvider, //starts us in front of the subwoofer, to score var startInFrontOfSpeaker = pose.createSetPositionCommand( - () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation)); + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation)); this.addCommands(startInFrontOfSpeaker); //fires the note we are holding at the start diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 7560faf3..ae675ed7 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -1,24 +1,13 @@ package competition.operator_interface; -import javax.inject.Inject; -import javax.inject.Provider; -import javax.inject.Singleton; - -import competition.auto_programs.DistanceShotFromMidShootThenShootNearestThree; -import competition.auto_programs.ShootThenMoveOutOfLine; -import competition.commandgroups.FireNoteCommandGroup; import competition.auto_programs.FromMidShootCollectShoot; -import competition.auto_programs.SubwooferShotFromMidShootThenShootNearestThree; -import competition.commandgroups.DriveToGivenNoteAndCollectCommandGroup; import competition.commandgroups.PrepareToFireAtAmpCommandGroup; import competition.commandgroups.PrepareToFireAtSpeakerCommandGroup; import competition.subsystems.arm.ArmSubsystem; -import competition.subsystems.arm.commands.ArmMaintainerCommand; import competition.subsystems.arm.commands.CalibrateArmsManuallyCommand; import competition.subsystems.arm.commands.ContinuouslyPointArmAtSpeakerCommand; import competition.subsystems.arm.commands.DisengageBrakeCommand; import competition.subsystems.arm.commands.EngageBrakeCommand; -import competition.subsystems.arm.commands.ManipulateArmBrakeCommand; import competition.subsystems.arm.commands.SetArmAngleCommand; import competition.subsystems.arm.commands.SetArmExtensionCommand; import competition.subsystems.collector.commands.EjectCollectorCommand; @@ -26,11 +15,10 @@ import competition.subsystems.collector.commands.IntakeCollectorCommand; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.drive.commands.AlignToNoteCommand; -import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand; -import competition.subsystems.oracle.SuperstructureAccordingToOracleCommand; -import competition.subsystems.oracle.SwerveAccordingToOracleCommand; import competition.subsystems.oracle.DynamicOracle; import competition.subsystems.oracle.ManualRobotKnowledgeSubsystem; +import competition.subsystems.oracle.SuperstructureAccordingToOracleCommand; +import competition.subsystems.oracle.SwerveAccordingToOracleCommand; import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.schoocher.commands.EjectScoocherCommand; import competition.subsystems.schoocher.commands.IntakeScoocherCommand; @@ -49,6 +37,9 @@ import xbot.common.trajectory.LowResField; import xbot.common.trajectory.XbotSwervePoint; +import javax.inject.Inject; +import javax.inject.Provider; +import javax.inject.Singleton; import java.util.ArrayList; /** @@ -132,9 +123,18 @@ public void setupMobilityComands( { double typicalVelocity = 2.5; // Manipulate heading and position for easy testing - resetHeading.setHeadingToApply(180); - var teleportRobot = pose.createSetPositionCommand(PoseSubsystem.SubwooferCentralScoringLocation); - operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(teleportRobot); + resetHeading.setHeadingToApply(() -> PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180)).getDegrees()); + + var teleportRobotToSubwooferTop = pose.createSetPositionCommand( + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferTopScoringLocation)); + operatorInterface.driverGamepad.getXboxButton(XboxButton.Y).onTrue(teleportRobotToSubwooferTop); + var teleportRobotToSubwooferMid = pose.createSetPositionCommand( + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation)); + operatorInterface.driverGamepad.getXboxButton(XboxButton.X).onTrue(teleportRobotToSubwooferMid); + var teleportRobotToSubwooferBottom = pose.createSetPositionCommand( + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation)); + operatorInterface.driverGamepad.getXboxButton(XboxButton.A).onTrue(teleportRobotToSubwooferBottom); + operatorInterface.driverGamepad.getXboxButton(XboxButton.Start).onTrue(resetHeading); LowResField fieldWithObstacles = oracle.getFieldWithObstacles(); @@ -182,7 +182,7 @@ public void setupMobilityComands( var goToAmp = createAndConfigureTypicalSwerveCommand( swerveCommandProvider.get(), PoseSubsystem.AmpScoringLocation, typicalVelocity, fieldWithObstacles); var goToSpeaker = createAndConfigureTypicalSwerveCommand( - swerveCommandProvider.get(), PoseSubsystem.SubwooferCentralScoringLocation, typicalVelocity, fieldWithObstacles); + swerveCommandProvider.get(), PoseSubsystem.BlueSubwooferCentralScoringLocation, typicalVelocity, fieldWithObstacles); // 3) Or pick up a new note from the source! var goToNoteSource = createAndConfigureTypicalSwerveCommand( @@ -226,6 +226,7 @@ public void setupOracleCommands(OperatorInterface oi, oi.driverGamepad.getXboxButton(XboxButton.Back).whileTrue(driveAccoringToOracle.alongWith(superstructureAccordingToOracle)); oi.driverGamepad.getPovIfAvailable(0).onTrue(new InstantCommand(() -> oracle.resetNoteMap())); + oi.driverGamepad.getPovIfAvailable(270).onTrue(new InstantCommand(() -> oracle.freezeConfigurationForAutonomous())); } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToCentralSubwooferCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToCentralSubwooferCommand.java index 1f5f143d..a1d7210f 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToCentralSubwooferCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToCentralSubwooferCommand.java @@ -32,7 +32,7 @@ public void initialize() { log.info("Intitializing"); ArrayList swervePoints = new ArrayList<>(); swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - PoseSubsystem.SubwooferCentralScoringLocation.getTranslation(), Rotation2d.fromDegrees(180), 10)); + PoseSubsystem.BlueSubwooferCentralScoringLocation.getTranslation(), Rotation2d.fromDegrees(180), 10)); this.logic.setKeyPoints(swervePoints); this.logic.setEnableConstantVelocity(true); this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); diff --git a/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java index 707168f5..50a0583a 100644 --- a/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java @@ -67,6 +67,9 @@ public SwerveDriveWithJoysticksCommand( this.triggerOnlyPowerScaling = pf.createPersistentProperty("TriggerOnlyPowerScaling", 0.75); this.triggerOnlyExponent = pf.createPersistentProperty("TriggerOnlyExponent", 2.0); + // We are already applying deadband to the joysticks; this additional layer of deadband is unnecessary. + decider.setDeadband(0.001); + // Set up a latch to trigger whenever we change the rotational mode. In either case, // there's some PIDs that will need to be reset, or goals that need updating. absoluteOrientationLatch = new Latch(absoluteOrientationMode.get(), EdgeType.Both, edge -> { @@ -126,19 +129,16 @@ public void execute() { // Grab all human sources of rotation intent double humanRotateIntentFromTriggers = getRotationIntentFromDriverTriggers(); - double humanRotateIntentFromStick = getRotationIntentFromDriverJoystick(); // Fuse them together while keeping them in the -1 to 1 range. This is to help avoid doing some kind of // conflicting move like trying to rotate in two directions at once. - double fusedHumanRotateIntent = MathUtils.constrainDoubleToRobotScale( - humanRotateIntentFromStick + humanRotateIntentFromTriggers); double rotateIntent = 0; if (absoluteOrientationMode.get()) { rotateIntent = getSuggestedRotateIntentForAbsoluteStickControl(humanRotateIntentFromTriggers); } else { // If we are in the typical "rotate using joystick to turn" mode, use the Heading Assist module to get the suggested power. - rotateIntent = scaleHumanRotationInput(fusedHumanRotateIntent); + rotateIntent = scaleHumanRotationInput(humanRotateIntentFromTriggers); } // -------------------------------------------------- @@ -305,7 +305,13 @@ private XYPair getTranslationIntentFromDriver() { (a) -> MathUtils.exponentAndRetainSign(a, (int) input_exponent.get())); // create new vector with the scaled magnitude and angle - XYPair translationIntent = XYPair.fromPolar(rawAngle-90, updatedMagnitude); + // However, if we are on the red alliance, we need to rotate the intent a lot more. + double angleOffset = 90; + if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red) { + angleOffset = 270; + } + + XYPair translationIntent = XYPair.fromPolar(rawAngle-angleOffset, updatedMagnitude); return translationIntent; } diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 6ab4a6ac..0658c044 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -46,6 +46,8 @@ public enum ScoringSubGoals { ArmSubsystem arm; int instructionNumber = 0; + double robotWidth = 0.914; + double scoringZoneOffset = 0.93; @Inject public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiringInfoSource noteFiringInfoSource, @@ -55,10 +57,6 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri 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; @@ -66,9 +64,6 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri this.currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; firstRunInNewGoal = true; setupLowResField(); - - //reserveNote(Note.KeyNoteNames.BlueSpikeMiddle); - //reserveNote(Note.KeyNoteNames.BlueSpikeBottom); } Pose2d activeScoringPosition; @@ -83,6 +78,19 @@ private void reserveNote(Note.KeyNoteNames specificNote) { noteCount++; } + private void createNoteLinkingObstacle(Note.KeyNoteNames firstNote, Note.KeyNoteNames secondNote) { + var first = noteMap.get(firstNote); + var second = noteMap.get(secondNote); + var obstacle = new Obstacle( + (first.getLocation().getTranslation().getX() + second.getLocation().getTranslation().getX()) / 2, + (first.getLocation().getTranslation().getY() + second.getLocation().getTranslation().getY()) / 2, + 1, + 1, + "Linker" + ); + field.addObstacle(obstacle); + } + private void createRobotObstacle(Translation2d location, double sideLength, String name) { field.addObstacle(new Obstacle(location.getX(), location.getY(), sideLength, sideLength, name)); } @@ -101,40 +109,147 @@ private Obstacle createObstacleWithRobotWidth(double x, double y, double width, return obstacle; } - private LowResField setupLowResField() { - field = new LowResField(); - // For now, just add the three columns in the middle. - // !!These values seem incorrect on simulator!!. - // createObstacleWithRobotWidth(3.2004, 4.105656, 0.254,0.254, .914, "BlueLeftStageColumn", field); - // widths and height are different to account for angle differences - //createObstacleWithRobotWidth(5.8129, 5.553456, 0.3469,0.3469, .914, "BlueTopStageColumn", field); - //createObstacleWithRobotWidth(5.8129, 2.657856,0.3469, 0.3469, .914, "BlueBottomStageColumn", field); - - // Blue obstacles + /** + * Helper function to create Obstacles for each of the permanent, non-changing aspects of the field, like + * the Subwoofer and Stage columns. + */ + private void setupPermanentObstacles() { + // Blue Stage createObstacleWithRobotWidth(PoseSubsystem.BlueLeftStageColumn, - PoseSubsystem.closeColumnWidth, PoseSubsystem.closeColumnWidth, .914, "BlueLeftStageColumn", field); + PoseSubsystem.closeColumnWidth, PoseSubsystem.closeColumnWidth, robotWidth, "BlueLeftStageColumn", field); createObstacleWithRobotWidth(PoseSubsystem.BlueTopStageColumn, - PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914,"BlueTopStageColumn", field); + PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, robotWidth,"BlueTopStageColumn", field); createObstacleWithRobotWidth(PoseSubsystem.BlueBottomStageColumn, - PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914, "BlueBottomStageColumn", field); + PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, robotWidth, "BlueBottomStageColumn", field); - var speaker = createObstacleWithRobotWidth(PoseSubsystem.BlueSubwoofer, - PoseSubsystem.SubwooferWidth, PoseSubsystem.SubwooferHeight, .914, "BlueSubwoofer", field); - speaker.defaultBottomLeft = false; - speaker.defaultTopLeft = false; - // Red obstacles + // Red Stage createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueLeftStageColumn), - PoseSubsystem.closeColumnWidth, PoseSubsystem.closeColumnWidth, .914, "RedLeftStageColumn", field); + PoseSubsystem.closeColumnWidth, PoseSubsystem.closeColumnWidth, robotWidth, "RedLeftStageColumn", field); createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueTopStageColumn), - PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914, "RedTopStageColumn", field); + PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, robotWidth, "RedTopStageColumn", field); createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueBottomStageColumn), - PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, .914, "RedBottomStageColumn", field); + PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, robotWidth, "RedBottomStageColumn", field); + + // Subwoofers + setupSubwooferTriad(PoseSubsystem.BlueSubwoofer, DriverStation.Alliance.Blue); + setupSubwooferTriad(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), DriverStation.Alliance.Red); + } + + /** + * Due to the way obstacles can be combined after the fact, it's easier to create the subwoofer as 3 separate + * obstacles, one for each of the scoring zones. + * @param subwooferCoreCoordinate The center of the overall subwoofer + * @param yOffset How many meters to adjust this particular component + * @param height The height in meterse of this particualr component + * @param alliance Is this the Blue or Red subwoofer? + * @param name A unique name for this component, like SubwooferTop. + */ + private void setupSubwooferComponent(Translation2d subwooferCoreCoordinate, double yOffset, double height, DriverStation.Alliance alliance, String name) { + var speaker = new Obstacle( + subwooferCoreCoordinate.getX(), + subwooferCoreCoordinate.getY()+yOffset, + PoseSubsystem.SubwooferWidth + robotWidth, + height, + alliance+name); + if (alliance== DriverStation.Alliance.Blue) { + speaker.defaultBottomLeft = false; + speaker.defaultTopLeft = false; + } else { + speaker.defaultBottomRight = false; + speaker.defaultTopRight = false; + } + field.addObstacle(speaker); + } + + /** + * Create the 3 subwoofer components, top/middle/bottom. + * @param subwooferCoreCoordinate The center of the overall subwoofer + * @param alliance Is this the Blue or Red subwoofer? + */ + private void setupSubwooferTriad(Translation2d subwooferCoreCoordinate, DriverStation.Alliance alliance) { + setupSubwooferComponent(subwooferCoreCoordinate, scoringZoneOffset, PoseSubsystem.SubwooferHeight / 6, alliance, "SubwooferTop"); + setupSubwooferComponent(subwooferCoreCoordinate, 0, 1.25, alliance, "SubwooferMid"); + setupSubwooferComponent(subwooferCoreCoordinate, -scoringZoneOffset, PoseSubsystem.SubwooferHeight / 6, alliance, "SubwooferBottom"); + } + + /** + * Reserve a scoring location, marking it inaccessible to the robot during autonomous. + * @param location The scoring location to mark as reserved by others during auto + */ + public void reserveScoringLocationForOtherTeams(ScoringLocation.WellKnownScoringLocations location) { + // Mark it as reserved in the map, so we don't try to navigate there + scoringLocationMap.get(location).setAvailability(Availability.ReservedByOthersInAuto); + // create the relevant obstacle so we path around any robot chilling there + + double xCoordinate = 0; + double yCoordinate = 0; + DriverStation.Alliance alliance = DriverStation.Alliance.Blue; + String locationName = ""; + + + switch (location) { + case SubwooferTopBlue -> { + xCoordinate = PoseSubsystem.BlueSubwoofer.getX(); + yCoordinate = PoseSubsystem.SpikeTop.getY(); + locationName = "SubwooferTopScoring"; + } + case SubwooferMiddleBlue -> { + xCoordinate = PoseSubsystem.BlueSubwoofer.getX()+1; + yCoordinate = PoseSubsystem.SpikeMiddle.getY(); + locationName = "SubwooferMiddleScoring"; + } + case SubwooferBottomBlue -> { + xCoordinate = PoseSubsystem.BlueSubwoofer.getX(); + yCoordinate = PoseSubsystem.SpikeBottom.getY(); + locationName = "SubwooferBottomScoring"; + } + case SubwooferTopRed -> { + xCoordinate = BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer).getX(); + yCoordinate = PoseSubsystem.SpikeTop.getY(); + alliance = DriverStation.Alliance.Red; + locationName = "SubwooferTopScoring"; + } + case SubwooferMiddleRed -> { + xCoordinate = BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer).getX()+1; + yCoordinate = PoseSubsystem.SpikeMiddle.getY(); + alliance = DriverStation.Alliance.Red; + locationName = "SubwooferMiddleScoring"; + } + case SubwooferBottomRed -> { + xCoordinate = BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer).getX(); + yCoordinate = PoseSubsystem.SpikeBottom.getY(); + alliance = DriverStation.Alliance.Red; + locationName = "SubwooferBottomScoring"; + } + default -> { + // No obstacle created; for now we only care about ones where robots are likely to start. + return; + } + } + + var obstacle = new Obstacle( + xCoordinate, + yCoordinate, + 1.25, + 1.25, + alliance.toString() + locationName); + + if (alliance == DriverStation.Alliance.Blue) { + obstacle.defaultBottomLeft = false; + obstacle.defaultTopLeft = false; + } else { + obstacle.defaultBottomRight = false; + obstacle.defaultTopRight = false; + } + + field.addObstacle(obstacle); + } + + private LowResField setupLowResField() { + field = new LowResField(); - speaker = createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), - PoseSubsystem.SubwooferWidth, PoseSubsystem.SubwooferHeight, .914, "RedSubwoofer", field); - speaker.defaultBottomLeft = false; - speaker.defaultTopLeft = false; + setupPermanentObstacles(); return field; } @@ -165,15 +280,34 @@ public void requestReevaluation() { * 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() { + noteMap = new NoteMap(); + scoringLocationMap = new ScoringLocationMap(); + + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + noteMap.get(Note.KeyNoteNames.RedSpikeTop).setAvailability(Availability.Unavailable); + noteMap.get(Note.KeyNoteNames.RedSpikeMiddle).setAvailability(Availability.Unavailable); + noteMap.get(Note.KeyNoteNames.RedSpikeBottom).setAvailability(Availability.Unavailable); + + scoringLocationMap.markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance.Red); + } else { + noteMap.get(Note.KeyNoteNames.BlueSpikeTop).setAvailability(Availability.Unavailable); + noteMap.get(Note.KeyNoteNames.BlueSpikeMiddle).setAvailability(Availability.Unavailable); + noteMap.get(Note.KeyNoteNames.BlueSpikeBottom).setAvailability(Availability.Unavailable); + + scoringLocationMap.markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance.Blue); + } + + // TODO: Read the values from the neotrellis to reserve more notes / scoring locations. } + /** + * The core state machine for the Oracle. Reads everything it can from the robot and its environment, + * and then recommends actions. Other commands like SwerveAcordingToOracle or + * SuperstructureControlAccordingToOracle may act on those recommendations. + */ @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: @@ -183,7 +317,7 @@ public void periodic() { Availability.Available).getLocation()); currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; - setSpecialAimTarget(new Pose2d(0, 5.5, Rotation2d.fromDegrees(0))); + setSpecialAimTarget(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SPEAKER_AIM_TARGET)); // Choose a good speaker scoring location // Publish a route from current position to that location firstRunInNewGoal = false; @@ -271,6 +405,12 @@ else if (suggestedNote.getAvailability() == Availability.AgainstObstacle) { }); } + /** + * Helper function that visualizes the bounding box of an obstacles using WPI trajectories, + * which draw nicely on AdvantageScope + * @param o The obstacle to visualize + * @return A WPI trajectory that can be visualized by AdvantageScope + */ private Trajectory obstacleToTrajectory(Obstacle o) { // create a trajectory using the 4 corners of the obstacle. ArrayList wpiStates = new ArrayList<>(); @@ -345,7 +485,11 @@ private void determineScoringSubgoal() { acceptableRangeBeforeScoringMeters = 0.05; } - if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters) && inUnderstoodRange) { + // TODO: also need to add a check to make sure our angular error is small enough + double angularError = Math.abs(pose.getAngularErrorToTranslation2dInDegrees(specialAimTarget.getTranslation())); + aKitLog.record("AngularErrorToSpecialTarget", angularError); + boolean pointingAtSpeaker = angularError < 6.0; + if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters) && inUnderstoodRange && pointingAtSpeaker) { currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; } else { currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; diff --git a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java index 3394e0e1..d081d586 100644 --- a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -15,21 +15,21 @@ private void initializeScoringLocations() { } private void initializeBlueScoringLocations() { - add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, - new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue, - new ScoringLocation(PoseSubsystem.SubwooferTopScoringLocation, Availability.Available)); + new ScoringLocation(PoseSubsystem.BlueSubwooferTopScoringLocation, Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, + new ScoringLocation(PoseSubsystem.BlueSubwooferCentralScoringLocation, Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, - new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, Availability.Available)); + new ScoringLocation(PoseSubsystem.BlueSubwooferBottomScoringLocation, 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)); + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.BlueSubwooferTopScoringLocation), Availability.Available)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed, + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.BlueSubwooferCentralScoringLocation), Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, - new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.Available)); + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.BlueSubwooferBottomScoringLocation), Availability.Available)); } public void markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance alliance) { diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index c727acb5..088276a0 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -8,11 +8,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import xbot.common.controls.sensors.XGyro.XGyroFactory; import xbot.common.logic.Latch; @@ -27,8 +24,6 @@ import javax.inject.Inject; import javax.inject.Singleton; - - import java.util.Optional; import java.util.function.Supplier; @@ -55,6 +50,7 @@ public class PoseSubsystem extends BasePoseSubsystem { private final Latch useVisionToUpdateGyroLatch; public static final Translation2d SPEAKER_POSITION = new Translation2d(-0.0381,5.547868); + public static final Pose2d SPEAKER_AIM_TARGET = new Pose2d(0, 5.5, Rotation2d.fromDegrees(180)); 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()); @@ -74,14 +70,14 @@ public class PoseSubsystem extends BasePoseSubsystem { public static double SubwooferWidth = 0.95; public static double SubwooferHeight = 1.1; - public static Translation2d BlueSubwoofer = new Translation2d(0.415, 5.57); + public static Translation2d BlueSubwoofer = new Translation2d(0.415, 5.55); 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)); + public static Pose2d BlueSubwooferTopScoringLocation = new Pose2d(0.751, 6.702, Rotation2d.fromDegrees(-120)); + public static Pose2d BlueSubwooferCentralScoringLocation = new Pose2d(1.383, 5.54, Rotation2d.fromDegrees(180)); + public static Pose2d BlueSubwooferBottomScoringLocation = new Pose2d(0.758, 4.395, Rotation2d.fromDegrees(120)); private DoubleProperty matchTime; @@ -185,17 +181,16 @@ protected void updateOdometry() { ); if (isUsingVisionAssistedPose()) { - //improveFusedOdometryUsingPhotonLib(updatedPosition); + improveFusedOdometryUsingPhotonLib(updatedPosition); } aKitLog.record("VisionEstimate", fusedSwerveOdometry.getEstimatedPosition()); aKitLog.record("WheelsOnlyEstimate", onlyWheelsGyroSwerveOdometry.getEstimatedPosition()); - // Pull out the new estimated pose from odometry. Note that for now, we only pull out X and Y - // and trust the gyro implicitly. Eventually, we should allow the gyro to be updated via vision - // if we have a lot of confidence in the vision data. + // For now, we only trust the odometry and the gyro. We will continue to log the vision data, + // but the few inches of error we are seeing in the vision system are causing us to miss var estimatedPosition = new Pose2d( - fusedSwerveOdometry.getEstimatedPosition().getTranslation(), + onlyWheelsGyroSwerveOdometry.getEstimatedPosition().getTranslation(), getCurrentHeading()); totalDistanceX = estimatedPosition.getX(); @@ -353,6 +348,16 @@ public double getDistanceFromSpeaker(){ PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SPEAKER_POSITION)); return distanceFromSpeakerInMeters; } + + public double getAngularErrorToSpeakerInDegrees() { + return getAngularErrorToTranslation2dInDegrees(PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SPEAKER_POSITION)); + } + + public double getAngularErrorToTranslation2dInDegrees(Translation2d targetPosition) { + var angleFromChassisToTarget = WrappedRotation2d.fromDegrees(targetPosition.minus(getCurrentPose2d().getTranslation()).getAngle().getDegrees()); + return getCurrentHeading().minus(angleFromChassisToTarget).getDegrees(); + } + @Override public void periodic() { super.periodic();