diff --git a/src/main/java/frc/robot/subsystems/.keep b/src/main/java/frc/robot/subsystems/.keep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..414f7c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.motorcontrol.VictorSP; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + + VictorSP intakeMotor1 = new VictorSP(1); + VictorSP intakeMotor2 = new VictorSP(2); + + public DigitalInput touchSensor = new DigitalInput(1); + + public Intake() { + intakeMotor2.setInverted(true); + intakeMotor1.addFollower(intakeMotor2); + } + + public void setPower(double power) { + intakeMotor1.set(power); + } + + public void intake() { + setPower(-0.7); + } + + public void outtake() { + setPower(0.7); + } + + public void stop() { + setPower(0); + } + + public Command intakeCommand() { + return Commands.runEnd(this::intake, this::stop, this); // .until(() -> beambrake.get()); + } + +} diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java new file mode 100644 index 0000000..35e5db8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LEDs extends SubsystemBase { + + AddressableLED leds; + AddressableLEDBuffer buffer; + + public LEDs(int portNum, int length) { + this.leds = new AddressableLED(portNum); + this.buffer = new AddressableLEDBuffer(length); + leds.setLength(buffer.getLength()); + leds.setData(buffer); + leds.start(); + } + + public void setColor(Color color) { + for (var i = 0; i < buffer.getLength(); i++) { + buffer.setLED(i, color); + } + leds.setData(buffer); + } + + public Command setAllianceColor() { + return Commands.run(() -> { + Color color = Color.kYellow; + if (DriverStation.getAlliance().isPresent()) { + if (DriverStation.getAlliance().get() == Alliance.Red) { + color = Color.kRed; + } else { + color = Color.kBlue; + } + } + setColor(color); + }, this); + } +} diff --git a/src/main/java/frc/robot/subsystems/LEDs.java.txt b/src/main/java/frc/robot/subsystems/LEDs.java.txt new file mode 100644 index 0000000..cd47c48 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDs.java.txt @@ -0,0 +1,126 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * This is the class header for the LEDs Subsystem + */ +public class LEDs extends SubsystemBase { + private AddressableLEDBuffer controLedBuffer; + private AddressableLED addressableLED; + + /** + * constructs a LED Subsystem + * + * @param length length of the addressable LEDS + * @param port port ID for PWM + */ + public LEDs(int length, int port) { + controLedBuffer = new AddressableLEDBuffer(length); + addressableLED = new AddressableLED(port); + + addressableLED.setLength(controLedBuffer.getLength()); + addressableLED.setData(controLedBuffer); + addressableLED.start(); + } + + /** + * Get LED strip length + * + * @return number of LEDs + */ + public int getLength() { + return controLedBuffer.getLength(); + } + + /** + * Set individual LED color via HSV + * + * @param index the LED index to set + * @param h the h value [0-180) + * @param s the s value [0-255] + * @param v the v value [0-255] + */ + public void setHSV(int index, int h, int s, int v) { + controLedBuffer.setHSV(index, h, s, v); + } + + /** + * Sets the LED output data. + */ + public void setData() { + addressableLED.setData(controLedBuffer); + } + + /** + * Set individual LED color via RGB + * + * @param index the LED index to set + * @param r the r value [0-255] + * @param g the g value [0-255] + * @param b the b value [0-255] + */ + public void setRGB(int index, int r, int g, int b) { + controLedBuffer.setRGB(index, r, g, b); + } + + /** + * Sets RGB Color of the entire LED strip + * + * @param r - [0 - 255] + * @param g - [0 - 255] + * @param b - [0 - 255] + */ + public void setRGB(int r, int g, int b) { + for (var i = 0; i < getLength(); i++) { + setRGB(i, r, g, b); + } + setData(); + } + + /** + * Set individual LED color via Color + * + * @param index the LED index to set + * @param color The color of the LED + */ + public void setColor(int index, Color color) { + controLedBuffer.setLED(index, color); + } + + /** + * Sets the Color of the entire LED strip + * + * @param color color set for the LEDs + */ + public void setColor(Color color) { + for (var i = 0; i < getLength(); i++) { + setColor(i, color); + } + setData(); + } + + public Color getColor(int index) { + return controLedBuffer.getLED(index); + } + + public Command setDefaultColor() { + Supplier colorSupplier = () -> { + Color color = Color.kWhite; + if (DriverStation.getAlliance().isPresent()) { + color = + DriverStation.getAlliance().get() == Alliance.Red ? Color.kRed : Color.kBlue; + } + return color; + }; + return Commands.run(() -> setColor(colorSupplier.get()), this).ignoringDisable(true); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java deleted file mode 100644 index 88e6569..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.subsystems.drive; - -import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * Drivetrain subsystem. - */ - -public class Drivetrain extends SubsystemBase { - private DrivetrainIO io; - private DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); - - /** - * Create Wrist Intake Subsystem - */ - public Drivetrain(DrivetrainIO io) { - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Drivetrain", inputs); - } -} - diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java deleted file mode 100644 index 842e1aa..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.subsystems.drive; - -import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * DrivetrainIO interface - */ -public interface DrivetrainIO { - /** - * Drivetrain IO - */ - @AutoLog - public static class DrivetrainIOInputs { - public Rotation2d gyroYaw = new Rotation2d(); - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(DrivetrainIOInputs inputs) {} - - /** Run the motor at the specified voltage. */ - public default void setDriveVoltage(double lvolts, double rvolts) {} - - -} diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java deleted file mode 100644 index f8f17d1..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * DrivetrainReal - */ -public class DrivetrainReal implements DrivetrainIO { - - - /** - * Drivetrain Real - */ - public DrivetrainReal() {} - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - inputs.gyroYaw = Rotation2d.fromDegrees(0); - } - - /** - * Drive Voltage - */ - public void setDriveVoltage(double lvolts, double rvolts) {} - -}