Merge pull request #91 from DeepBlueRobotics/limelightDisabled-flag

Limelight-disabling flag

Author: FriedLongJohns

src/main/java/org/carlmontrobotics/Config.java

@@ -24,6 +24,7 @@ public abstract class Config implements Sendable {
     protected boolean exampleFlagEnabled = false;
     protected boolean swimShady = false;
     protected boolean setupSysId = false;
+	protected boolean limelightDisabled = false;
     protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of
                                                        // outtake through SmartDashboard
                                                        // Note: disables joystick control of arm and
@@ -49,7 +50,11 @@ public boolean useSmartDashboardControl() {
         return useSmartDashboardControl;
     }
 
-    // --- For clarity, place additional config settings ^above^ this line ---
+    public boolean isLimelightDisabled() {
+        return limelightDisabled;
+    }
+
+    // --- Place additional config settings ^above^ this line ---
 
     private static class MethodResult {
         String methodName = null;

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

@@ -11,6 +11,8 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 
+import static org.carlmontrobotics.Config.CONFIG;
+
 public class AimArmSpeaker extends Command {
     private final Arm arm;
     private final Limelight ll;
@@ -45,6 +47,6 @@ public void end(boolean interrupted) {}
     // Returns true when the command should end.
     @Override
     public boolean isFinished() {
-        return arm.armAtSetpoint();
+        return arm.armAtSetpoint() || CONFIG.isLimelightDisabled();
     }
 }

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

@@ -16,6 +16,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
+import static org.carlmontrobotics.Config.CONFIG;
+
 public class AlignToApriltag extends Command {
 
     public final TeleopDrive teleopDrive;
@@ -106,6 +108,6 @@ public boolean isFinished() {
         // return false;
         // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint());
         // SmartDashboard.putNumber("Error", rotationPID.getPositionError());
-        return rotationPID.atSetpoint();
+        return rotationPID.atSetpoint() || CONFIG.isLimelightDisabled();
     }
 }

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

@@ -16,6 +16,8 @@
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj2.command.Command;
 
+import static org.carlmontrobotics.Config.CONFIG;
+
 public class AlignToNote extends Command {
 
     public final TeleopDrive teleopDrive;
@@ -54,7 +56,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return false;
+        return CONFIG.isLimelightDisabled();
         // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint());
         // SmartDashboard.putNumber("Error", rotationPID.getPositionError());
         // return rotationPID.atSetpoint();

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

@@ -18,6 +18,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
+import static org.carlmontrobotics.Config.CONFIG;
+
 public class AutoMATICALLYGetNote extends Command {
   /** Creates a new AutoMATICALLYGetNote. */
   private Drivetrain dt;
@@ -125,6 +127,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return intake.outtakeDetectsNote();
+    return intake.outtakeDetectsNote() || CONFIG.isLimelightDisabled();
   }
 }

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

@@ -9,6 +9,8 @@
 import org.carlmontrobotics.subsystems.Limelight;
 import org.carlmontrobotics.subsystems.LimelightHelpers;
 
+import static org.carlmontrobotics.Config.CONFIG;
+
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -54,6 +56,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return timer.get() >= 0.5;
+    return timer.get() >= 0.5 || CONFIG.isLimelightDisabled();
   }
 }

src/main/java/org/carlmontrobotics/subsystems/Limelight.java

@@ -136,4 +136,4 @@ public double getOptimizedArmAngleRadsMT2() {
   public boolean seesTag() {
     return LimelightHelpers.getTV(SHOOTER_LL_NAME);
   }
-}
+}