From d8d6319eb8f47414114f79c1813e6f91e6f3170c Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Thu, 28 Mar 2024 16:50:01 -0400 Subject: [PATCH] did some crazy led code, hoping it works --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 7 ++-- .../java/frc/robot/commands/auto/AutoAim.java | 17 +++++++--- .../frc/robot/commands/leds/DefaultLED.java | 32 +++++++++++++++++-- .../frc/robot/subsystems/LEDSubsystem.java | 5 +++ src/main/java/frc/robot/utils/UtilFuncs.java | 11 +++++++ 6 files changed, 63 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d54d25e..afe5105 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -173,7 +173,7 @@ public static class Presets { public static class Ports { public static final int DRIVER_CONTROLLER = 0; public static final int OPERATOR_CONTROLLER = 1; - public static final int LEDS = 0; // pwm port + public static final int LEDS = 1; // pwm port } // static field constants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87acee5..2bf05bf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer { private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem); private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem(); private final IntakeSubsystem _intakeSubsystem = new IntakeSubsystem(); - private final LEDSubsystem _ledSubsystem = new LEDSubsystem(0, 100); + private final LEDSubsystem _ledSubsystem = new LEDSubsystem(Constants.Ports.LEDS, 35); private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem(); // controllers (for driver and operator) @@ -111,7 +111,10 @@ public RobotContainer() { // Non drive/operate default commands _intakeSubsystem.setDefaultCommand(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.NONE)); - _ledSubsystem.setDefaultCommand(new DefaultLED(_ledSubsystem)); + _ledSubsystem.setDefaultCommand(new DefaultLED( + _ledSubsystem, + () -> _swerveSubsystem.atDesiredHeading() && _elevatorSubsystem.atDesiredHeight() && _shooterSubsystem.atDesiredAngle() + )); configureBindings(); diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java index 1d91524..84b3f88 100644 --- a/src/main/java/frc/robot/commands/auto/AutoAim.java +++ b/src/main/java/frc/robot/commands/auto/AutoAim.java @@ -6,7 +6,11 @@ import java.util.function.DoubleSupplier; +import org.ejml.dense.row.decomposition.UtilDecompositons_FDRM; + +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.elevator.SetElevator; import frc.robot.commands.shooter.SetShooter; import frc.robot.commands.swerve.SetHeading; @@ -14,11 +18,13 @@ import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; +import frc.robot.subsystems.LEDSubsystem.LEDState; +import frc.robot.utils.UtilFuncs; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoAim extends ParallelCommandGroup { +public class AutoAim extends SequentialCommandGroup { /** * Creates a new AutoAim. * @@ -53,10 +59,11 @@ private AutoAim( // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce), - new SetShooter(shooter, shooterAngle, runOnce), - new SetElevator(elevator, elevatorHeight, runOnce) - // some led command here + new ParallelCommandGroup( + new SetHeading(swerve, xSpeed, ySpeed, swerveHeading, runOnce), + new SetShooter(shooter, shooterAngle, runOnce), + new SetElevator(elevator, elevatorHeight, runOnce) + ).beforeStarting(() -> UtilFuncs.SetLEDs(LEDState.AIM)).finallyDo(() -> UtilFuncs.SetLEDs(LEDState.DEFAULT)) ); } diff --git a/src/main/java/frc/robot/commands/leds/DefaultLED.java b/src/main/java/frc/robot/commands/leds/DefaultLED.java index e4a914f..3b40a04 100644 --- a/src/main/java/frc/robot/commands/leds/DefaultLED.java +++ b/src/main/java/frc/robot/commands/leds/DefaultLED.java @@ -1,22 +1,33 @@ /* Copyright (C) 2024 Team 334. All Rights Reserved.*/ package frc.robot.commands.leds; +import java.util.function.BooleanSupplier; + import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.Constants.LEDColors; import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.utils.UtilFuncs; /** * @author Lucas Ou */ public class DefaultLED extends Command { - private LEDSubsystem _leds; + private final LEDSubsystem _leds; + + private final BooleanSupplier _isAimed; /** Creates a new TestLED. */ - public DefaultLED(LEDSubsystem leds) { + public DefaultLED( + LEDSubsystem leds, + BooleanSupplier isAimed + ) { // Use addRequirements() here to declare subsystem dependencies. _leds = leds; + _isAimed = isAimed; + addRequirements(_leds); } @@ -28,7 +39,22 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - _leds.setColor(UtilFuncs.GetAlliance() == Alliance.Red ? LEDColors.RED : LEDColors.BLUE); + switch (UtilFuncs.GetLEDs()) { + case DEFAULT: + _leds.setColor(UtilFuncs.GetAlliance() == Alliance.Red ? LEDColors.RED : LEDColors.BLUE); + break; + + case AIM: + if (!_isAimed.getAsBoolean()) { + _leds.blink(Constants.LEDColors.YELLOW, Constants.LEDColors.NOTHING, 1); + } else { + _leds.setColor(Constants.LEDColors.GREEN); + } + break; + + default: + break; + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 7d08002..a642f3c 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -23,6 +23,11 @@ public class LEDSubsystem extends SubsystemBase { private Timer _ledTimer = new Timer(); + public enum LEDState { + DEFAULT, + AIM + } + /** Creates a new LEDSubsystem. */ public LEDSubsystem(int port, int ledNumber) { _ledNumber = ledNumber; diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index 05340b7..01c9f66 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/src/main/java/frc/robot/utils/UtilFuncs.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.LEDSubsystem.LEDState; import frc.robot.utils.helpers.AllianceHelper; /** Any utility functions are here. */ @@ -23,6 +24,16 @@ public final class UtilFuncs { private static Pose3d SPEAKER_RED_POSE; private static Pose3d SPEAKER_BLUE_POSE; + private static LEDState _ledState = LEDState.DEFAULT; + + public static void SetLEDs(LEDState ledState) { + _ledState = ledState; + } + + public static LEDState GetLEDs() { + return _ledState; + } + /** * Converts limelight pose data into wpilib "Pose2d". *