diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..15cec71 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -92,6 +92,10 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/simgui.json b/simgui.json index 8c63454..c639b3c 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,14 @@ { + "HALProvider": { + "Addressable LEDs": { + "0": { + "columns": 44 + }, + "window": { + "visible": true + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -36,5 +46,8 @@ }, "NetworkTables Info": { "visible": true + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 39a2bdb..27cbd23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -156,4 +156,10 @@ public static final class VisionConstants { public static final double kFieldLength = 16.54175; } + public static final class LEDConstants { + // TODO: find plz + public static final int kLEDPort = 0; + public static final int kLEDLength = 97; + } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5cc7405..adb44ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,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; @@ -34,6 +35,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; @@ -52,6 +54,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); @@ -130,6 +133,10 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + m_ledSubsystem.setDefaultCommand( + new DefaultLEDCommand(m_ledSubsystem, m_intakeSubsystem, m_shooterSubsystem, m_climberSubsystem) + ); + m_robotDrive.setDefaultCommand( new RunCommand( () -> m_robotDrive.drive( @@ -211,6 +218,8 @@ private void configureBindings() { return m_operatorController.getRightTriggerAxis() > 0.5; }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + + // Spin-up Shooter, Operator Controller left trigger new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime)) 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..b598a8f --- /dev/null +++ b/src/main/java/frc/robot/commands/DefaultLEDCommand.java @@ -0,0 +1,92 @@ +// 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.ClimberSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; + +public class DefaultLEDCommand extends Command { + private LEDSubsystem m_ledSubsystem; + private IntakeSubsystem m_intakeSubsystem; + private ShooterSubsystem m_shooterSubsystem; + private ClimberSubsystem m_climberSubsystem; + + private double armSetpoint; + private double shootSpeed; + private int[] rgb = new int[3]; + + /** Creates a new SetLED. */ + public DefaultLEDCommand(LEDSubsystem LED, IntakeSubsystem intake, ShooterSubsystem shooter, ClimberSubsystem climb) { + m_ledSubsystem = LED; + m_intakeSubsystem = intake; + m_shooterSubsystem = shooter; + m_climberSubsystem = climb; + 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; + + //check for arm angle & haveNote + armSetpoint = m_intakeSubsystem.getArmPosition(); + if (armSetpoint == IntakeConstants.kIntakeLoweredAngle) { + rgb[0] = 255; + rgb[1] = 0; + rgb[2] = 0; + } else if (armSetpoint == IntakeConstants.kIntakeRaisedAngle && m_intakeSubsystem.haveNote()) { + rgb[0] = 0; + rgb[1] = 0; + rgb[2] = 255; + } + + //check for shooter spinup and override b4 value + shootSpeed = m_shooterSubsystem.returnCurrentSpeed(); + if (shootSpeed > 500 && shootSpeed < 1899 * Math.PI) { // if charging up + rgb[0] = 128; + rgb[1] = 0; + rgb[2] = 255; + } else if (shootSpeed > 1899 * Math.PI) { // if the shooter is ready to shoot + rgb[0] = 0; + rgb[1] = 255; + rgb[2] = 0; + } + + //check for pneumatics state and override b4 value + if (m_climberSubsystem.getState() == Value.kForward){ + rgb[0] = 257; + rgb[1] = 257; + rgb[2] = 257; + } + + 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/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 3a42743..9bd6f9c 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -62,6 +62,10 @@ public void reverse() { m_state = kReverse; } + public Value getState(){ + return m_state; + } + /** * Toggles the state of the compressor (on/off) */ diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java new file mode 100644 index 0000000..eb1b672 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -0,0 +1,93 @@ +// 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.subsystems; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.LEDConstants; + +public class LEDSubsystem extends SubsystemBase { + private final AddressableLED m_LED = new AddressableLED(LEDConstants.kLEDPort); + private final AddressableLEDBuffer m_LEDBuffer = new AddressableLEDBuffer(LEDConstants.kLEDLength); + private int m_rainbowFirstPixelHue = 0; + + /** Creates a new {@link LEDSubsystem}. */ + public LEDSubsystem() { + m_LED.setLength(LEDConstants.kLEDLength); // 29 + m_LED.setData(m_LEDBuffer); + m_LED.start(); + + // Yellow when bot turns on. + setLED(50, 50, 0); + + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } + + /** + * Sets the rgb value for all LEDs. + * + * @param r Red 0-255 + * @param g Green 0-255 + * @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 == 256 && g == 256 && b == 256) { + rainbow(); + return; + } + + else if (r == 257 && g == 257 && b == 257){ + golden(); + 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() { + // 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 += 2; + // Check bounds + m_rainbowFirstPixelHue %= 180; + + m_LED.setData(m_LEDBuffer); + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } + + private void golden() { + // 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())) % 120 + 90; + // Set the value + m_LEDBuffer.setHSV(i, hue, 240, 180); + } + // Increase by to make the rainbow "move" + m_rainbowFirstPixelHue += 1; + // Check bounds + m_rainbowFirstPixelHue %= 120; + + + m_LED.setData(m_LEDBuffer); + SmartDashboard.putString("led", m_LEDBuffer.getLED(1).toString()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index dc4cd49..54ffc08 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,11 +5,13 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkBase.IdleMode; + import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.Constants.ShooterConstants; public class ShooterSubsystem extends SubsystemBase { @@ -20,6 +22,8 @@ public class ShooterSubsystem extends SubsystemBase { private double m_topSpeed = 0; private double m_bottomSpeed = 0; + private double m_simRPM = 0; + public ShooterSubsystem() { m_bottom.setIdleMode(IdleMode.kCoast); m_top.setIdleMode(IdleMode.kCoast); @@ -57,14 +61,16 @@ public void setShootingSpeed(ShootSpeed speed) { } public double returnCurrentSpeed() { - return m_bottom.getEncoder().getVelocity(); + if (m_topSpeed > 0.5 && m_simRPM < 15) m_simRPM++; + else if (m_simRPM > 0) m_simRPM--; + return Robot.isReal() ? m_bottom.getEncoder().getVelocity() : m_simRPM * 400; } @Override public void periodic() { // This method will be called once per scheduler run // SmartDashboard.putNumber("bottom Speed", m_bottomSpeed); - // SmartDashboard.putNumber("top Speed", m_topSpeed); + SmartDashboard.putNumber("top Speed", m_bottom.getEncoder().getVelocity()); m_bottom.set(m_bottomSpeed); m_top.set(m_topSpeed);