diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 26f3fdc..9a5b419 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -394,9 +394,16 @@ public final class Constants { } public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; + public static final int LED_SPARK_ID = 9; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + + public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; + public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; + public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; + + public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; + public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index caec0f0..52f5bb1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,7 +56,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems -// import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; import frc4388.robot.subsystems.Lidar; @@ -85,10 +85,10 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - // private final LED m_robotLED = new LED(); + private final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 99ce643..f885ca7 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -19,12 +19,17 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ElevatorConstants; +import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.subsystems.LED; +import frc4388.utility.LEDPatterns; +import frc4388.utility.TimesNegativeOne; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ private TalonFX elevatorMotor; private TalonFX endeffectorMotor; + private LED led; private long wait = 0; private long maxWait = 1000; @@ -56,9 +61,10 @@ public class Elevator extends SubsystemBase { private CoordinationState currentState; - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch) { + public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { elevatorMotor = elevatorTalonFX; endeffectorMotor = endeffectorTalonFX; + this.led = led; this.basinBeamBreak = basinLimitSwitch; this.endeffectorLimitSwitch = endeffectorLimitSwitch; @@ -89,6 +95,7 @@ public class Elevator extends SubsystemBase { endeffectorMotor.set(0); } + public void transitionState(CoordinationState state) { currentState = state; switch (currentState) { @@ -96,66 +103,77 @@ public class Elevator extends SubsystemBase { wait = System.currentTimeMillis() + maxWait; PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.WAITING_PATTERN); break; } case WatingBeamTripped: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.DOWN_PATTERN); break; } case Ready: { PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.DOWN_PATTERN); break; } case Hovering: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.READY_PATTERN); break; } case L2Score: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } case ScoringFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } case PrimedThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } case ScoringThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } case BallRemoverL2Primed: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } case BallRemoverL2Go: { PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } @@ -168,6 +186,7 @@ public class Elevator extends SubsystemBase { case BallRemoverL3Go: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); break; } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e9e070c..41fa708 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -22,69 +22,16 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - static AddressableLED m_led; - static AddressableLEDBuffer m_ledBuffer; - static LED m_self; - /** - * Add your docs here. - */ + private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + private double mode = LEDConstants.DEFAULT_PATTERN.getValue(); - public LED(){ - if(m_self != null) - return; - m_led = new AddressableLED(9); - m_ledBuffer = new AddressableLEDBuffer(10); - m_led.setLength(m_ledBuffer.getLength()); - m_led.setData(m_ledBuffer); - m_led.start(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); - } - public static LED getInstance() { - if(m_self == null) - m_self = new LED(); - return m_self; + public void setMode(LEDPatterns pattern){ + this.mode = pattern.getValue(); } + @Override - public void periodic(){ - //gamermode(); - //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); - return; - } - static int firstcolor = 0; - static void gamermode() { - for(int i = 0; i < m_ledBuffer.getLength(); i++) { - final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; - setLEDHSV(i, hue, 255, 128); - } - firstcolor +=3; - firstcolor %= 180; - } - /** - * Add your docs here. - */ - public static void updateLED (){ - gamermode(); - // m_LEDController.set(m_currentPattern.getValue()); + public void periodic() { + LEDController.set(mode); } - /** - * Add your docs here. - */ - public static void setLEDRGB(int lednum, int r, int g, int b){ - m_ledBuffer.setRGB(lednum, r, g, b); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); - } - public static void setLEDHSV(int lednum, int hue, int sat, int val){ - m_ledBuffer.setRGB(lednum, hue, sat, val); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); - } - /** - * Add your docs here. - * @return - */ - public AddressableLEDBuffer getBuffer() { - return m_ledBuffer; - } } \ No newline at end of file