add note detection commands

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java

@@ -0,0 +1,82 @@
+/************************ PROJECT JIM *************************/
+/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/
+/* This work is licensed under the terms of the MIT license.  */
+/**************************************************************/
+
+package com.stuypulse.robot.commands.swerve;
+
+import static com.stuypulse.robot.constants.Settings.NoteDetection.*;
+
+import com.stuypulse.robot.constants.Settings.Swerve;
+import com.stuypulse.robot.subsystems.vision.NoteVision;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+
+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.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveDriveToNote extends Command {
+
+    private final SwerveDrive swerve;
+    private final Odometry odometry;
+    private final NoteVision vision;
+
+    private final HolonomicController controller;
+    private final BStream aligned;
+
+    public SwerveDriveDriveToNote(){
+        this.swerve = SwerveDrive.getInstance();
+        this.odometry = Odometry.getInstance();
+        this.vision = NoteVision.getInstance();
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P,Translation.I,Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)
+        );
+
+        SmartDashboard.putData("Note Detection/Controller", controller);
+
+        aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME));
+
+        addRequirements(swerve);
+    }
+
+    private boolean isAligned() {
+        return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get());
+    }
+
+    @Override
+    public void execute() {
+        Translation2d targetTranslation = odometry.getTranslation().plus(
+            new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation()));
+
+        Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle();
+
+        Pose2d targetPose = new Pose2d(targetTranslation, targetRotation);
+
+        if (vision.hasNoteData()) {
+            // drive to note
+            swerve.setChassisSpeeds(controller.update(targetPose, odometry.getPose()));
+        }
+        else {
+            // only rotate toward saved note pose
+            swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetTranslation, odometry.getRotation())));
+        }
+
+        SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return aligned.get();
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java

