cleaned out some commands

src/main/java/org/carlmontrobotics/RobotContainer.java

@@ -184,16 +184,15 @@ private void setBindingsDriver() {
 
     new POVButton(driverController, 0)
             .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter),
-                    new AutoMATICALLYGetNote(drivetrain, limelight,
-                            intakeShooter)));
+                    new AutoMATICALLYGetNote(drivetrain, limelight)));
 
     axisTrigger(driverController, Axis.kLeftTrigger)
         // .onTrue(new AlignToApriltag(drivetrain, limelight));
         .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false)))
         .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true)));
 
     axisTrigger(driverController, Manipulator.SHOOTER_BUTTON)
-        .whileTrue(new AlignToApriltagMegatag2(drivetrain, limelight));
+            .whileTrue(new AlignToApriltag(drivetrain, limelight));
     new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue(
         new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain));
     new JoystickButton(driverController, Driver.rotateFieldRelative90Deg)

src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java

@@ -9,36 +9,34 @@
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AimArmSpeaker extends Command {
-  private final Arm arm;
-  private final Limelight ll;
-
-  /** Creates a new AimOuttakeSpeaker. */
-  public AimArmSpeaker(Arm arm, Limelight ll) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(this.arm = arm);
-    this.ll = ll;
-  }
-
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    double goal = ll.getOptimizedArmAngleRadsMT2();
-    arm.setArmTarget(goal);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return arm.armAtSetpoint();
-  }
+    private final Arm arm;
+    private final Limelight ll;
+
+    /** Creates a new AimOuttakeSpeaker. */
+    public AimArmSpeaker(Arm arm, Limelight ll) {
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(this.arm = arm);
+        this.ll = ll;
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        double goal = ll.getOptimizedArmAngleRadsMT2();
+        arm.setArmTarget(goal);
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {}
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {}
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return arm.armAtSetpoint();
+    }
 }

src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AlignToApriltag extends Command {
@@ -21,18 +22,25 @@ public class AlignToApriltag extends Command {
     public final Drivetrain drivetrain;
     private Limelight limelight;
 
-    public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1],
-            thetaPIDController[2]);
+    public final PIDController rotationPID = new PIDController(
+                    SmartDashboard.getNumber("apriltag align kp",
+                                    thetaPIDController[0]),
+                    SmartDashboard.getNumber("apriltag align ki",
+                                    thetaPIDController[1]),
+                    SmartDashboard.getNumber("apriltag align kd",
+                                    thetaPIDController[2]));
 
     public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
-        this.limelight = limelight;  
+            this.limelight = limelight;
         this.drivetrain = drivetrain;
         this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();
 
         rotationPID.enableContinuousInput(-180, 180);
         Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
-                .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
-        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
+                        .plus(Rotation2d.fromRadians(
+                                        limelight.getRotateAngleRadMT2()));
+        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(),
+                        -180, 180));
         rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
         SendableRegistry.addChild(this, rotationPID);
         addRequirements(drivetrain);
@@ -41,14 +49,21 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
     @Override
     public void execute() {
         Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
-                .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
-        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
+                        .plus(Rotation2d.fromRadians(
+                                        limelight.getRotateAngleRadMT2()));
+        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(),
+                        -180, 180));
+        double rotationDrive = rotationPID.calculate(drivetrain.getHeading());
+        if (!limelight.seesTag()) {
+                rotationDrive = 0;
+        }
+
         if (teleopDrive == null)
-            drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
+                drivetrain.drive(0, 0, rotationDrive);
         else {
             double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
             drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1],
-                    rotationPID.calculate(drivetrain.getHeading()));
+                            rotationDrive);
         }
     }
 

src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java

@@ -27,11 +27,9 @@ public class AutoMATICALLYGetNote extends Command {
   Timer timer = new Timer();
   private IntakeShooter intake;
 
-  public AutoMATICALLYGetNote(Drivetrain dt, Limelight ll,
-      IntakeShooter intake) {
+  public AutoMATICALLYGetNote(Drivetrain dt, Limelight ll) {
     addRequirements(this.dt = dt);
     this.ll = ll;
-    this.intake = intake;
     //addRequirements(this.effector = effector);
     // Use addRequirements() here to declare subsystem dependencies.
   }
@@ -51,6 +49,11 @@ public void execute() {
     double forwardDistErrMeters = ll.getDistanceToNoteMeters(); 
     double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad);
 
+    forwardDistErrMeters = Math.max(
+        forwardDistErrMeters
+            * SmartDashboard.getNumber("forward speed multiplier", 1.5),
+        MIN_MOVEMENT_METERSPSEC);
+
     if (LimelightHelpers.getTV(INTAKE_LL_NAME)) {
       dt.drive(forwardDistErrMeters, strafeDistErrMeters, angleErrRad);
     }
@@ -108,6 +111,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return intake.intakeDetectsNote();
+    return intake.outtakeDetectsNote();
   }
 }

src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java

@@ -8,7 +8,6 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.util.sendable.SendableRegistry;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class RotateToFieldRelativeAngle extends Command {