From cc052eecadf481621b0e3fca5217e89127dd3f45 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Fri, 15 Mar 2024 19:12:17 -0700 Subject: [PATCH] LED stuff --- src/main/java/frc/robot/RobotContainer.java | 7 + .../frc/robot/commands/DefaultLEDCommand.java | 79 +++++++++++ src/main/java/frc/robot/commands/SetLED.java | 125 ------------------ .../frc/robot/subsystems/LEDSubsystem.java | 27 ++++ 4 files changed, 113 insertions(+), 125 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DefaultLEDCommand.java delete mode 100644 src/main/java/frc/robot/commands/SetLED.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21cfc10..24e24d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.commands.DefaultLEDCommand; import frc.robot.commands.IntakeArmPositionCommand; import frc.robot.commands.NoteIntakeCommand; import frc.robot.commands.NoteOuttakeCommand; @@ -36,6 +37,7 @@ import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; @@ -54,6 +56,7 @@ public class RobotContainer { private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); @@ -120,6 +123,10 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + m_ledSubsystem.setDefaultCommand( + new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem) + ); + m_robotDrive.setDefaultCommand( new RunCommand( () -> m_robotDrive.drive( diff --git a/src/main/java/frc/robot/commands/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/DefaultLEDCommand.java new file mode 100644 index 0000000..79c0fb9 --- /dev/null +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -0,0 +1,79 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.IntakeConstants; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class DefaultLEDCommand extends Command { + private LEDSubsystem m_ledSubsystem; + private IntakeSubsystem m_intakeSubsystem; + private ShooterSubsystem m_shooterSubsystem; + + private double armSetpoint; + private double shootSpeed; + private int[] rgb = new int[3]; + + /** Creates a new SetLED. */ + public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter) { + m_ledSubsystem = LED; + m_intakeSubsystem = intake; + m_shooterSubsystem = shooter; + addRequirements(m_ledSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // default color is invalid so the subsystem will set it to rainbow + rgb[0] = 256; + rgb[1] = 256; + rgb[2] = 256; + + armSetpoint = m_intakeSubsystem.getArmPosition(); + if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) { + rgb[0] = 255; + rgb[1] = 165; + rgb[2] = 0; + } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle) { + rgb[0] = 0; + rgb[1] = 0; + rgb[2] = 255; + } + + shootSpeed = m_shooterSubsystem.returnCurrentSpeed(); + if (shootSpeed > 500 && shootSpeed < 1899 * 2) {// if charging up + rgb[0] = 150; + rgb[1] = 150; + rgb[2] = 0; + } else if (shootSpeed > 1899 * 2) {// if the shooter is ready to shoot + rgb[0] = 0; + rgb[1] = 255; + rgb[2] = 0; + } + + m_ledSubsystem.setLED(rgb[0], rgb[1], rgb[2]); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java deleted file mode 100644 index f45d3a3..0000000 --- a/src/main/java/frc/robot/commands/SetLED.java +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.LEDSubsystem; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.ShooterConstants;; - -public class SetLED extends Command { - private LEDSubsystem m_ledSubsystem; - - private DriveSubsystem m_driveSubsystem = new DriveSubsystem(); - - private Pose2d m_odometry = m_driveSubsystem.getOdometry(); - - private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX, - ShooterConstants.kBottomShootX, - //Intake Positions - IntakeConstants.KLeftRingX, - IntakeConstants.KRightRingX, - }; - private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY, - ShooterConstants.kBottomShootY, IntakeConstants.KTopLeftRingY, - //Intake Positions - IntakeConstants.KTopLeftRingY, - IntakeConstants.KMiddleLeftRingY, - IntakeConstants.KBottomLeftRingY, - IntakeConstants.KTopRightRingY, - IntakeConstants.KSecondTopRightRingY, - IntakeConstants.KMiddleRightRingY, - IntakeConstants.KSecondBottomRightRingY, - IntakeConstants.KBottomRightRingY - }; - private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle, - ShooterConstants.kBottomShootAngle }; - - /** Creates a new SetLED. */ - public SetLED(LEDSubsystem LED) { - m_ledSubsystem = LED; - addRequirements(m_ledSubsystem); - - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double robotX = m_odometry.getX(); - double robotY = m_odometry.getY(); - for (int i = 0; i < 3; i++) { - boolean validX = false; - boolean validY = false; - boolean validA = false; - double X = m_pointsX[i]; - double Y = m_pointsY[i]; - double A = m_angles[i]; - if (X + ShooterConstants.kPositionTolerance > robotX - && robotX > X - ShooterConstants.kPositionTolerance) { - validX = true; - } - if (Y + ShooterConstants.kPositionTolerance > robotY - && robotY > Y - ShooterConstants.kPositionTolerance) { - validY = true; - break; - } - if (A + ShooterConstants.kAngleTolerance > m_odometry.getRotation().getDegrees() - && m_odometry.getRotation().getDegrees() > A - ShooterConstants.kAngleTolerance) { - validA = true; - break; - } - if (validX && validY && validA) { - m_ledSubsystem.setLED(0, 255, 0); - break; - } else { - m_ledSubsystem.setLED(255, 0, 0); - } - } - - //Lighting for Intake Positions - //Run through the next 8 positions on the list - for(int i = 3; i < 11; i++){ - //Figures out the rings x value (there is only 2 independent values per side) - double X; - if(i <= 5){ - X = m_pointsX[3]; - }else{ - X = m_pointsX[4]; - } - //Calculate distance to point - double dist = Math.hypot(robotX -X,robotY - m_pointsY[i]); - //Check if within an acceptable distance - if(dist >= IntakeConstants.kIntakeDist - IntakeConstants.KIntakePositionTolerance && - dist <= IntakeConstants.kIntakeDist + IntakeConstants.KIntakePositionTolerance ){ - //Estimates the proper angle - double estimateAngle = Math.toDegrees(Math.atan2(robotY - m_pointsY[i],robotX - X)); - //Checks if the angle is within an acceptable measure - if(Math.abs(m_odometry.getRotation().getDegrees() - estimateAngle) <= IntakeConstants.KIntakeAngleTolerance){ - m_ledSubsystem.setLED(0,0, 255); - } - } - } - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 8f4f94a..ff0d08d 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.LEDConstants; public class LEDSubsystem extends SubsystemBase { @@ -34,10 +35,36 @@ public LEDSubsystem() { * @param b Blue 0-255 */ public void setLED(int r, int g, int b) { + + // If we get an invalid value just set it to a rainbow + if (r > 255 || b > 255 || g > 255) { + rainbow(); + return; + } + for (var i = 0; i < LEDConstants.kLEDLength; i++) { m_LEDBuffer.setRGB(i, r, g, b); } m_LED.setData(m_LEDBuffer); SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); } + + private void rainbow() { + int m_rainbowFirstPixelHue = 0; + // For every pixel + for (var i = 0; i < m_LEDBuffer.getLength(); i++) { + // Calculate the hue - hue is easier for rainbows because the color + // shape is a circle so only one value needs to precess + final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_LEDBuffer.getLength())) % 180; + // Set the value + m_LEDBuffer.setHSV(i, hue, 255, 128); + } + // Increase by to make the rainbow "move" + m_rainbowFirstPixelHue += 3; + // Check bounds + m_rainbowFirstPixelHue %= 180; + + m_LED.setData(m_LEDBuffer); + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } } \ No newline at end of file