diff --git a/CompSeasonBot2021/src/main/java/frc/robot/RobotContainer.java b/CompSeasonBot2021/src/main/java/frc/robot/RobotContainer.java index 3fa3e0c..934a92d 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/RobotContainer.java +++ b/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 diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/AutoPIDShoot.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/AutoPIDShoot.java index 00c674a..47d8a2b 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/AutoPIDShoot.java +++ b/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. diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightAutoAim.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightAutoAim.java index 04e1de0..d85849d 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightAutoAim.java +++ b/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. diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightLED.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightLED.java index a77d282..c4120c2 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/LimelightLED.java +++ b/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; } diff --git a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightCenter.java b/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightCenter.java index 246af74..64c71a0 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/commands/auto/LimelightCenter.java +++ b/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. diff --git a/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Limelight.java b/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Limelight.java index b805ca6..e8cbea0 100644 --- a/CompSeasonBot2021/src/main/java/frc/robot/subsystems/Limelight.java +++ b/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; }