Algae Alignment State Machine

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,48 +35,176 @@ 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();
         double deltaX = idealFinalPosition.getX() - currentTranslation.getX();
         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,

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<Pose2d> getBlueReefFacePoses() {
+        return Arrays.asList(
+                Landmarks.BlueCloseAlgae,
+                Landmarks.BlueCloseLeftAlgae,
+                Landmarks.BlueCloseRightAlgae,
+                Landmarks.BlueFarLeftAlgae,
+                Landmarks.BlueFarAlgae,
+                Landmarks.BlueFarRightAlgae);
+    }
+
     public Pose2d getClosestReefFacePose() {
         Pose2d currentPose = getCurrentPose2d();
 
-        List<Pose2d> reefFacePoses = Arrays.asList(
-                convertBlueToRedIfNeeded(Landmarks.BlueCloseAlgae),
-                convertBlueToRedIfNeeded(Landmarks.BlueCloseLeftAlgae),
-                convertBlueToRedIfNeeded(Landmarks.BlueCloseRightAlgae),
-                convertBlueToRedIfNeeded(Landmarks.BlueFarLeftAlgae),
-                convertBlueToRedIfNeeded(Landmarks.BlueFarAlgae),
-                convertBlueToRedIfNeeded(Landmarks.BlueFarRightAlgae));
+        List<Pose2d> reefFacePoses = getBlueReefFacePoses();
+        reefFacePoses.replaceAll(PoseSubsystem::convertBlueToRedIfNeeded);
+
+        return currentPose.nearest(reefFacePoses);
+    }
+
+    public Pose2d getClosestReefFacePoseByAlliance(DriverStation.Alliance alliance) {
+        Pose2d currentPose = getCurrentPose2d();
+
+        List<Pose2d> reefFacePoses = getBlueReefFacePoses();
+        if (alliance == DriverStation.Alliance.Red) {
+            reefFacePoses.replaceAll(PoseSubsystem::convertBluetoRed);
+        }
 
         return currentPose.nearest(reefFacePoses);
     }

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)) {