add note detection commands

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

@@ -0,0 +1,82 @@
+/************************ PROJECT JIM *************************/
+/* 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;
+
+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/SwerveDriveTranslateToNote.java

@@ -0,0 +1,78 @@
+/************************ PROJECT JIM *************************/
+/* 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;
+
+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 SwerveDriveTranslateToNote extends Command {
+
+    private final SwerveDrive swerve;
+    private final Odometry odometry;
+    private final NoteVision vision;
+
+    private final HolonomicController controller;
+    private final BStream aligned;
+
+    public SwerveDriveTranslateToNote(){
+        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);
+
+        // translate to note only if note in view
+        if (vision.hasNoteData()) {
+            swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(odometry.getTranslation(), targetRotation)));
+        }
+
+        SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return aligned.get();
+    }
+}

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();
+    }
+}