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