diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 8677ca5..962d355 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -268,7 +268,7 @@ public static final class RollerConfigs { public static final class LightsConstants { - public static final int LENGTH = 22; + public static final int LENGTH = 144; } public static final class PivotConstants { @@ -399,8 +399,8 @@ public static final class Configs { public static final SparkFlexConfig RIGHT_MOTOR = MOTOR_BASE.clone().setInverted(false); public static final SparkPIDControllerConfig PID = new SparkPIDControllerConfig() - .setPID(0.00075, 0.0001, 0.0003) - .setIZone(10.0); + .setPID(0.00047, 0.000002, 0.0) + .setIZone(100.0); public static final RelativeEncoderConfig ENCODER = new RelativeEncoderConfig() .setPositionConversionFactor(REL_ENC_CONVERSION) diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java index bc36be2..fb8df63 100644 --- a/src/main/java/org/team340/robot/RobotContainer.java +++ b/src/main/java/org/team340/robot/RobotContainer.java @@ -93,14 +93,13 @@ public static void init() { private static void configBindings() { // Set default commands. intake.setDefaultCommand(intake.maintainPosition()); + lights.setDefaultCommand(lights.defaultCommand(feeder::hasNote)); pivot.setDefaultCommand(pivot.maintainPosition()); shooter.setDefaultCommand(shooter.targetDistance(swerve::getSpeakerDistance, 3500.0, swerve::inOpponentWing)); swerve.setDefaultCommand(swerve.drive(RobotContainer::getDriveX, RobotContainer::getDriveY, RobotContainer::getDriveRotate, true)); - new Trigger(feeder::hasNote).whileTrue(lights.hasNote()); - Routines.onDisable().schedule(); - RobotModeTriggers.disabled().whileTrue(Routines.onDisable()); + RobotModeTriggers.disabled().onTrue(Routines.onDisable()); /** * Driver bindings. @@ -131,8 +130,7 @@ private static void configBindings() { driver.leftBumper().whileTrue(Routines.prepFeed(RobotContainer::getDriveX, RobotContainer::getDriveY)); // POV Up => Barf Forwards (Hold) - driver.povUp().whileTrue(Routines.poop(true)); - // driver.povUp().whileTrue(Routines.barfForward()); + driver.povUp().whileTrue(Routines.barfForward()); // POV Down => Pivot Down (Tap) driver.povDown().onTrue(pivot.goTo(PivotConstants.DOWN_POSITION)); diff --git a/src/main/java/org/team340/robot/commands/Routines.java b/src/main/java/org/team340/robot/commands/Routines.java index f5fc992..8acf7d9 100644 --- a/src/main/java/org/team340/robot/commands/Routines.java +++ b/src/main/java/org/team340/robot/commands/Routines.java @@ -191,10 +191,7 @@ public static Command barfBackward() { * Calls {@code onDisable()} for all subsystems. */ public static Command onDisable() { - return parallel( - sequence(waitSeconds(6.0), parallel(climber.onDisable(), feeder.onDisable(), intake.onDisable(), pivot.onDisable())), - lights.onDisable() - ) + return sequence(waitSeconds(6.0), parallel(climber.onDisable(), feeder.onDisable(), intake.onDisable(), pivot.onDisable())) .withName("Routines.onDisable()"); } } diff --git a/src/main/java/org/team340/robot/subsystems/Lights.java b/src/main/java/org/team340/robot/subsystems/Lights.java index d6f3f6f..189020c 100644 --- a/src/main/java/org/team340/robot/subsystems/Lights.java +++ b/src/main/java/org/team340/robot/subsystems/Lights.java @@ -1,13 +1,14 @@ package org.team340.robot.subsystems; -import static edu.wpi.first.wpilibj2.command.Commands.idle; -import static edu.wpi.first.wpilibj2.command.Commands.sequence; -import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; - import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; import org.team340.lib.GRRSubsystem; +import org.team340.lib.util.Alliance; +import org.team340.lib.util.Math2; import org.team340.robot.Constants.LightsConstants; import org.team340.robot.Constants.RobotMap; @@ -18,6 +19,7 @@ public class Lights extends GRRSubsystem { private final AddressableLED lights; private final AddressableLEDBuffer buffer; + private final Timer timer; /** * Create the lights subsystem. @@ -26,10 +28,12 @@ public Lights() { super("Lights"); lights = new AddressableLED(RobotMap.LIGHTS); buffer = new AddressableLEDBuffer(LightsConstants.LENGTH); + timer = new Timer(); lights.setLength(buffer.getLength()); lights.setData(buffer); lights.start(); + timer.start(); } /** @@ -46,30 +50,49 @@ private void setRGB(int r, int g, int b) { } /** - * Displays the "Has Note" signal. + * The default command for the lights. */ - public Command hasNote() { - return sequence( - runOnce(() -> setRGB(0, 255, 0)), - waitSeconds(0.4), - runOnce(() -> setRGB(0, 0, 0)), - waitSeconds(0.4), - runOnce(() -> setRGB(0, 255, 0)), - waitSeconds(0.4), - runOnce(() -> setRGB(0, 0, 0)), - waitSeconds(0.4), - runOnce(() -> setRGB(0, 255, 0)), - idle(this) - ) - .finallyDo(() -> setRGB(0, 0, 0)) - .withName("lights.hasNote()"); - } - - public Command onDisable() { + public Command defaultCommand(Supplier hasNote) { return commandBuilder() - .onExecute(() -> setRGB(255, 0, 0)) + .onExecute(() -> { + if (DriverStation.isTeleopEnabled()) { + if (hasNote.get()) { + double time = timer.get(); + for (int i = 0; i < buffer.getLength() / 2; i++) { + int v = (int) ((Math.cos((time * 20.0) - ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) * 100.0); + buffer.setRGB(i, v, v, v); + buffer.setRGB(buffer.getLength() - i - 1, v, v, v); + } + lights.setData(buffer); + } else { + int v = (int) (((Math.cos(timer.get() * 8.5) + 1.0) * 87.5) + 25.0); + if (Alliance.isBlue()) { + setRGB(0, 0, v); + } else { + setRGB(v, 0, 0); + } + } + } else if (DriverStation.isAutonomousEnabled()) { + double time = timer.get(); + for (int i = 0; i < buffer.getLength() / 2; i++) { + int v = (int) ((Math.cos((time * 30.0) - ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) * 100.0); + if (Alliance.isBlue()) { + buffer.setRGB(i, 0, 0, v); + buffer.setRGB(buffer.getLength() - i - 1, 0, 0, v); + } else { + buffer.setRGB(i, v, 0, 0); + buffer.setRGB(buffer.getLength() - i - 1, v, 0, 0); + } + } + lights.setData(buffer); + } else if (DriverStation.isDisabled()) { + if (Alliance.isBlue()) setRGB(0, 0, 150); else setRGB(150, 0, 0); + } else { + setRGB(0, 0, 0); + } + }) .onEnd(() -> setRGB(0, 0, 0)) .ignoringDisable(true) - .withName("lights.onDisable()"); + .withName("lights.defaultCommand()"); } } diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index e666663..4d3f4ba 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -40,7 +40,7 @@ import org.team340.robot.Constants.FieldPositions; import org.team340.robot.Constants.SwerveConstants; -/**\[] +/** * The swerve subsystem. */ public class Swerve extends SwerveBase {