continue lights subsystem

team581 · Aug 26, 2023 · a732c5a · a732c5a
1 parent 78d3b17
commit a732c5a
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/lights/LightsSubsystem.java b/src/main/java/frc/robot/lights/LightsSubsystem.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.intake.HeldGamePiece;
 import frc.robot.intake.IntakeState;
@@ -15,6 +16,11 @@
 import frc.robot.util.scheduling.SubsystemPriority;
 
 public class LightsSubsystem extends LifecycleSubsystem{
+
+  private static final double FAST_BLINK_DURATION = 0.08;
+  private static final double SLOW_BLINK_DURATION = 0.25;
+
+
   private final CANdle candle;
   private final IntakeSubsystem intake;
   private final SuperstructureManager superstructure;
@@ -36,7 +42,8 @@ public LightsSubsystem(
   }
   public void enabledPeriodic(){
     HeldGamePiece gamePiece = intake.getGamePiece();
-    IntakeState intakeMode = intake.get();
+    IntakeState intakeMode = intake.getIntakeState();
+    HeldGamePiece superstructureMode = superstructure.();
 
     if (DriverStation.isDisabled()) {
       if (FmsSubsystem.isRedAlliance()) {
@@ -47,15 +54,15 @@ public void enabledPeriodic(){
         blinkPattern = BlinkPattern.SOLID;
       }
     }else if (gamePiece == HeldGamePiece.CUBE) {
-      if (intakeMode == IntakeMode.INTAKE_CUBE) {
+      if (intakeMode == IntakeState.INTAKE_CUBE) {
         color = Color.kPurple;
         blinkPattern = BlinkPattern.BLINK_FAST;
       } else {
         color = Color.kPurple;
         blinkPattern = BlinkPattern.SOLID;
       }
     } else if (gamePiece == HeldGamePiece.CONE) {
-      if (intakeMode == IntakeMode.INTAKE_CONE) {
+      if (intakeMode == IntakeState.INTAKE_CONE) {
         color = Color.kYellow;
         blinkPattern = BlinkPattern.BLINK_FAST;
       } else {
@@ -72,7 +79,34 @@ public void enabledPeriodic(){
       color = Color.kWhite;
       blinkPattern = BlinkPattern.SOLID;
     }
+
+    Color8Bit color8Bit = new Color8Bit(color);
+    if (blinkPattern == BlinkPattern.SOLID) {
+      candle.setLEDs(color8Bit.red, color8Bit.green, color8Bit.blue);
+    } else {
+      double time = blinkTimer.get();
+      double onDuration = 0;
+      double offDuration = 0;
+
+      if (blinkPattern == BlinkPattern.BLINK_FAST) {
+        onDuration = FAST_BLINK_DURATION;
+        offDuration = FAST_BLINK_DURATION * 2;
+      } else if (blinkPattern == BlinkPattern.BLINK_SLOW) {
+        onDuration = SLOW_BLINK_DURATION;
+        offDuration = SLOW_BLINK_DURATION * 2;
+      }
+
+      if (time >= offDuration) {
+        blinkTimer.reset();
+        candle.setLEDs(0, 0, 0);
+      } else if (time >= onDuration) {
+        candle.setLEDs(color8Bit.red, color8Bit.green, color8Bit.blue);
+      }
+    }
+
+
   }
 
 
+
 }
diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java
@@ -1,6 +1,9 @@
 package frc.robot.managers.superstructure;
 
+import java.lang.Thread.State;
+
 import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.intake.HeldGamePiece;
 import frc.robot.intake.IntakeSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 
@@ -33,5 +36,7 @@ public boolean atGoal(Rotation2d shoulderAngle, Rotation2d wristAngle) {
 public void enabledPeriodic() {
   motionmanager.set
 }
-  // create a setPositionCommand method, which finishes once at goal
+
+
+// create a setPositionCommand method, which finishes once at goal
 }