automatic note intake

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

@@ -8,6 +8,8 @@
 import static frc.robot.Constants.VisionConstants.BACK_RIGHT_CAMERA;
 import static frc.robot.Constants.VisionConstants.NOTE_CAMERA;
 
+import java.util.function.BooleanSupplier;
+
 import com.choreo.lib.ChoreoTrajectory;
 import edu.wpi.first.cscore.MjpegServer;
 import edu.wpi.first.cscore.UsbCamera;
@@ -43,6 +45,7 @@
 import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand;
 import frc.robot.commands.shooter.flywheel.ShooterFlywheelShuttleCommand;
 import frc.robot.commands.swerve.AlignCommand;
+import frc.robot.commands.swerve.AutoIntakeSequence;
 import frc.robot.commands.swerve.NoteAlignCommand;
 import frc.robot.commands.swerve.SwerveStopCommand;
 import frc.robot.commands.vision.CalculateBackCameraTransformCommand;
@@ -544,10 +547,12 @@ private void configureBindings() {
                     ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
         );
 
-        /* Note align -- deprecated, new version in the works*/
+        /* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
         driveController.getNoteAlign().onTrue(
-            new NoteAlignCommand(swerveSubsystem, noteDetector, driveController)
-                .unless(() -> noteDetector.getNote().isEmpty())
+            new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem,
+                                   swerveSubsystem, noteDetector,
+                                   driveController, lightBarSubsystem)                  
+            .onlyWhile(driveController.getNoteAlign())
         );
 
         /* Swerve Stop -- Pressing the button completely stops the robot's motion. */

src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java

@@ -0,0 +1,31 @@
+package frc.robot.commands.swerve;
+
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand;
+import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand;
+import frc.robot.controllers.BaseDriveController;
+import frc.robot.subsystems.intake.IntakePivotSubsystem;
+import frc.robot.subsystems.intake.IntakeRollerSubsystem;
+import frc.robot.subsystems.leds.LEDSubsystem;
+import frc.robot.subsystems.leds.LightBarSubsystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.vision.NoteDetectionWrapper;
+
+public class AutoIntakeSequence extends SequentialCommandGroup {
+
+    public AutoIntakeSequence(IntakeRollerSubsystem intakeRollerSubsystem,
+                              IntakePivotSubsystem intakePivotSubsystem,
+                              SwerveSubsystem swerveSubsystem,
+                              NoteDetectionWrapper noteDetector,
+                              BaseDriveController driveController,
+                              LightBarSubsystem lightBarSubsystem) {
+
+        addCommands(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1),
+                    new ParallelRaceGroup(
+                        new NoteAlignCommand(swerveSubsystem, noteDetector, driveController),
+                        new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem)
+                    )
+        );
+    }
+}

src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java

@@ -1,6 +1,8 @@
 package frc.robot.commands.swerve;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.controllers.BaseDriveController;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
@@ -12,8 +14,10 @@ public class NoteAlignCommand extends Command {
     private final SwerveSubsystem swerveSubsystem;
     private final NoteDetectionWrapper noteDetector;
     private final BaseDriveController driveController;
+    private final PIDController yawController;
 
     private double noteYawOffsetDegrees;
+    private double angularVelocity;
 
     /**
      * Constructs a new {@link NoteAlignCommand}.
@@ -29,6 +33,13 @@ public NoteAlignCommand(SwerveSubsystem swerveSubsystem,
         this.noteDetector = noteDetector;
         this.driveController = driveController;
 
+        this.yawController = new PIDController(2.5, 0, 0);
+        yawController.setSetpoint(0);
+        yawController.setTolerance(1);
+        // yawController.enableContinuousInput(-90, 90);
+
+        angularVelocity = 0;
+
         this.addRequirements(swerveSubsystem);
     }
 
@@ -63,10 +74,14 @@ after. If it remains out of frame, this issue should be rather evident (the robo
             is probably not worth logging. */
         }
 
-        swerveSubsystem.setDrivePowersWithHeadingLock(
-            driveController.getForwardPower(), driveController.getLeftPower(), 
-            swerveSubsystem.getRobotPosition().getRotation().plus(
-            Rotation2d.fromDegrees(1.25 * -noteYawOffsetDegrees))
+        angularVelocity = yawController.calculate(
+            Units.degreesToRadians(noteYawOffsetDegrees) - angularVelocity * .1
+        );
+
+        swerveSubsystem.setRobotRelativeDrivePowers(
+            .3,
+            driveController.getLeftPower(),
+            angularVelocity / SwerveSubsystem.MAX_OMEGA
         );
     }