update note align

grt192 · Apr 8, 2024 · 3c5dfd1 · 3c5dfd1
1 parent ad61120
commit 3c5dfd1
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 8 deletions.
diff --git 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
@@ -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
+        );
     }
 }