Java_2026/src/main/java/frc/robot/commands/lightStrip/SetLed.java at bb930787f6609157af2cc593a1573c85d3e0ef7e · FRC4048/Java_2026 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
package frc.robot.commands.lightStrip;

import frc.robot.constants.enums.ShootingState;
import frc.robot.constants.enums.Trench;
import frc.robot.constants.enums.ShootingState.ShootState;
import frc.robot.subsystems.LightStripSubsystem;
import frc.robot.subsystems.swervedrive.SwerveSubsystem;
import frc.robot.utils.BlinkinPattern;
import frc.robot.utils.logging.commands.LoggableCommand;
import org.littletonrobotics.junction.Logger;

import java.util.function.BooleanSupplier;

public class SetLed extends LoggableCommand{

    private final LightStripSubsystem lightStrip;
    private final SwerveSubsystem drivebase;
    private ShootingState shootingState;
    private double x;
    private double y;
    private BooleanSupplier stalledIntake;

    public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState, BooleanSupplier stalledIntake) {

        this.lightStrip = lightStrip;
        this.drivebase = drivebase;
        this.shootingState = shootingState;
        this.stalledIntake = stalledIntake;
        addRequirements(lightStrip);

    }

    @Override
    public void initialize() {

    }

    @Override
    public void execute() {
        Logger.recordOutput("IntakeJammed", stalledIntake);
        x = drivebase.getPose().getX();
        y = drivebase.getPose().getY();

        // If statement checks if the robot is near the trench
        if (x > Trench.RED_BOTTOM_LOWER.getX() && x < Trench.RED_BOTTOM_HIGHER.getX() && y > Trench.RED_BOTTOM_LOWER.getY() && y < Trench.RED_BOTTOM_HIGHER.getY() ||
            x > Trench.RED_TOP_LOWER.getX() && x < Trench.RED_TOP_HIGHER.getX() && y > Trench.RED_TOP_LOWER.getY() && y < Trench.RED_TOP_HIGHER.getY() ||
            x > Trench.BLUE_BOTTOM_LOWER.getX() && x < Trench.BLUE_BOTTOM_HIGHER.getX() && y > Trench.BLUE_BOTTOM_LOWER.getY() && y < Trench.BLUE_BOTTOM_HIGHER.getY() ||
            x > Trench.BLUE_TOP_LOWER.getX() && x < Trench.BLUE_TOP_HIGHER.getX() && y > Trench.BLUE_TOP_LOWER.getY() && y < Trench.BLUE_TOP_HIGHER.getY()) {

                if (shootingState.getShootState() != ShootState.STOPPED) { // Light strip only blinks if the shooting state is not stopped
                    lightStrip.setPattern(BlinkinPattern.STROBE_RED);
                }

        } else if (stalledIntake.getAsBoolean()) {
            lightStrip.setPattern(BlinkinPattern.ORANGE);
        } else {

            switch (shootingState.getShootState()) {
            case STOPPED:
                lightStrip.setPattern(BlinkinPattern.WHITE);
                break;
            case FIXED:
                lightStrip.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE);
                break;
            case FIXED_2:
                lightStrip.setPattern(BlinkinPattern.RED_ORANGE);
                break;
            case SHOOTING_HUB:
                lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE);
                break;
            case SHUTTLING:
                lightStrip.setPattern(BlinkinPattern.GREEN);
                break;
            }

        }

    }

    @Override
    public boolean isFinished() {
        return false;
    }

    @Override
    public void end(boolean interrupted) {

    }

}