Adds note detection with AutoAlign

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

@@ -46,12 +46,12 @@ public static enum Mode {
     }
 
     public static final class FeatureFlags {
-        public static final boolean runVision = true;
+        public static final boolean runVision = false;
 
-        public static final boolean runIntake = true;
-        public static final boolean runScoring = true;
-        public static final boolean runEndgame = true;
-        public static final boolean runDrive = true;
+        public static final boolean runIntake = false;
+        public static final boolean runScoring = false;
+        public static final boolean runEndgame = false;
+        public static final boolean runDrive = false;
 
         public static final boolean enableLEDS = false;
     }

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

@@ -41,7 +41,7 @@ public void robotInit() {
 
         if (Constants.currentMode == Constants.Mode.REAL) {
             // TODO: Log data to a USB drive on the RIO
-            Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+            // Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
             Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
             pdh = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
         } else if (Constants.currentMode == Constants.Mode.SIM) {

src/main/java/frc/robot/RobotContainer.java

@@ -39,6 +39,7 @@
 import frc.robot.subsystems.localization.CameraContainerReal;
 import frc.robot.subsystems.localization.CameraContainerReplay;
 import frc.robot.subsystems.localization.CameraContainerSim;
+import frc.robot.subsystems.localization.NoteDetection;
 import frc.robot.subsystems.localization.VisionLocalizer;
 import frc.robot.subsystems.scoring.AimerIO;
 import frc.robot.subsystems.scoring.AimerIORoboRio;
@@ -84,6 +85,8 @@ public class RobotContainer {
 
     LED leds;
 
+    NoteDetection noteDetection;
+
     public RobotContainer() {
         configureSubsystems();
         configureModes();
@@ -219,6 +222,8 @@ public void configureSubsystems() {
         }
 
         if (FeatureFlags.enableLEDS) leds = new LED(scoringSubsystem);
+
+        noteDetection = new NoteDetection(() -> new Pose2d(0, 0, new Rotation2d()));
     }
 
     // spotless:off
@@ -240,6 +245,10 @@ private void configureBindings() {
                     () -> drivetrain.seedFieldRelative(getPoseAgainstSpeaker()))
                 );
 
+            controller.a()
+                .onTrue(new InstantCommand(
+                    () -> drivetrain.setAlignTarget(AlignTarget.NOTE)));
+
             controller.b()
                 .onTrue(new InstantCommand(
                     () -> drivetrain.setAlignTarget(AlignTarget.SPEAKER)));
@@ -679,11 +688,11 @@ public void robotPeriodic() {
     public void disabledPeriodic() {
         /* set to coast mode when circuit open */
         if (brakeSwitch != null && brakeSwitch.get()) {
-            scoringSubsystem.setBrakeMode(false);
-            endgameSubsystem.setBrakeMode(false);
+            if (FeatureFlags.runScoring) scoringSubsystem.setBrakeMode(false);
+            if (FeatureFlags.runEndgame) endgameSubsystem.setBrakeMode(false);
         } else {
-            scoringSubsystem.setBrakeMode(true);
-            endgameSubsystem.setBrakeMode(true);
+            if (FeatureFlags.runScoring) scoringSubsystem.setBrakeMode(true);
+            if (FeatureFlags.runEndgame) endgameSubsystem.setBrakeMode(true);
         }
     }
 

src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java

@@ -20,6 +20,8 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Subsystem;
@@ -29,8 +31,11 @@
 import frc.robot.Constants.TunerConstants;
 import frc.robot.utils.GeomUtil;
 import frc.robot.utils.InterpolateDouble;
+import java.util.List;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.TargetCorner;
 
 /**
  * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used
@@ -42,8 +47,14 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
 
     private double alignError = 0.0;
 
+    private Timer elapsedDriveToNote = new Timer();
+    private double estimatedTimeToNote = 0;
+    private boolean drivingToNote = false;
+    private double allotedErrorTime = 2;
+
     public enum AlignTarget {
         NONE,
+        NOTE,
         AMP,
         SPEAKER,
         SOURCE
@@ -57,6 +68,8 @@ public enum AlignState {
     private AlignTarget alignTarget = AlignTarget.NONE;
     private AlignState alignState = AlignState.MANUAL;
 
+    private PhotonCamera colorCamera = new PhotonCamera("photonvision-orange");
+
     private static InterpolateDouble noteTimeToGoal =
             new InterpolateDouble(ScoringConstants.timeToGoalMap(), 0.0, 2.0);
 
@@ -68,6 +81,8 @@ public enum AlignState {
 
     private Supplier<Translation2d> getRobotVelocity = () -> new Translation2d();
 
+    private Supplier<Boolean> hasNote = () -> false;
+
     private PIDController thetaController =
             new PIDController(
                     DriveConstants.alignmentkPMax,
@@ -76,7 +91,7 @@ public enum AlignState {
 
     private SwerveRequest.FieldCentric driveFieldCentric = new SwerveRequest.FieldCentric();
     private SwerveRequest.RobotCentric driveRobotCentric = new SwerveRequest.RobotCentric();
-    private SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+    // private SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
 
     private static final double kSimLoopPeriod = 0.02; // Original: 5 ms
     private Notifier simNotifier = null;
@@ -101,6 +116,8 @@ public CommandSwerveDrivetrain(
          * In short, never trust the Command Scheduler.
          */
         CommandScheduler.getInstance().registerSubsystem(this);
+        elapsedDriveToNote.reset();
+        elapsedDriveToNote.stop();
     }
 
     public CommandSwerveDrivetrain(
@@ -138,6 +155,10 @@ public void setSourceSupplier(Supplier<Rotation2d> getFieldToSource) {
         this.getFieldToSource = getFieldToSource;
     }
 
+    public void setNoteAcquiredSupplier(Supplier<Boolean> hasNote) {
+        this.hasNote = hasNote;
+    }
+
     public void setAlignTarget(AlignTarget alignTarget) {
         this.alignTarget = alignTarget;
     }
@@ -233,8 +254,48 @@ public void setGoalChassisSpeeds(ChassisSpeeds chassisSpeeds) {
     private void controlDrivetrain() {
         Pose2d pose = getFieldToRobot.get();
         Rotation2d desiredHeading = pose.getRotation();
+
+        if (alignState != AlignState.ALIGNING || alignTarget != AlignTarget.NOTE) {
+            drivingToNote = false;
+            elapsedDriveToNote.stop();
+            elapsedDriveToNote.reset();
+        }
+
         if (alignState == AlignState.ALIGNING) {
             switch (alignTarget) {
+                    /*case NOTE:
+                    var cameraResult = colorCamera.getLatestResult();
+                    if (cameraResult.hasTargets()) {
+                        Pose2d notePose = getNotePoseFromTarget(cameraResult.getBestTarget());
+
+                        if (!drivingToNote) {
+
+                            double distance =
+                                    Math.sqrt(
+                                            Math.pow(notePose.getX(), 2)
+                                                    + Math.pow(notePose.getY(), 2));
+
+                            drivingToNote = true;
+                            estimatedTimeToNote =
+                                    distance / DriveConstants.MaxSpeedMetPerSec + allotedErrorTime;
+                            elapsedDriveToNote.reset();
+                            elapsedDriveToNote.start();
+
+                            setGoalChassisSpeeds(
+                                    new ChassisSpeeds(DriveConstants.MaxSpeedMetPerSec, 0, 0),
+                                    false);
+                            desiredHeading = notePose.getRotation();
+                        } else if (elapsedDriveToNote.get() > estimatedTimeToNote
+                                || hasNote.get()) {
+                            setGoalChassisSpeeds(new ChassisSpeeds());
+                        } else {
+                            setGoalChassisSpeeds(
+                                    new ChassisSpeeds(DriveConstants.MaxSpeedMetPerSec, 0, 0),
+                                    false);
+                            desiredHeading = notePose.getRotation();
+                        }
+                    }
+                    break;*/
                 case AMP:
                     desiredHeading = getFieldToAmp.get();
                     break;
@@ -347,5 +408,16 @@ public boolean isAligned() {
     @Override
     public void periodic() {
         controlDrivetrain();
+
+        List<TargetCorner> corners =
+                colorCamera.getLatestResult().getBestTarget().getDetectedCorners();
+        double[][] cornersCoordinates = new double[4][2];
+        /*for (int i = 0; i < 4; i++) {
+            cornersCoordinates[i] = getCoordinateOfTargetCorner(corners.get(i));
+        }*/
+        SmartDashboard.putNumberArray("coordinates 0", cornersCoordinates[0]);
+        SmartDashboard.putNumberArray("coordinates 1", cornersCoordinates[1]);
+        SmartDashboard.putNumberArray("coordinates 2", cornersCoordinates[2]);
+        SmartDashboard.putNumberArray("coordinates 3", cornersCoordinates[3]);
     }
 }

src/main/java/frc/robot/subsystems/localization/NoteDetection.java

@@ -0,0 +1,84 @@
+package frc.robot.subsystems.localization;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.List;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+
+public class NoteDetection extends SubsystemBase {
+
+    private PhotonCamera colorCamera = new PhotonCamera("note-detector");
+    private Supplier<Pose2d> robotPose;
+
+    public NoteDetection(Supplier<Pose2d> robotPose) {
+        this.robotPose = robotPose;
+        SmartDashboard.putBoolean("running", true);
+    }
+
+    private Pose2d getNotePoseFromTarget(PhotonTrackedTarget target) {
+        double transformWidth = 1 * 78; // = distance at setpoint / width at setpoint
+        double noteWidth = 0.1;
+        // w of real note = 35 cm
+        // .405 * 74 * 35 = d * wd
+
+        List<TargetCorner> corners = target.getMinAreaRectCorners();
+
+        double[][] cornersCoordinates = new double[4][2];
+        SmartDashboard.putNumber("corners", corners.size());
+        SmartDashboard.putNumber("area", target.getArea());
+        for (int i = 0; i < corners.size(); i++) {
+            cornersCoordinates[i][0] = corners.get(i).x;
+            cornersCoordinates[i][1] = corners.get(i).y;
+            SmartDashboard.putNumber("coordinate" + (i) + "x", cornersCoordinates[i][0]);
+            SmartDashboard.putNumber("coordinate" + (i) + "y", cornersCoordinates[i][1]);
+        }
+
+        double width = cornersCoordinates[1][0] - cornersCoordinates[0][0];
+        double distanceForward = transformWidth / width;
+        // alternatively: instead of using magnification maybe try using just y instead?
+        SmartDashboard.putNumber("distanceForward", distanceForward);
+        // THEORETICALLY WORKING
+
+        double widthShift = 0.180 * 199 / 2;
+        double distanceSide =
+                widthShift
+                        * noteWidth
+                        / ((cornersCoordinates[1][0] + cornersCoordinates[0][0]) / 2 - 160)
+                        / distanceForward;
+        SmartDashboard.putNumber("distanceSize", distanceSide);
+
+        double distance =
+                Math.sqrt(distanceForward * distanceForward + distanceSide * distanceSide);
+        double angle = Math.tanh(distanceSide / distanceForward);
+
+        Rotation2d fromForward = robotPose.get().getRotation().plus(new Rotation2d(angle));
+
+        SmartDashboard.putNumber("width", width);
+        SmartDashboard.putNumber(
+                "centerx", (cornersCoordinates[1][0] + cornersCoordinates[0][0]) / 2);
+        SmartDashboard.putNumber(
+                "centery", (cornersCoordinates[1][1] + cornersCoordinates[0][1]) / 2);
+
+        return new Pose2d(
+                robotPose.get().getX() + Math.cos(fromForward.getRadians()) * distance,
+                robotPose.get().getY() + Math.sin(fromForward.getRadians()) * distance,
+                fromForward);
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putBoolean("hello", true);
+        var cameraResult = colorCamera.getLatestResult();
+        if (cameraResult.hasTargets()) {
+            Pose2d notePose = getNotePoseFromTarget(cameraResult.getBestTarget());
+            Logger.recordOutput("Note Position", notePose);
+            SmartDashboard.putBoolean("target found", true);
+        }
+    }
+}

vendordeps/photonlib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "v2024.2.7",
+    "version": "v2024.3.1",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
     "frcYear": "2024",
     "mavenUrls": [
@@ -14,7 +14,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-cpp",
-            "version": "v2024.2.7",
+            "version": "v2024.3.1",
             "libName": "photonlib",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -29,7 +29,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2024.2.7",
+            "version": "v2024.3.1",
             "libName": "photontargeting",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -46,12 +46,12 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-java",
-            "version": "v2024.2.7"
+            "version": "v2024.3.1"
         },
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-java",
-            "version": "v2024.2.7"
+            "version": "v2024.3.1"
         }
     ]
 }