@@ -0,0 +1,72 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Driver.Drive;
+import com.stuypulse.robot.constants.Settings.NoteDetection.Rotation;
+import com.stuypulse.robot.constants.Settings.Swerve;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.subsystems.vision.NoteVision;
+import com.stuypulse.stuylib.control.angle.AngleController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.Angle;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
+import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveNoteAlignedDrive extends Command {
+
+    private final SwerveDrive swerve;
+    private final Odometry odometry;
+    private final NoteVision noteVision;
+
+    private final VStream speed; 
+
+    private final AngleController alignController;
+
+    public SwerveDriveNoteAlignedDrive(Gamepad driver) {
+        swerve = SwerveDrive.getInstance();
+        odometry = Odometry.getInstance();
+        noteVision = NoteVision.getInstance();
+
+        speed = VStream.create(driver::getLeftStick)
+            .filtered(
+                new VDeadZone(Drive.DEADBAND),
+                x -> x.clamp(1.0),
+                x -> Settings.vpow(x, Drive.POWER.get()),
+                x -> x.mul(Drive.MAX_TELEOP_SPEED.get()),
+                new VRateLimit(Drive.MAX_TELEOP_ACCEL),
+                new VLowPassFilter(Drive.RC)
+            );  
+
+        alignController = new AnglePIDController(Rotation.P, Rotation.I, Rotation.D);
+
+        addRequirements(swerve);
+    }
+
+   @Override
+    public void execute() {
+        Translation2d targetTranslation = odometry.getTranslation().plus(
+            new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation()));
+
+        Rotation2d targetRotation = noteVision.getEstimatedNotePose().minus(targetTranslation).getAngle();
+
+        double angularVel = -alignController.update(
+            Angle.fromRotation2d(targetRotation),
+            Angle.fromRotation2d(odometry.getRotation())
+        );
+
+        // robot relative
+        swerve.drive(speed.get(), angularVel);
+
+        SmartDashboard.putNumber("Note Detection/Angle Output", alignController.getOutput());
+
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveResetHeading.java

@@ -0,0 +1,22 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class SwerveDriveResetHeading extends InstantCommand {
+
+    public SwerveDriveResetHeading(Rotation2d heading) {
+        super(() -> {
+            Odometry o = Odometry.getInstance();
+            o.reset(new Pose2d(o.getTranslation(), heading));
+        });
+    }
+
+    public SwerveDriveResetHeading() {
+        this(new Rotation2d());
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java

@@ -9,7 +9,7 @@
 public class SwerveDriveXMode extends Command {
 
     // { front right, front left, back right, back left }
-    private static final SwerveModuleState[] states = new SwerveModuleState[] {
+    private static final SwerveModuleState[] states = {
         new SwerveModuleState(0, Rotation2d.fromDegrees(225)),
         new SwerveModuleState(0, Rotation2d.fromDegrees(315)),
         new SwerveModuleState(0, Rotation2d.fromDegrees(45)),

src/main/java/com/stuypulse/robot/commands/swerve/robotrelative/SwerveDriveDriveToNote.java

@@ -0,0 +1,78 @@
+/************************ PROJECT IZZI *************************/
+/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/
+/* This work is licensed under the terms of the MIT license.  */
+/**************************************************************/
+
+package com.stuypulse.robot.commands.swerve.robotrelative;
+
+import static com.stuypulse.robot.constants.Settings.NoteDetection.*;
+
+import com.stuypulse.robot.constants.Settings.Swerve;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.subsystems.vision.NoteVision;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+
+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.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveDriveToNote extends Command {
+
+    private final SwerveDrive swerve;
+    private final NoteVision vision; 
+    private final Odometry odometry;
+
+    private final HolonomicController controller;
+    private final BStream aligned;
+
+    public SwerveDriveDriveToNote(){
+        this.swerve = SwerveDrive.getInstance();
+        this.vision = NoteVision.getInstance();
+        odometry = Odometry.getInstance(); 
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P,Translation.I,Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)
+        );
+
+        SmartDashboard.putData("Note Detection/Controller", controller);
+
+        aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME));
+
+        addRequirements(swerve);
+    }
+
+    private boolean isAligned() {
+        return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get());
+    }
+
+    @Override
+    public void execute() {
+        Translation2d robotToNote = vision.getRobotRelativeNotePose();
+        Pose2d targetPose = new Pose2d(
+            new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation()),
+            new Rotation2d());
+
+        if(vision.hasNoteData()) {
+            swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(robotToNote, robotToNote.getAngle())));
+        }
+        else {
+            swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetPose.getTranslation(), odometry.getRotation())));
+        }
+
+        SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return aligned.get();
+    }
+}

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -96,6 +96,7 @@ public interface Swerve {
         // between wheel centers
         double WIDTH = Units.inchesToMeters(20.75);
         double LENGTH = Units.inchesToMeters(20.75);
+        double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(18); // TODO: redetermine for izzi (from reteP)
 
         SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Max Module Speed (meter per s)", 5.0);
         SmartNumber MAX_TURNING = new SmartNumber("Swerve/Max Turn Velocity (rad per s)", 6.28);

src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java

@@ -9,6 +9,8 @@
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 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.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
@@ -86,6 +88,22 @@ public Pose2d getPose() {
         return estimator.getEstimatedPosition();
     }
 
+    /**
+     * Returns the translation of the robot.
+     * @return the translation of the robot
+     */
+    public Translation2d getTranslation() {
+        return getPose().getTranslation();
+    }
+
+    /**
+     * Returns the rotation of the robot.
+     * @return the rotation of the robot
+     */
+    public Rotation2d getRotation() {
+        return getPose().getRotation();
+    }
+
     /**
      * Reset the pose of the odometry to the given pose.
      * @param pose the pose to reset to

src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java

@@ -37,7 +37,7 @@ protected LLNoteVision() {
     }
 
     /**
-     * Returns whether the Limelight has data.
+     * Get whether the Limelight has data.
      * @return whether the Limelight has data
      */
     @Override
@@ -51,16 +51,28 @@ public boolean hasNoteData() {
     }
 
     /**
-     * Returns the estimated pose of the note.
+     * Get the estimated pose of the note.
      * @return the estimated pose of the note
      */
     @Override
     public Translation2d getEstimatedNotePose() {
         return notePose;
     }
 
+    @Override
+    public Translation2d getRobotRelativeNotePose() {
+        Translation2d sum = new Translation2d();
+
+        for (Limelight limelight : limelights) {
+            sum = sum.plus(new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())));
+        }
+
+        return sum.div(limelights.length);
+    }
+
     /**
-     * Calculates the estimated pose of the note.
+     * @Calculates the estimated pose of the note by averaging data from all available limelights.
+     * Sets the pose to `notePose` that can be accessed with `getEstimatedNotePose()`.
      */
     private void updateNotePose() {
         Translation2d sum = new Translation2d();

src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java

@@ -1,5 +1,6 @@
 package com.stuypulse.robot.subsystems.vision;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -18,5 +19,10 @@ public static NoteVision getInstance() {
     public abstract boolean hasNoteData();
 
     public abstract Translation2d getEstimatedNotePose();
+    public abstract Translation2d getRobotRelativeNotePose();
+
+    public final Rotation2d getRotationToNote() {
+        return getRobotRelativeNotePose().getAngle();
+    }
 
 }

src/main/java/com/stuypulse/robot/util/vision/Limelight.java

@@ -100,7 +100,11 @@ public double getYAngle() {
         return tyData - Units.radiansToDegrees(POSITIONS[limelightID].getRotation().getY());
     }
 
-    // limelight targets far end of note, so have to subtract half of note length
+    /**
+     * Calculates the distance from the robot's center to the note's center.
+     * Limelight targets far end of note, so half of note length is substracted.
+     * @return distance from robot center to note center.
+     */
     public double getDistanceToNote() {
         Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle());
         return POSITIONS[limelightID].getZ() / -yRotation.getTan() + POSITIONS[limelightID].getX() - Field.NOTE_LENGTH / 2.0;