Removed isFinished from DefaultCandle and Updated logic.

Needs tested before merging.
FRCTeam1987 · Mar 19, 2024 · bc1c238 · bc1c238
1 parent 4fd17dc
commit bc1c238
Showing 1 changed file with 13 additions and 31 deletions.
diff --git a/src/main/java/frc/robot/commands/qol/DefaultCANdle.java b/src/main/java/frc/robot/commands/qol/DefaultCANdle.java
@@ -30,57 +30,39 @@ public void initialize() {}
 
   @Override
   public void execute() {
-    double dist = Util.getInterpolatedDistance(PHOTON_SPEAKER);
-    long currentTimestamp = (long) (Timer.getFPGATimestamp() / BLINK_CONSTANT);
-    if (SHOOTER.isRearBroken()
-        && SHOOTER.isCenterBroken()
-        && PHOTON_SPEAKER.hasTargets()
-        && (dist > 2.25 && dist < 4.25)
-        && (Util.isWithinTolerance(PHOTON_SPEAKER.getYawVal(), 0.0, 1))
-        && (currentTimestamp % 2 == 0)) {
-      CANDLES.setColorRightOff();
+    if (!SHOOTER.isRearBroken()) {
+      CANDLES.setColorLeftRed();
       return;
     }
 
-    if (SHOOTER.isCenterBroken()
-        && PHOTON_SPEAKER.hasTargets()
-        && (dist > 2.25 && dist < 4.25)
-        && (Util.isWithinTolerance(PHOTON_SPEAKER.getYawVal(), 0.0, 1))
-        && !(currentTimestamp % 2 == 0)) {
-      CANDLES.setColorRightGreen();
-      return;
-    }
+    if (!SHOOTER.isCenterBroken()) {
+      CANDLES.setColorLeftBrown();
 
-    if (SHOOTER.isCenterBroken()
-        && PHOTON_SPEAKER.hasTargets()
-        && (dist > 2.25 && dist < 4.25)
-        && !(Util.isWithinTolerance(PHOTON_SPEAKER.getYawVal(), 0.0, 1))) {
-      CANDLES.setColorRightGreen();
       return;
     }
 
-    if (SHOOTER.isCenterBroken() && PHOTON_SPEAKER.hasTargets() && !(dist > 2.25 && dist < 4.25)) {
+    CANDLES.setColorLeft(0, 128, 128);
+    if (!PHOTON_SPEAKER.hasTargets()) {
       CANDLES.setColorRightRed();
       return;
     }
 
-    if (SHOOTER.isCenterBroken() && !PHOTON_SPEAKER.hasTargets()) {
+    double dist = Util.getInterpolatedDistance(PHOTON_SPEAKER);
+
+    if (dist < 2.25 && dist > 4.25) {
       CANDLES.setColorRightRed();
       return;
     }
 
-    if (!SHOOTER.isCenterBroken()) {
-      CANDLES.setColorLeftBrown();
+    if (Util.isWithinTolerance(PHOTON_SPEAKER.getYawVal(), 0.0, 1)
+        && ((Timer.getFPGATimestamp()) / BLINK_CONSTANT) == 0) {
+      CANDLES.setColorRightOff();
       return;
     }
 
-    CANDLES.setColorLeftRed();
+    CANDLES.setColorRightGreen();
   }
 
   @Override
   public void end(boolean interrupted) {}
-
-  public boolean isFinished() {
-    return false;
-  }
 }