PathFinding and Vision

src/main/java/frc/robot/DriverJoystick.java

@@ -2,17 +2,29 @@
 
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.IOConstants;
+import frc.robot.constants.AutoConstants;
+import frc.robot.constants.AutoConstants.DriveWaypoints;
 import frc.robot.libraries.XboxController1038;
 import frc.robot.subsystems.DriveTrain;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.IdealStartingState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 
 public class DriverJoystick extends XboxController1038 {
     // Subsystem Dependencies
     private final DriveTrain driveTrain = DriveTrain.getInstance();
 
+    private Pose2d currentPose;
+
     // Previous Status
     private double prevX = 0;
     private double prevY = 0;
@@ -82,6 +94,20 @@ private DriverJoystick() {
 
         // Lock the wheels into an X formation
         super.xButton.whileTrue(this.driveTrain.setX());
+        super.aButton.whileTrue(
+                new InstantCommand(() -> {
+                    this.currentPose = this.driveTrain.getState().Pose;
+                }).andThen(AutoBuilder.followPath(new PathPlannerPath(
+                        PathPlannerPath.waypointsFromPoses(this.currentPose, DriveWaypoints.Algae23.getEndpoint()),
+                        new PathConstraints(
+                                DriveConstants.MaxSpeed,
+                                AutoConstants.kMaxAccelerationMetersPerSecondSquared,
+                                AutoConstants.kMaxAngularSpeedRadiansPerSecond,
+                                AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared),
+                        new IdealStartingState(
+                                driveTrain.getState().Speeds.vxMetersPerSecond,
+                                driveTrain.getState().Pose.getRotation()),
+                        new GoalEndState(0, Rotation2d.kZero)))));
     }
 
     /**

src/main/java/frc/robot/constants/AutoConstants.java

@@ -4,17 +4,65 @@
 
 import com.pathplanner.lib.config.RobotConfig;
 
+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.math.trajectory.TrapezoidProfile;
 
 public final class AutoConstants {
+    public enum DriveWaypoints {
+        LeftCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        Algae23(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        Algae34(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        Processor(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        LeftFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))),
+        RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0)));
+
+        private Pose2d endpoint;
+
+        private DriveWaypoints(Pose2d endpoint) {
+            this.endpoint = endpoint;
+        }
+
+        public Pose2d getEndpoint() {
+            return endpoint;
+        }
+
+    }
+
     public static final double kMaxAccelerationMetersPerSecondSquared = 7;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
     public static final double kPXController = 1.0;
     public static final double kIXController = 0.35;
+    public static final double kDController = 0.0;
     public static final double kPThetaController = 1.0;
     public static final double kIThetaController = 0.05;
+    public static final double kDThetaController = 0.0;
 
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(

src/main/java/frc/robot/constants/VisionConstants.java

@@ -1,5 +1,15 @@
 package frc.robot.constants;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
 public final class VisionConstants {
     public static final String kTableName = "Vision";
     public static final String kValuesTopic = "values";
@@ -8,8 +18,8 @@ public final class VisionConstants {
     public static final String kEnabled0Topic = "enable0";
     public static final String kEnabled1Topic = "enable1";
 
-    public static final double width = 1280;
-    public static final double height = 720;
+    public static final double width = 800;
+    public static final double height = 600;
     public static final double fov = 100;
 
     public static final double driveP = 0.005;
@@ -20,4 +30,10 @@ public final class VisionConstants {
     public static final double spinD = 0.0002;
     public static final double spinSetpoint = 0.0;
     public static final double aprilTagArea = 28908;
+
+    public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+    public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0.1, 0.0, 0.1),
+            new Rotation3d(0, 0, 0));
+    public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+    public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
 }

src/main/java/frc/robot/subsystems/Dashboard.java

@@ -13,13 +13,14 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.autons.AutonSelector.AutonChoices;
+import com.pathplanner.lib.path.PathPlannerPath;
 
 public class Dashboard extends SubsystemBase {
     // Inputs
-    private DriveTrain driveTrain = DriveTrain.getInstance();
+    private final DriveTrain driveTrain = DriveTrain.getInstance();
 
     // Choosers
-    private SendableChooser<AutonChoices> autoChooser = new SendableChooser<>();
+    private final SendableChooser<AutonChoices> autoChooser = new SendableChooser<>();
     private SendableChooser<Double> delayChooser = new SendableChooser<>();
 
     // Tabs
@@ -100,6 +101,10 @@ public void periodic() {
         field.setRobotPose(driveTrain.getState().Pose);
     }
 
+    public void addPath(PathPlannerPath path) {
+        this.field.getObject("poses").setPoses(path.getPathPoses());
+    }
+
     /**
      * Puts the given {@link Trajectory} on the dashboard
      *

src/main/java/frc/robot/subsystems/DriveTrain.java

@@ -12,9 +12,14 @@
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.util.DriveFeedforwards;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -24,6 +29,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.SwerveConstants;
 
@@ -142,6 +148,24 @@ private DriveTrain() {
                 SwerveConstants.FrontRight,
                 SwerveConstants.BackLeft,
                 SwerveConstants.FrontRight);
+        AutoBuilder.configure(
+                () -> this.getState().Pose,
+                this::resetPose,
+                () -> this.getState().Speeds,
+                (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> {
+                    this.setControl(
+                            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds)
+                                    .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons())
+                                    .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()));
+                },
+                new PPHolonomicDriveController(
+                        new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController,
+                                AutoConstants.kDController),
+                        new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController,
+                                AutoConstants.kDThetaController)),
+                AutoConstants.kRobotConfig.get(),
+                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                this);
         if (Utils.isSimulation()) {
             startSimThread();
         }
@@ -181,7 +205,7 @@ public Command setX() {
      * Returns a command that applies the specified control request to this swerve
      * drivetrain.
      *
-     * @param request Function returning the request to apply
+     * @param requestSupplier Function returning the request to apply
      * @return Command to run
      */
     public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {

src/main/java/frc/robot/subsystems/Vision.java

@@ -0,0 +1,153 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.VisionConstants;
+
+import java.util.List;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class Vision extends SubsystemBase {
+    private final PhotonCamera frontCam;
+    private final PhotonCamera backCam;
+    private final PhotonPoseEstimator photonEstimator;
+    private Matrix<N3, N1> curStdDevs;
+
+    private static Vision instance;
+
+    public static Vision getInstance() {
+        if (instance == null) {
+            instance = new Vision();
+        }
+        return instance;
+    }
+
+    private Vision() {
+        frontCam = new PhotonCamera("frontCam");
+        backCam = new PhotonCamera("backCam");
+
+        photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                VisionConstants.kRobotToCam);
+        photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS);
+    }
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be
+     * empty. This should
+     * only be called once per loop.
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate
+     *         timestamp, and targets
+     *         used for estimation.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+        Optional<EstimatedRobotPose> visionEst = Optional.empty();
+
+        for (var change : frontCam.getAllUnreadResults()) {
+            visionEst = photonEstimator.update(change);
+            updateEstimationStdDevs(visionEst, change.getTargets());
+        }
+        return visionEst;
+    }
+
+    /**
+     * Calculates new standard deviations This algorithm is a heuristic that creates
+     * dynamic standard
+     * deviations based on number of tags, estimation strategy, and distance from
+     * the tags.
+     *
+     * @param estimatedPose The estimated pose to guess standard deviations for.
+     * @param targets       All targets in this camera frame
+     */
+    private void updateEstimationStdDevs(
+            Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
+        if (estimatedPose.isEmpty()) {
+            // No pose input. Default to single-tag std devs
+            curStdDevs = VisionConstants.kSingleTagStdDevs;
+
+        } else {
+            // Pose present. Start running Heuristic
+            var estStdDevs = VisionConstants.kSingleTagStdDevs;
+            int numTags = 0;
+            double avgDist = 0;
+
+            // Precalculation - see how many tags we found, and calculate an
+            // average-distance metric
+            for (var tgt : targets) {
+                var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+                if (tagPose.isEmpty())
+                    continue;
+                numTags++;
+                avgDist += tagPose
+                        .get()
+                        .toPose2d()
+                        .getTranslation()
+                        .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+            }
+
+            if (numTags == 0) {
+                // No tags visible. Default to single-tag std devs
+                curStdDevs = VisionConstants.kSingleTagStdDevs;
+            } else {
+                // One or more tags visible, run the full heuristic.
+                avgDist /= numTags;
+                // Decrease std devs if multiple targets are visible
+                if (numTags > 1)
+                    estStdDevs = VisionConstants.kMultiTagStdDevs;
+                // Increase std devs based on (average) distance
+                if (numTags == 1 && avgDist > 4)
+                    estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+                else
+                    estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+                curStdDevs = estStdDevs;
+            }
+        }
+    }
+
+    /**
+     * Returns the latest standard deviations of the estimated pose from {@link
+     * #getEstimatedGlobalPose()}, for use with {@link
+     * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator
+     * SwerveDrivePoseEstimator}. This should
+     * only be used when there are targets visible.
+     */
+    public Matrix<N3, N1> getEstimationStdDevs() {
+        return curStdDevs;
+    }
+
+    // public int getBestTarget() {
+    // return frontCam.getAllUnreadResults().get(1).getBestTarget().getFiducialId();
+    // }
+}