Merge pull request #5 from team-3482/vision-detection

Vision detection merge into main

.pathplanner/settings.json

@@ -1,12 +1,12 @@
 {
   "robotWidth": 0.71,
-  "robotLength": 0.51,
+  "robotLength": 0.71,
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.73,
-  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
-  "maxModuleSpeed": 4.73
+  "defaultMaxVel": 4.3,
+  "defaultMaxAccel": 2.0,
+  "defaultMaxAngVel": 720.0,
+  "defaultMaxAngAccel": 540.0,
+  "maxModuleSpeed": 4.8
 }

src/main/java/frc/robot/Robot.java

@@ -8,7 +8,7 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.constants.PhysicalConstants.LimelightConstants;
+import frc.robot.constants.LimelightConstants;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;

src/main/java/frc/robot/auto/DriveToNoteCommand.java

@@ -0,0 +1,149 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.auto;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+
+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.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.limelights.DetectionSubsystem;
+import frc.robot.swerve.CommandSwerveDrivetrain;
+
+/**
+ * A command that wraps a PathPlanner command that
+ * paths to the nearest visible note and turns to face it.
+ * Accounts for increased accuraccy with decreased distance.
+ */
+public class DriveToNoteCommand extends Command {
+    private final PathConstraints constraints = new PathConstraints(
+        4.3, 2,
+        Units.degreesToRadians(720), Units.degreesToRadians(540)
+    );
+
+    private Command pathingCommand;
+    private Timer timer;
+    private Pose2d currentNotePose;
+    private boolean finished;
+
+    /** Creates a new DriveToNoteCommand. */
+    public DriveToNoteCommand() {
+        setName("DriveToNoteCommand");
+        this.timer = new Timer();
+        // No requirements, because PathPlanner's Command requires the DriveTrain
+        // This wrapper is only useful for creating the PathPlanner Path
+        addRequirements();
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        this.finished = false;
+        this.timer.restart();
+
+        Pose2d[] notePoses = DetectionSubsystem.getInstance().getRecentNotePoses();
+        if (notePoses.length == 0) {
+            this.finished = true;
+            return;
+        }
+
+        this.currentNotePose = notePoses[0];
+
+        this.pathingCommand = generatePath(this.currentNotePose);
+        CommandScheduler.getInstance().schedule(this.pathingCommand);
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        if (!this.pathingCommand.isScheduled() || this.finished) return;
+
+        // Run this check every 5 robot cycles / 3 Detection cycles
+        if (this.timer.hasElapsed(0.1)) {
+            this.timer.restart();
+        }
+        else {
+            return;
+        }
+
+        Pose2d[] notePoses = DetectionSubsystem.getInstance().getRecentNotePoses();
+        // Note is stale by over 1.5 s so it has been discarded.
+        // Go by last known note position. 
+        if (notePoses.length == 0) {
+            return;
+        }
+        Pose2d newNotePose = notePoses[0];
+
+        // If within 1 meter, the path should be nearly perfect.
+        if (CommandSwerveDrivetrain.getInstance().getState().Pose.getTranslation()
+            .getDistance(newNotePose.getTranslation()) <= 1
+        ) {
+            return;
+        }
+        // If deviated over 15 cm from the original Note, repath
+        else if (this.currentNotePose.getTranslation().getDistance(newNotePose.getTranslation()) >= 0.15) {
+            this.currentNotePose = newNotePose;
+
+            CommandScheduler.getInstance().cancel(this.pathingCommand);
+            this.pathingCommand = generatePath(this.currentNotePose);
+            CommandScheduler.getInstance().schedule(this.pathingCommand);
+        }
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        if (this.pathingCommand != null && this.pathingCommand.isScheduled()) {
+            CommandScheduler.getInstance().cancel(this.pathingCommand);
+        }
+        this.timer.stop();
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return this.finished || !this.pathingCommand.isScheduled();
+    }
+
+    /**
+     * Helper that generates a PathPlanner Command to path to the note's position.
+     * @param notePose - The position to path to.
+     * @return The PathPlanner Command.
+     */
+    private Command generatePath(Pose2d notePose) {
+        Translation2d botTranslation = CommandSwerveDrivetrain.getInstance().getState().Pose.getTranslation();
+
+        // Takes into account angles in quadrants II and III
+        Rotation2d faceNoteRot = new Rotation2d(Math.atan2(
+            notePose.getY() - botTranslation.getY(),
+            notePose.getX() - botTranslation.getX()
+        ));
+
+        Pose2d botPose = new Pose2d(botTranslation, faceNoteRot);
+        notePose = new Pose2d(notePose.getTranslation(), faceNoteRot);
+
+        GoalEndState goalEndState = new GoalEndState(
+            0, // End with enough speed to pick up the note
+            faceNoteRot,
+            true
+        );
+
+        PathPlannerPath path = new PathPlannerPath(
+            PathPlannerPath.bezierFromPoses(botPose, notePose),
+            this.constraints,
+            goalEndState
+        );
+        path.preventFlipping = true; // Field-relative Note position won't change based on alliance
+
+        return AutoBuilder.followPath(path);
+    }
+}

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

