Note align

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

@@ -534,6 +534,11 @@ private void configureBindings() {
                     mechController.setRumble(RumbleType.kBothRumble, 0);
                 }
             }
+            if (shooterFlywheelSubsystem.atSpeed()) {
+                mechController.setRumble(RumbleType.kBothRumble, .4);
+            } else {
+                mechController.setRumble(RumbleType.kBothRumble, 0);
+            }
 
             if (noteInBack 
                 && !intakeRollerSubsystem.getRockwellSensorValue()
@@ -550,7 +555,7 @@ private void configureBindings() {
         }, shooterFlywheelSubsystem
         ));
 
-        dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .6).onlyWhile(dPadRight));
+        dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .65, mechController).onlyWhile(dPadRight));
 
         // intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
         //     intakePosition = MathUtil.clamp(intakePosition, 0, 1);

src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelShuttleCommand.java

@@ -4,6 +4,8 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.FieldManagementSubsystem;
 import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem;
@@ -16,16 +18,17 @@ public class ShooterFlywheelShuttleCommand extends Command {
 
     private static final double MAX_ROTATION_POWER = 0.3;
     private static final double ERROR_MULTIPLIER = 0.08;
-    private static final double TARGET_ANGLE = Units.degreesToRadians(70);
+    private static final double TARGET_ANGLE = Units.degreesToRadians(42);
 
-    private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(0), Units.inchesToMeters(323));
-    private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(323));
+    private final Translation2d BLUE_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(0), Units.inchesToMeters(300));
+    private final Translation2d RED_SHUTTLE_POINT = new Translation2d(Units.inchesToMeters(652.73), Units.inchesToMeters(300));
 
     private final SwerveSubsystem swerveSubsystem;
     private final ShooterFlywheelSubsystem flywheelSubsystem;
     private final ShooterPivotSubsystem pivotSubsystem;
     private final FieldManagementSubsystem fmsSubsystem;
     private final double shooterSpeed;
+    private XboxController controller;
 
 
     private boolean redAlliance = true;
@@ -34,13 +37,14 @@ public class ShooterFlywheelShuttleCommand extends Command {
     /** Constructor for Shuttle Command. */
     public ShooterFlywheelShuttleCommand(SwerveSubsystem swerveSubsystem, 
            ShooterFlywheelSubsystem flywheelSubsystem, FieldManagementSubsystem fmsSubsystem, 
-           ShooterPivotSubsystem pivotSubsystem, double speed) {
+           ShooterPivotSubsystem pivotSubsystem, double speed, XboxController mechController) {
 
         this.swerveSubsystem = swerveSubsystem;
         this.flywheelSubsystem = flywheelSubsystem;
         this.pivotSubsystem = pivotSubsystem;
         this.fmsSubsystem = fmsSubsystem;
         this.shooterSpeed = speed;
+        this.controller = mechController;
 
         addRequirements(flywheelSubsystem, pivotSubsystem);
     }
@@ -55,6 +59,11 @@ public void initialize() {
     public void execute() {
         flywheelSubsystem.setShooterMotorSpeed(shooterSpeed);
         pivotSubsystem.setAngle(TARGET_ANGLE);
+        if(flywheelSubsystem.atSpeed()) {
+            controller.setRumble(RumbleType.kBothRumble, .4);
+        } else {
+            controller.setRumble(RumbleType.kBothRumble, 0);
+        }
     }
 
     /** Run after command has ended. Turns of shooter and swerve speed.*/

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

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

src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java

@@ -114,7 +114,7 @@ public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier re
                            Units.degreesToRadians(36), 
                            Units.degreesToRadians(31), //4
                            Units.degreesToRadians(28),
-                           Units.degreesToRadians(27),
+                           Units.degreesToRadians(27.5),
                            Units.degreesToRadians(28.5)};
 
         // X = distances, Y = angles in rads