diff --git a/src/main/java/frc/robot/lights/LightsSubsystem.java b/src/main/java/frc/robot/lights/LightsSubsystem.java index 4778d54..c69b274 100644 --- 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,7 +54,7 @@ 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 { @@ -55,7 +62,7 @@ public void enabledPeriodic(){ 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 index 82c22d0..fbec9db 100644 --- 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 }