@@ -4,10 +4,16 @@
 
 package frc.robot.constants;
 
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import com.ctre.phoenix6.mechanisms.swerve.utility.PhoenixPIDController;
+
 /**
  * Constants used throughout the code that are not categorized in other constants files.
  */
 public final class Constants {
+    /** Constants used for maintaining a heading when using {@link SwerveRequest#FieldCentricFacingAngle} */
+    public static final PhoenixPIDController HeadingControllerFacingAngle = new PhoenixPIDController(5.5, 0, 0.1);
+
     /**
      * Tab names in Shuffleboard.
      */

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

@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.constants;
+
+import java.util.function.Function;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.limelights.DetectionSubsystem;
+
+/** Constants for Limelight-related code. */
+public final class LimelightConstants {
+    /** Spams "Bad LL 2D/3D Pose Data" when no data is coming from the NetworkTableInstance for a LL. */
+    public static final boolean SPAM_BAD_DATA = true;
+    /** Enables publishing of the CameraFeeds to Shuffleboard on startup. */
+    public static final boolean PUBLISH_CAMERA_FEEDS = true;
+
+    /** Front left Limelight (April Tags). */
+    public static final String FRONT_APRIL_TAG_LL = "limelight-stheno";
+    /** Front right Limelight (Note detection). */
+    public static final String NOTE_DETECTION_LL = "limelight-medusa";
+    /** Back Limelight (April Tags). */
+    public static final String BACK_APRIL_TAG_LL = "limelight-euryale";
+
+    /** Constants used for AprilTag Limelights and vision algorithms. */
+    public static final class VisionConstants {
+        /** The distance within which to use Limelight data in meters. This is measured from tag to camera.*/
+        public static final int TRUST_TAG_DISTANCE = 4;
+
+        /** All valid tag IDs (used for tag filtering) */
+        public static final int[] ALL_TAG_IDS = new int[]{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 };
+
+        /** Crop window size when no tags are in view (used for smart cropping) */
+        public static final double DEFAULT_CROP_SIZE = 0.85;
+
+        // See https://docs.limelightvision.io/docs/docs-limelight/hardware-comparison.
+        /** Horizontal FOV of LL3G in degrees (used for smart cropping) */
+        public static final double FOV_X = 82;
+        /** Vertical FOV of LL3G in degrees (used for smart cropping) */
+        public static final double FOV_Y = 56.2;
+        /** FOV area of the LL3g in degrees squared (used for smart cropping) */
+        public static final double FOV_AREA = FOV_X * FOV_Y;
+    }
+
+    /** Constants used for the note detection Limelight and detection algorithms. */
+    public static final class DetectionConstants {
+        /**
+         * Note data older than this time in seconds will be discarded.
+         * @see {@link DetectionSubsystem#processRecentDetectionDatas()}
+         */
+        public static final double STALE_DATA_CUTOFF = 1.5;
+        /** Whether or not the DetectionSubsystem will publish the top 3 most recent Notes. */
+        public static final boolean PUBLISH_NOTE_POSES = true;
+        /**
+         * Do not trust any note distances further than this value in meters from the camera.
+         * @apiNote Above 3 meters has significant drops in accuracy.
+         * It is not recommended unless often updating the position when closing in on it.
+         */
+        public static final double MAX_NOTE_DISTANCE_TRUST = 4;
+        /** Do not turn towards notes further than this value in meters from the robot. */
+        public static final double MAX_NOTE_DISTANCE_DRIVING = 3.5;
+
+        /**
+         * The real-life outer diameter of a note in meters.
+         * @see Page 34 of the 2024 game manual :
+         * https://firstfrc.blob.core.windows.net/frc2024/Manual/2024GameManual.pdf
+         */
+        public static final double NOTE_DIAMETER = 0.36;
+        /** The real-life height of a note in meters.
+         * @see Page 34 of the 2024 game manual :
+         * https://firstfrc.blob.core.windows.net/frc2024/Manual/2024GameManual.pdf 
+         */
+        public static final double NOTE_HEIGHT = 0.05;
+
+        /** The position of the Limelight relative to the center of the bot. */
+        public static final Pose3d LIMELIGHT_POSITION = new Pose3d(
+            new Translation3d(0.34, 0.25, 0.27),
+            new Rotation3d(
+                0,
+                Units.degreesToRadians(-20),
+                Units.degreesToRadians(-20)
+            )
+        );
+
+        /** Heuristic conversion factor. Works with Limelight Resolution 1280x960. */
+        public static final double PIXEL_TO_RAD = 900;
+
+        /** The width of the screen in pixels, based on resolution. */
+        public static final int SCREEN_WIDTH = 1280;
+        /** The hieght of the screen in pixels, based on resolution. */
+        public static final int SCREEN_HEIGHT = 960 - 144; // Cropped .7 in the +y direction ; 144 = (960 / 2 * 0.3).
+
+        /**
+         * A formula that attempts to account for the detection error of far-away notes. 
+         * This should only be applied for notes 0.5 m or more away.
+         * @see https://www.desmos.com/calculator/zcrdcjewgg
+         */
+        public static Function<Double, Double> WIDTH_DIST_ERROR_CORRECTION = (Double d) -> {
+            final double m = 0.058;
+            final double b = m * Math.exp(0.5);
+            return m * Math.exp(d) - b;
+        };
+
+        /**
+         * A formula that attempts to account for the detection error of far-away notes. 
+         * This should only be applied for notes 0.5 m or more away.
+         * @see https://www.desmos.com/calculator/uyzeqe3i0l
+         */
+        public static Function<Double, Double> PITCH_DIST_ERROR_CORRECTION = (Double d) -> {
+            final double m = 0.028;
+            final double b = m; // * Math.exp(0)
+            return m * Math.exp(d) - b;
+        };
+    }
+}

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

@@ -16,30 +16,4 @@ public static final class RobotConstants {
         /** Name of the CTRE CAN bus (configured on the CANivore). */
         public static final String CTRE_CAN_BUS = "ctre";
     }
-
-    /** Constants for limelight-related data. */
-    public static final class LimelightConstants {
-        /** Spams "Bad LL 2D/3D Pose Data" when no data is coming from the NetworkTableInstance for a LL. */
-        public static final boolean SPAM_BAD_DATA = true;
-        /** The distance within which to use Limelight data in meters. This is measured from tag to camera.*/
-        public static final int TRUST_TAG_DISTANCE = 4;
-
-        /** Front left Limelight (April Tags). */
-        public static final String FRONT_APRIL_TAG_LL = "limelight-stheno";
-        /** Front right Limelight (Note detection). */
-        public static final String NOTE_DETECTION_LL = "limelight-medusa"; // TODO : configure detection limelight
-        /** Back Limelight (April Tags). */
-        public static final String BACK_APRIL_TAG_LL = "limelight-euryale";
-
-        /** All valid tag IDs (used for tag filtering) */
-        public static final int[] ALL_TAG_IDS = new int[]{ 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 };
-
-        // See https://docs.limelightvision.io/docs/docs-limelight/hardware-comparison.
-        /** Horizontal FOV of LL3G in degrees (used for smart cropping) */
-        public static final double FOV_X = 82;
-        /** Vertical FOV of LL3G in degrees (used for smart cropping) */
-        public static final double FOV_Y = 56.2;
-        /** FOV area of the LL3g in degrees squared (used for smart cropping) */
-        public static final double FOV_AREA = FOV_X * FOV_Y;
-    }
 }

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

@@ -12,7 +12,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.DriverStation;
-import frc.robot.vision.VisionSubsystem;
+import frc.robot.limelights.VisionSubsystem;
 
 /**
  * A class that stores positions used for initialization and calculations.