diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 8fc23a7e..6be1c5f8 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 8fc23a7ebeb03c016e77ff97f98f9d414ee7ed69 +Subproject commit 6be1c5f86abe27a70331312291e0a36b03906d95 diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java index e78e9d35..af920db9 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToNearestReefFaceForAlgaeCommand.java @@ -2,18 +2,29 @@ import competition.operator_interface.OperatorInterface; import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.oracle.ReefCoordinateGenerator; +import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; 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.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; import xbot.common.command.BaseCommand; +import xbot.common.math.MathUtils; +import xbot.common.math.PIDDefaults; +import xbot.common.math.PIDManager; import xbot.common.math.XYPair; +import xbot.common.properties.DistanceProperty; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.drive.control_logic.HeadingModule; import javax.inject.Inject; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { @@ -24,33 +35,155 @@ public class AlignToNearestReefFaceForAlgaeCommand extends BaseCommand { final DriveSubsystem drive; final PoseSubsystem pose; final OperatorInterface oi; + final ReefCoordinateGenerator reefCoordinateGenerator; Translation2d idealFinalPosition; Angle idealFinalHeading; + private final PIDManager driveToAlgaePidManager; + private final PIDManager snapToAlgaePIDManager; + + final DistanceProperty interstitialThresholdToRailDrive; + final DistanceProperty interstitialDistance; + final DoubleProperty interstitialApproachSpeedFactor; + + public enum AlignmentState { + ToInterstitial, + RailDrive, + } + + public AlignmentState currentAlignmentState; + public Pose2d interstitialPoint; + @Inject public AlignToNearestReefFaceForAlgaeCommand(HeadingModule.HeadingModuleFactory headingModuleFactory, DriveSubsystem drive, PoseSubsystem pose, - AprilTagVisionSubsystemExtended vision, OperatorInterface oi) { - this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid()); + AprilTagVisionSubsystemExtended vision, OperatorInterface oi, + PIDManager.PIDManagerFactory pidFactory, PropertyFactory pf, + ReefCoordinateGenerator reefCoordinateGenerator) { this.vision = vision; this.drive = drive; this.pose = pose; this.oi = oi; + this.reefCoordinateGenerator = reefCoordinateGenerator; this.addRequirements(drive); + + driveToAlgaePidManager = pidFactory.create( + this.getPrefix() + "DriveToAlgaePositionalPID", + new PIDDefaults( + 0.75, // P + 0, // I + 4.0, // D + 0.0, // F + 0.6, // Max output + -0.6, // Min output + 0.05, // Error threshold + 0.005, // Derivative threshold + 0.2) // Time threshold + ); + driveToAlgaePidManager.setEnableErrorThreshold(true); + driveToAlgaePidManager.setEnableTimeThreshold(true); + + snapToAlgaePIDManager = pidFactory.create( + this.getPrefix() + "SnapToAlgaeHeadingPID", + new PIDDefaults( + 0.005, // P + 0, // I + 0, // D + 0.0, // F + 0.75, // Max output + -0.75, // Min output + 2.0, // Error threshold + 0.2, // Derivative threshold + 0.2) // Time threshold + ); + snapToAlgaePIDManager.setEnableErrorThreshold(true); + snapToAlgaePIDManager.setEnableTimeThreshold(true); + + this.headingModule = headingModuleFactory.create(snapToAlgaePIDManager); + + pf.setPrefix(this.getPrefix()); + interstitialThresholdToRailDrive = pf.createPersistentProperty("InterstitialThresholdToRailDrive", Meters.of(0.2)); + interstitialDistance = pf.createPersistentProperty("InterstitialDistance", Meters.of(1.75)); + interstitialApproachSpeedFactor = pf.createPersistentProperty("InterstitialApproachSpeedFactor", 0.5); } @Override public void initialize() { log.info("Initializing"); + Pose2d currentPose = pose.getCurrentPose2d(); - Pose2d closestPose = pose.getClosestReefFacePose(); + // Firstly, figure out if we are on red/blue side of the field, and we'll center accordingly + // There exist a possibility that we want to steal algae on the opposition's reef (although it goes both ways) + DriverStation.Alliance alliance = DriverStation.Alliance.Blue; + if (currentPose.getTranslation().getX() > PoseSubsystem.fieldXMidpointInMeters.in(Meters)) { + alliance = DriverStation.Alliance.Red; + } + + Pose2d closestPose = pose.getClosestReefFacePoseByAlliance(alliance); idealFinalPosition = closestPose.getTranslation(); - idealFinalHeading = closestPose.getRotation().getMeasure().plus(Degrees.of(180)); + idealFinalHeading = closestPose.getRotation().getMeasure().plus(Degrees.of(180)); // Aligning backwards + + double distanceToClosestReefFace = currentPose.getTranslation().getDistance(idealFinalPosition); + + // Determine our starting state, do we need to drive to an interstitial? + if (distanceToClosestReefFace < interstitialDistance.get().in(Meters)) { + currentAlignmentState = AlignmentState.RailDrive; + } else { + currentAlignmentState = AlignmentState.ToInterstitial; + interstitialPoint = reefCoordinateGenerator.getPoseRelativeToReefFace( + alliance, + Landmarks.getReefFaceFromTagId(vision.getClosestReefTagIdFromTranslation(idealFinalPosition)), + interstitialDistance.get(), + Meters.zero() + ); + + // Recall how we are aligning backwards + interstitialPoint = new Pose2d( + interstitialPoint.getTranslation(), + interstitialPoint.getRotation().plus(new Rotation2d(Math.PI)) + ); + } } @Override public void execute() { + switch (currentAlignmentState) { + case ToInterstitial -> { + driveToInterstitial(); + var currentTranslation = pose.getCurrentPose2d().getTranslation(); + double distanceFromInterstitial = currentTranslation.getDistance(interstitialPoint.getTranslation()); + if (distanceFromInterstitial < interstitialThresholdToRailDrive.get().in(Meters)) { + currentAlignmentState = AlignmentState.RailDrive; + } + } + case RailDrive -> railDrive(); + default -> {} + } + aKitLog.record("AlignmentState", currentAlignmentState); + } + + public void driveToInterstitial() { + Pose2d currentPose = pose.getCurrentPose2d(); + + // Same interstitial driving logic as AlignCameraToAprilTagCalculator + var vectorTowardsInterstitial = interstitialPoint.getTranslation().minus(currentPose.getTranslation()); + var normalizedVector = vectorTowardsInterstitial.div(vectorTowardsInterstitial.getNorm()); + var driveIntent = new XYPair( + normalizedVector.getX(), + normalizedVector.getY()).scale(interstitialApproachSpeedFactor.get() + ); + var rotationIntent = headingModule.calculateHeadingPower(interstitialPoint.getRotation()); + + drive.fieldOrientedDrive( + driveIntent, + rotationIntent, + pose.getCurrentHeading().getDegrees(), + new XYPair() + ); + } + + private void railDrive() { // Calculate the vector to center our robot on "rails" var currentTranslation = pose.getCurrentPose2d().getTranslation(); var currentHeading = pose.getCurrentHeading().getRadians(); @@ -58,14 +191,20 @@ public void execute() { double deltaY = idealFinalPosition.getY() - currentTranslation.getY(); double robotRelativeTagLocationY = deltaX * Math.sin(-currentHeading) + deltaY * Math.cos(-currentHeading); - // Everything below here can and will be extracted because it is repeated code with + // Everything below here can and should be extracted because it is repeated code with // DriveWithSnapToReefTagCommand - var centeringTranslation2d = drive.getPowerForRelativePositionChange(new Translation2d(0, robotRelativeTagLocationY)); + var centeringTranslation2d = drive.getPowerForRelativePositionChange( + driveToAlgaePidManager, + new Translation2d(0, robotRelativeTagLocationY) + ); XYPair centeringVector = new XYPair(centeringTranslation2d.getX(), centeringTranslation2d.getY()); centeringVector = centeringVector.rotate(pose.getCurrentHeading().getDegrees()); var fieldVectorTranslation2d = oi.driverGamepad.getLeftFieldOrientedVector(); - XYPair fieldVectorXYPair = new XYPair(fieldVectorTranslation2d.getX(), fieldVectorTranslation2d.getY()); + XYPair fieldVectorXYPair = new XYPair( + MathUtils.deadband(fieldVectorTranslation2d.getX(), 0.05), + MathUtils.deadband(fieldVectorTranslation2d.getY(), 0.05) + ); double railsSimilarityToDriver = fieldVectorXYPair.dotProduct( new XYPair( Math.cos(idealFinalHeading.in(Radians) + Math.PI), @@ -75,7 +214,7 @@ public void execute() { double rotateIntent = headingModule.calculateHeadingPower(idealFinalHeading.in(Degrees)); XYPair onRailsVector = XYPair.fromPolar(idealFinalHeading.in(Degrees), railsSimilarityToDriver); - var combinedVector = onRailsVector.clone().add(centeringVector); + var combinedVector = onRailsVector.add(centeringVector); drive.fieldOrientedDrive( combinedVector, diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 35c14841..189fd392 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -14,7 +14,6 @@ import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; import competition.subsystems.vision.CoprocessorCommunicationSubsystem; -import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -25,6 +24,7 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -39,7 +39,6 @@ import xbot.common.properties.Property; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.pose.BasePoseSubsystem; -import xbot.common.subsystems.vision.AprilTagVisionSubsystem; @Singleton public class PoseSubsystem extends BasePoseSubsystem { @@ -398,16 +397,32 @@ public void ingestSimulatedSwerveModulePositions(SwerveModulePosition[] position this.simulatedModulePositions = Optional.of(positions); } + public static List getBlueReefFacePoses() { + return Arrays.asList( + Landmarks.BlueCloseAlgae, + Landmarks.BlueCloseLeftAlgae, + Landmarks.BlueCloseRightAlgae, + Landmarks.BlueFarLeftAlgae, + Landmarks.BlueFarAlgae, + Landmarks.BlueFarRightAlgae); + } + public Pose2d getClosestReefFacePose() { Pose2d currentPose = getCurrentPose2d(); - List reefFacePoses = Arrays.asList( - convertBlueToRedIfNeeded(Landmarks.BlueCloseAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueCloseLeftAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueCloseRightAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueFarLeftAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueFarAlgae), - convertBlueToRedIfNeeded(Landmarks.BlueFarRightAlgae)); + List reefFacePoses = getBlueReefFacePoses(); + reefFacePoses.replaceAll(PoseSubsystem::convertBlueToRedIfNeeded); + + return currentPose.nearest(reefFacePoses); + } + + public Pose2d getClosestReefFacePoseByAlliance(DriverStation.Alliance alliance) { + Pose2d currentPose = getCurrentPose2d(); + + List reefFacePoses = getBlueReefFacePoses(); + if (alliance == DriverStation.Alliance.Red) { + reefFacePoses.replaceAll(PoseSubsystem::convertBluetoRed); + } return currentPose.nearest(reefFacePoses); } diff --git a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java index e9e56258..81cff064 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -96,6 +96,19 @@ public int getTargetAprilTagID(Landmarks.ReefFace reefFace) { return getTargetAprilTagID(targetReefFacePose); } + public int getClosestReefTagIdFromTranslation(Translation2d translation) { + double minDistance = Double.MAX_VALUE; + int closestTagId = -1; + for (Pose2d reefFacePose : aprilTagIDHashMap.keySet()) { + if (translation.getDistance(reefFacePose.getTranslation()) < minDistance) { + minDistance = translation.getDistance(reefFacePose.getTranslation()); + closestTagId = aprilTagIDHashMap.get(reefFacePose); + } + } + + return closestTagId; + } + public boolean areAllCamerasConnected() { for (int i = 0; i < cameras.length; i++) { if (!isCameraConnected(i)) {