From 3a9964f982c29176889af333f84a1f897dd26516 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Sun, 14 Jul 2024 17:25:08 -0700 Subject: [PATCH] initial --- src/main/java/org/carlmontrobotics/Config.java | 7 ++++++- .../java/org/carlmontrobotics/commands/AimArmSpeaker.java | 4 +++- .../org/carlmontrobotics/commands/AlignToApriltag.java | 4 +++- .../java/org/carlmontrobotics/commands/AlignToNote.java | 4 +++- .../carlmontrobotics/commands/AutoMATICALLYGetNote.java | 4 +++- .../java/org/carlmontrobotics/commands/MoveToNote.java | 4 +++- .../java/org/carlmontrobotics/subsystems/Limelight.java | 2 +- 7 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index f8a03627..e92be016 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/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; diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java index 7e2dd3cc..8973b5d8 100644 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java +++ b/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(); } } diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java b/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java index c1899263..f15887b7 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java +++ b/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(); } } diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToNote.java b/src/main/java/org/carlmontrobotics/commands/AlignToNote.java index e0fb9b36..f8c941ba 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToNote.java +++ b/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(); diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index dbd3ff32..5ae13ea0 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java +++ b/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(); } } diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java index 81a4dc7a..8f5dd2bb 100644 --- a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java +++ b/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(); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index b9e04aa9..cdb41ac3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -136,4 +136,4 @@ public double getOptimizedArmAngleRadsMT2() { public boolean seesTag() { return LimelightHelpers.getTV(SHOOTER_LL_NAME); } -} \ No newline at end of file +}