From aa510a4ec737362fc5b4bcb04cdca4f5bf003bda Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 20:11:44 -0800 Subject: [PATCH 01/10] Use newer commonlib --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index fd5d2a28..caf9df9f 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit fd5d2a286b21633db0dc12b72020d00ff76a1df8 +Subproject commit caf9df9f2c7d4496602ce70721ba00d6c74f40db From 73d1fe2598797d3556fe032b40b5858f648eba47 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 20:12:00 -0800 Subject: [PATCH 02/10] Extra assistance when aiming at a goal --- .../subsystems/pose/PoseSubsystem.java | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index c727acb5..f6bd3bc3 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; @@ -74,7 +69,7 @@ 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)); @@ -185,17 +180,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 +347,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(); From dc656db96ea79249bfab8973bac6d6f4f5cdb218 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 20:13:20 -0800 Subject: [PATCH 03/10] More scaffolding for reserving positions for autonomous programs --- .../subsystems/oracle/DynamicOracle.java | 183 ++++++++++++++---- .../subsystems/oracle/ScoringLocationMap.java | 8 +- 2 files changed, 154 insertions(+), 37 deletions(-) diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 6ab4a6ac..2ee036f9 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,7 +57,7 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri this.noteMap = new NoteMap(); this.scoringLocationMap = new ScoringLocationMap(); - // TODO: adjust this during autonomous init + // TODO: adjust this during autonomous init rather than here noteMap.markAllianceNotesAsUnavailable(DriverStation.Alliance.Red); scoringLocationMap.markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance.Red); @@ -67,8 +69,16 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri firstRunInNewGoal = true; setupLowResField(); - //reserveNote(Note.KeyNoteNames.BlueSpikeMiddle); - //reserveNote(Note.KeyNoteNames.BlueSpikeBottom); + reserveNote(Note.KeyNoteNames.BlueSpikeTop); + reserveNote(Note.KeyNoteNames.BlueSpikeMiddle); + reserveNote(Note.KeyNoteNames.BlueSpikeBottom); + + reserveScoringLocation(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue); + reserveScoringLocation(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue); + //reserveScoringLocation(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue); + + createNoteLinkingObstacle(Note.KeyNoteNames.BlueSpikeTop, Note.KeyNoteNames.BlueSpikeMiddle); + createNoteLinkingObstacle(Note.KeyNoteNames.BlueSpikeMiddle, Note.KeyNoteNames.BlueSpikeBottom); } Pose2d activeScoringPosition; @@ -83,6 +93,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 +124,134 @@ 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 + 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, "Blue"); + setupSubwooferTriad(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), "Red"); + } + + private void setupSubwooferTriad(Translation2d subwooferCoreCoordinate, String alliance) { + var speakerTop = new Obstacle( + subwooferCoreCoordinate.getX(), + subwooferCoreCoordinate.getY()+scoringZoneOffset, + PoseSubsystem.SubwooferWidth + robotWidth, + PoseSubsystem.SubwooferHeight / 6, + alliance+"SubwooferTop"); + speakerTop.defaultBottomLeft = false; + speakerTop.defaultTopLeft = false; + field.addObstacle(speakerTop); + + var speakerMid = new Obstacle( + subwooferCoreCoordinate.getX(), + subwooferCoreCoordinate.getY(), + PoseSubsystem.SubwooferWidth + robotWidth, + 1.25, + alliance+"SubwooferMid"); + speakerMid.defaultBottomLeft = false; + speakerMid.defaultTopLeft = false; + field.addObstacle(speakerMid); + + var speakerBottom = new Obstacle( + subwooferCoreCoordinate.getX(), + subwooferCoreCoordinate.getY()-scoringZoneOffset, + PoseSubsystem.SubwooferWidth + robotWidth, + PoseSubsystem.SubwooferHeight / 6, + alliance+"SubwooferBottom"); + speakerBottom.defaultBottomLeft = false; + speakerBottom.defaultTopLeft = false; + field.addObstacle(speakerBottom); + } + + public void reserveScoringLocation(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; + } + } - speaker = createObstacleWithRobotWidth(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), - PoseSubsystem.SubwooferWidth, PoseSubsystem.SubwooferHeight, .914, "RedSubwoofer", field); - speaker.defaultBottomLeft = false; - speaker.defaultTopLeft = false; + 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(); + + setupPermanentObstacles(); return field; } @@ -170,10 +287,6 @@ public void freezeConfigurationForAutonomous() { @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: @@ -345,7 +458,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..e6c1de3c 100644 --- a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -15,19 +15,19 @@ 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)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, + new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue, new ScoringLocation(PoseSubsystem.SubwooferBottomScoringLocation, 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)); + add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleRed, + new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferCentralScoringLocation), Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferBottomRed, new ScoringLocation(PoseSubsystem.convertBluetoRed(PoseSubsystem.SubwooferBottomScoringLocation), Availability.Available)); } From 059c9d39ad32ef069be4ec58eb202561e7f1d73e Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:18:45 -0800 Subject: [PATCH 04/10] Cleanup and comments --- .../OperatorCommandMap.java | 21 +--- .../subsystems/oracle/DynamicOracle.java | 119 +++++++++++------- .../subsystems/pose/PoseSubsystem.java | 1 + 3 files changed, 80 insertions(+), 61 deletions(-) diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 7560faf3..bd0bd86b 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; @@ -41,7 +29,6 @@ import competition.subsystems.shooter.commands.WarmUpShooterCommand; import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; import xbot.common.controls.sensors.XXboxController.XboxButton; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; @@ -49,6 +36,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; /** @@ -226,6 +216,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/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index 2ee036f9..0658c044 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -57,10 +57,6 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri this.noteMap = new NoteMap(); this.scoringLocationMap = new ScoringLocationMap(); - // TODO: adjust this during autonomous init rather than here - noteMap.markAllianceNotesAsUnavailable(DriverStation.Alliance.Red); - scoringLocationMap.markAllianceScoringLocationsAsUnavailable(DriverStation.Alliance.Red); - this.pose = pose; this.arm = arm; @@ -68,17 +64,6 @@ public DynamicOracle(NoteCollectionInfoSource noteCollectionInfoSource, NoteFiri this.currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; firstRunInNewGoal = true; setupLowResField(); - - reserveNote(Note.KeyNoteNames.BlueSpikeTop); - reserveNote(Note.KeyNoteNames.BlueSpikeMiddle); - reserveNote(Note.KeyNoteNames.BlueSpikeBottom); - - reserveScoringLocation(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue); - reserveScoringLocation(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue); - //reserveScoringLocation(ScoringLocation.WellKnownScoringLocations.SubwooferBottomBlue); - - createNoteLinkingObstacle(Note.KeyNoteNames.BlueSpikeTop, Note.KeyNoteNames.BlueSpikeMiddle); - createNoteLinkingObstacle(Note.KeyNoteNames.BlueSpikeMiddle, Note.KeyNoteNames.BlueSpikeBottom); } Pose2d activeScoringPosition; @@ -124,6 +109,10 @@ private Obstacle createObstacleWithRobotWidth(double x, double y, double width, return obstacle; } + /** + * 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, @@ -143,43 +132,52 @@ private void setupPermanentObstacles() { PoseSubsystem.farColumnWidths, PoseSubsystem.farColumnWidths, robotWidth, "RedBottomStageColumn", field); // Subwoofers - setupSubwooferTriad(PoseSubsystem.BlueSubwoofer, "Blue"); - setupSubwooferTriad(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), "Red"); + setupSubwooferTriad(PoseSubsystem.BlueSubwoofer, DriverStation.Alliance.Blue); + setupSubwooferTriad(BasePoseSubsystem.convertBlueToRed(PoseSubsystem.BlueSubwoofer), DriverStation.Alliance.Red); } - private void setupSubwooferTriad(Translation2d subwooferCoreCoordinate, String alliance) { - var speakerTop = new Obstacle( - subwooferCoreCoordinate.getX(), - subwooferCoreCoordinate.getY()+scoringZoneOffset, - PoseSubsystem.SubwooferWidth + robotWidth, - PoseSubsystem.SubwooferHeight / 6, - alliance+"SubwooferTop"); - speakerTop.defaultBottomLeft = false; - speakerTop.defaultTopLeft = false; - field.addObstacle(speakerTop); - - var speakerMid = new Obstacle( + /** + * 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(), + subwooferCoreCoordinate.getY()+yOffset, PoseSubsystem.SubwooferWidth + robotWidth, - 1.25, - alliance+"SubwooferMid"); - speakerMid.defaultBottomLeft = false; - speakerMid.defaultTopLeft = false; - field.addObstacle(speakerMid); + height, + alliance+name); + if (alliance== DriverStation.Alliance.Blue) { + speaker.defaultBottomLeft = false; + speaker.defaultTopLeft = false; + } else { + speaker.defaultBottomRight = false; + speaker.defaultTopRight = false; + } + field.addObstacle(speaker); + } - var speakerBottom = new Obstacle( - subwooferCoreCoordinate.getX(), - subwooferCoreCoordinate.getY()-scoringZoneOffset, - PoseSubsystem.SubwooferWidth + robotWidth, - PoseSubsystem.SubwooferHeight / 6, - alliance+"SubwooferBottom"); - speakerBottom.defaultBottomLeft = false; - speakerBottom.defaultTopLeft = false; - field.addObstacle(speakerBottom); + /** + * 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"); } - public void reserveScoringLocation(ScoringLocation.WellKnownScoringLocations location) { + /** + * 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 @@ -282,8 +280,31 @@ 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() { @@ -296,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; @@ -384,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<>(); diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index f6bd3bc3..59d1bde9 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -50,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()); From e8d24130ffb360291ce5e70d17c3b790570bd951 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:55:18 -0800 Subject: [PATCH 05/10] Use newer commonlib --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index caf9df9f..4bd97383 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit caf9df9f2c7d4496602ce70721ba00d6c74f40db +Subproject commit 4bd97383e5715c44a5ae6230657befb6b51246fa From 825309c1d74f85ad63c68d13aa29d42cffba3447 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:55:50 -0800 Subject: [PATCH 06/10] Adding "Blue" to some locations for clarity --- .../DistanceShotFromMidShootThenShootNearestThree.java | 2 +- .../auto_programs/FromMidShootCollectShoot.java | 5 ++--- ...SubwooferShotFromMidShootThenShootNearestThree.java | 3 +-- .../competition/auto_programs/TwoNoteGriefAuto.java | 10 +--------- .../competition/subsystems/pose/PoseSubsystem.java | 6 +++--- 5 files changed, 8 insertions(+), 18 deletions(-) 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/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 59d1bde9..088276a0 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -75,9 +75,9 @@ public class PoseSubsystem extends BasePoseSubsystem { 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; From 77c164b74d06125a45a206d74824b4ab38f76ec0 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:56:00 -0800 Subject: [PATCH 07/10] Fix bugs in swerve drive --- .../SwerveDriveWithJoysticksCommand.java | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) 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; } From c5b59e589237dc6245608ff4a7ff085886773a2b Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:56:11 -0800 Subject: [PATCH 08/10] Name change and appease checkstyle --- .../subsystems/oracle/ScoringLocationMap.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java index e6c1de3c..d081d586 100644 --- a/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java +++ b/src/main/java/competition/subsystems/oracle/ScoringLocationMap.java @@ -16,20 +16,20 @@ private void initializeScoringLocations() { private void initializeBlueScoringLocations() { add(ScoringLocation.WellKnownScoringLocations.SubwooferTopBlue, - new ScoringLocation(PoseSubsystem.SubwooferTopScoringLocation, Availability.Available)); + new ScoringLocation(PoseSubsystem.BlueSubwooferTopScoringLocation, Availability.Available)); add(ScoringLocation.WellKnownScoringLocations.SubwooferMiddleBlue, - new ScoringLocation(PoseSubsystem.SubwooferCentralScoringLocation, Availability.Available)); + 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.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.SubwooferCentralScoringLocation), Availability.Available)); + 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) { From 2b916f397dcefa321c454355a02b065776d1b2a0 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:56:30 -0800 Subject: [PATCH 09/10] Add buttons to "teleport" robot to specific positions for testing auto starting positions. --- .../operator_interface/OperatorCommandMap.java | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index bd0bd86b..ae675ed7 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -29,6 +29,7 @@ import competition.subsystems.shooter.commands.WarmUpShooterCommand; import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; import xbot.common.controls.sensors.XXboxController.XboxButton; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; @@ -122,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(); @@ -172,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( From a777ed6b181c7ec5312522e5d153d9d913c8b374 Mon Sep 17 00:00:00 2001 From: John Gilbert Date: Fri, 1 Mar 2024 23:56:36 -0800 Subject: [PATCH 10/10] Absorb name change --- .../drive/commands/DriveToCentralSubwooferCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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());