Edit LED

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

@@ -191,8 +191,6 @@ private void configureButtonBindings() {
 
     xboxStartButton.whileHeld(new RunBallFeed(ballFeed, 0.75));
     xBoxBackButton.whileHeld(new RunBallFeed(ballFeed, -0.75));
-
-    new LimelightLED(limelight, 0); // Disable Limelight LEDs
 
     SmartDashboard.putData("Score Power Cell", new ShootPowerCell(intake, ballFeed, drivetrain, shooter));
     SmartDashboard.putNumber("Ballfeed Speed", ballFeed.getBallFedVelocity());
@@ -236,7 +234,6 @@ private void configureButtonBindings() {
     SmartDashboard.putNumber("Gyro Angle", drivetrain.ahrs.getAngle());
 
     SmartDashboard.putData("Limelight PID", new LimelightAutoAim(limelight, drivetrain));
-    //SmartDashboard.putNumber("LkFF", limelight.getPIDController().getP());
 
     // Interstellar Accuracy Challenge Speed Buttons
     /* Ignore this for now

CompSeasonBot2021/src/main/java/frc/robot/commands/AutoPIDShoot.java

@@ -36,7 +36,7 @@ public AutoPIDShoot(Hood hood, Limelight limelight, BallFeed ballfeed, Drivetrai
   @Override
   public void initialize() {
     new ExtendHood(this.hood);
-    this.limelight.toggleLEDs(1);
+    this.limelight.setLEDState(1);
     new RunBothFeeders(this.ballfeed);
     new RunShooterPID(this.shooter, 2500);
     new WaitCommand(2.0);
@@ -59,7 +59,7 @@ public void execute() {}
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    this.limelight.toggleLEDs(0);
+    this.limelight.setLEDState(0);
   }
 
   // Returns true when the command should end.

CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightAutoAim.java

@@ -44,7 +44,7 @@ public LimelightAutoAim(Limelight limelightInstance, Drivetrain drivetrainInstan
     this.drivetrain = drivetrainInstance;
     addRequirements(limelight);
 
-    this.limelight.toggleLEDs(1);
+    this.limelight.setLEDState(1);
 
     // Configure additional PID options by calling `getController` here.
     limelight.getPIDController().setTolerance(this.limelight.getThreshold()); // 1 degree for now
@@ -53,7 +53,7 @@ public LimelightAutoAim(Limelight limelightInstance, Drivetrain drivetrainInstan
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    this.limelight.toggleLEDs(0);
+    this.limelight.setLEDState(0);
   }
 
   // Returns true when the command should end.

CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightLED.java

@@ -24,7 +24,7 @@ public LimelightLED(Limelight limelight, int state) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    limelight.toggleLEDs(this.state);
+    limelight.setLEDState(this.state);
     this.finished = true;
   }
 

CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightCenter.java

@@ -34,7 +34,7 @@ public class LimelightCenter extends CommandBase {
   public LimelightCenter(Limelight limelight, Drivetrain drivetrain) {
     this.drivetrain = drivetrain;
     this.limelight = limelight;
-    this.limelight.toggleLEDs(1);
+    this.limelight.setLEDState(1);
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(limelight, drivetrain);
   }
@@ -92,7 +92,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    this.limelight.toggleLEDs(0);
+    this.limelight.setLEDState(0);
   }
 
   // Returns true when the command should end.

CompSeasonBot2021/src/main/java/frc/robot/subsystems/Limelight.java

@@ -20,6 +20,8 @@ public class Limelight extends SubsystemBase {
   private NetworkTableEntry targetAreaEntry;
   private NetworkTableEntry ledModeEntry;
 
+  private boolean LEDsEnabled = false;
+
   private final PIDController limelightPID;
   private final double kP, kI, kD, kMaxOutput, kMinOutput, kThreshold;  //Gains, may move elsewhere.
   //private final double kIz, kFF;
@@ -34,6 +36,9 @@ public Limelight() {
     this.yOffsetEntry = limelight.getEntry("ty");
     this.targetAreaEntry = limelight.getEntry("ta");
     this.ledModeEntry = limelight.getEntry("ledMode");
+
+    this.setLEDState(0);
+    this.LEDsEnabled = false;
 
     kP = 0.03;
     kI = 0;
@@ -118,14 +123,31 @@ public double getDistance() {
   }*/
 
   /**
-   * Toggles the LED of the limelight.
-   * @param state - 0 for off and 1 for on
+   * Sets the state of the Limelight LEDs.
+   * @param state - 0 is off and 1 is on.
    */
-  public void toggleLEDs(int state) {
+  public void setLEDState(int state) {
     if (state == 0) {
-      this.ledModeEntry.setNumber(1);
+      this.ledModeEntry.setNumber(1); // 1 is off.
+      this.LEDsEnabled = true;
     } else if (state == 1) {
-      this.ledModeEntry.setNumber(3);
+      this.ledModeEntry.setNumber(3); // 3 is on.
+      this.LEDsEnabled = false;
+    } else {
+      return;
+    }
+  }
+
+  /**
+   * Toggles the Limelight LEDs.
+   */
+  public void toggleLEDs() {
+    if (this.LEDsEnabled == false) {
+      this.ledModeEntry.setNumber(3); // 1 is on.
+      this.LEDsEnabled = true;
+    } else if (this.LEDsEnabled == true) {
+      this.ledModeEntry.setNumber(1); // 3 is off.
+      this.LEDsEnabled = false;
     } else {
       return;
     }