From 3c5dfd164d707286717cc5f00726ad437335013c Mon Sep 17 00:00:00 2001
From: CrolineCrois <maoninii@gmail.com>
Date: Mon, 8 Apr 2024 12:50:17 -0700
Subject: [PATCH] update note align

---
 .../robot/commands/swerve/AutoIntakeSequence.java    |  5 +++--
 .../frc/robot/commands/swerve/NoteAlignCommand.java  | 12 ++++++------
 2 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java b/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java
index 2215d032..ebdbd018 100644
--- a/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java
+++ b/src/main/java/frc/robot/commands/swerve/AutoIntakeSequence.java
@@ -1,5 +1,6 @@
 package frc.robot.commands.swerve;
 
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand;
@@ -22,8 +23,8 @@ public AutoIntakeSequence(IntakeRollerSubsystem intakeRollerSubsystem,
                               LightBarSubsystem lightBarSubsystem) {
 
         addCommands(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1),
-                    new ParallelRaceGroup(
-                        new NoteAlignCommand(swerveSubsystem, noteDetector, driveController),
+                    new ParallelCommandGroup(
+                        new NoteAlignCommand(swerveSubsystem, noteDetector, driveController).until(intakeRollerSubsystem::getFrontSensorValue),
                         new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem)
                     )
         );
diff --git a/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java
index 0fb4ebd5..ceebd834 100644
--- a/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java
+++ b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java
@@ -33,7 +33,7 @@ public NoteAlignCommand(SwerveSubsystem swerveSubsystem,
         this.noteDetector = noteDetector;
         this.driveController = driveController;
 
-        this.yawController = new PIDController(2.5, 0, 0);
+        this.yawController = new PIDController(4, 0, 0);
         yawController.setSetpoint(0);
         yawController.setTolerance(1);
         // yawController.enableContinuousInput(-90, 90);
@@ -59,11 +59,6 @@ public void initialize() {
 
     }
 
-    @Override
-    public boolean isFinished() {
-        return !driveController.getNoteAlign().getAsBoolean();
-    }
-
     @Override
     public void execute() {
         try {
@@ -88,5 +83,10 @@ after. If it remains out of frame, this issue should be rather evident (the robo
     @Override
     public void end(boolean interrupted) {
         System.out.println("Ended NoteAlignCommand");
+        swerveSubsystem.setRobotRelativeDrivePowers(
+            0,
+            0,
+            0
+        );
     }
 }