diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index edea6066..0d76b80a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -42,7 +42,6 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.subsystems.vision.NoteVision; -import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.robot.util.PathUtil.AutonConfig; import edu.wpi.first.math.geometry.Rotation2d; @@ -121,8 +120,8 @@ private void configureDriverBindings() { // intaking driver.getRightTriggerButton() .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE)) - .andThen(new BuzzController(driver) + .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) + .andThen(new BuzzController(driver) .alongWith(new LEDSet(LEDInstructions.PICKUP) .withTimeout(3.0)))); @@ -135,7 +134,7 @@ private void configureDriverBindings() { driver.getRightBumper() .onTrue(new ShooterPodiumShot()) .whileTrue(new SwerveDriveToShoot() - .deadlineWith(new LEDSet(LEDInstructions.ASSIST_FLASH)) + .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)) .andThen(new ShooterWaitForTarget() .withTimeout(1.5)) .andThen(new ConveyorShoot())) @@ -160,7 +159,8 @@ private void configureDriverBindings() { driver.getLeftMenuButton() .whileTrue(ConveyorToAmp.withCheckLift() .andThen(AmperToHeight.untilDone(Lift.AMP_SCORE_HEIGHT)) - .andThen(new AmperScore())) + .andThen(new AmperScore() + .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE)))) .onFalse(new AmperStop()) .onFalse(new AmperToHeight(Lift.MIN_HEIGHT)); @@ -169,51 +169,63 @@ private void configureDriverBindings() { .onTrue(new AmperScoreTrap()) .onFalse(new AmperStop()); + // lift to trap driver.getDPadRight() .onTrue(new ConditionalCommand(new ConveyorToAmp(), new DoNothingCommand(), () -> Intake.getInstance().hasNote()) .andThen(new AmperToHeight(Lift.TRAP_SCORE_HEIGHT))); + // reset heading driver.getDPadUp() .onTrue(new SwerveDriveResetHeading()); + // trap outtake driver.getDPadDown() .onTrue(new AmperScoreSpeed(-Score.TRAP_SPEED)) .onFalse(new AmperStop()); + // automatic swerve drive driver.getTopButton() // on command start .onTrue(new BuzzController(driver, Assist.BUZZ_INTENSITY) - .deadlineWith(new LEDSet(LEDInstructions.GREEN))) + .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))) .onTrue(new SwerveDriveAutomatic(driver) // after command end .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY) - .deadlineWith(new LEDSet(LEDInstructions.GREEN))) + .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))) .andThen(new WaitCommand(Driver.Drive.BUZZ_DURATION)) .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY) - .deadlineWith(new LEDSet(LEDInstructions.GREEN)))); + .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))); + // climb driver.getRightButton() - .whileTrue(new SwerveDriveToClimb()); + .whileTrue(new SwerveDriveToClimb() + .deadlineWith(new LEDSet(LEDInstructions.TRAP_ALIGN))); + // drive to chain driver.getBottomButton() - .whileTrue(new SwerveDriveDriveToChain()); + .whileTrue(new SwerveDriveDriveToChain() + .deadlineWith(new LEDSet(LEDInstructions.TRAP_ALIGN))); } private void configureOperatorBindings() { + // climber new Trigger(() -> operator.getLeftStick().magnitude() > Settings.Operator.DEADBAND.get()) .whileTrue(new ClimberDrive(operator)); + // climber LED new Trigger(() -> operator.getLeftY() > Settings.Operator.DEADBAND.get()) .onTrue(new LEDSet(LEDInstructions.CLIMB_UP) .withTimeout(1.0)); + // move note to amp new Trigger(() -> operator.getLeftY() > 0.25) .onTrue(new ConveyorToAmp() .andThen(new ShooterStop())); + new Trigger(() -> operator.getRightStick().magnitude() > Settings.Operator.DEADBAND.get()) .whileTrue(new AmperLiftDrive(operator)); @@ -222,7 +234,7 @@ private void configureOperatorBindings() { .onFalse(new IntakeStop()); operator.getRightTriggerButton() .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE)) + .deadlineWith(new LEDSet(LEDInstructions.INTAKE)) .andThen(new BuzzController(driver) .alongWith(new LEDSet(LEDInstructions.PICKUP) .withTimeout(3.0)))); @@ -262,7 +274,7 @@ private void configureOperatorBindings() { // human player attention button operator.getRightMenuButton() - .whileTrue(new LEDSet(LEDInstructions.PULSE_PURPLE)); + .whileTrue(new LEDSet(LEDInstructions.ATTENTION)); operator.getLeftMenuButton() .onTrue(new AmperToConveyor()) diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 8b7acdce..50d66b63 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -60,7 +60,7 @@ public AmpScoreRoutine() { new ConveyorToAmp(), new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get())) .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE) - .deadlineWith(new LEDSet(LEDInstructions.GREEN)) + .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN)) ), new ParallelCommandGroup( @@ -72,7 +72,7 @@ public AmpScoreRoutine() { AMP_WALL_SCORE_Y_TOLERANCE, AMP_WALL_SCORE_ANGLE_TOLERANCE) .withTimeout(SCORE_ALIGN_TIMEOUT) - .deadlineWith(new LEDSet(LEDInstructions.GREEN)) + .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE)) ), AmperScore.untilDone(), diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index d197144e..d89a4d70 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj2.command.Command; @@ -27,7 +28,6 @@ public class LEDDefaultMode extends Command { private final LEDController leds; - private final Intake intake; private final Amper amper; @@ -44,9 +44,10 @@ private LEDInstruction getInstruction() { && amper.isAtTargetHeight(0.15)) return LEDInstructions.GREEN; - if (intake.hasNote()) - return LEDInstructions.RED; - + if (intake.hasNote()) { + return LEDInstructions.CONTAINS_NOTE; + } + return LEDInstructions.DEFAULT; } diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index a31c70b9..9b473ef0 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -46,9 +46,9 @@ public interface LEDInstructions { LEDInstruction WHITE = new LEDSingleColor(new SLColor(255, 255, 255)); LEDInstruction YELLOW = new LEDSingleColor(new SLColor(255, 255, 0)); - LEDInstruction BANGLADESH = new LEDSection(new SLColor[] {SLColor.RED, SLColor.BLACK, SLColor.DARK_GREEN}); + LEDInstruction BANGLADESH = new LEDSection(new SLColor[] {SLColor.RED, SLColor.BLACK, SLColor.DARK_GREEN}, false); - LEDInstruction CLIMB_UP = new LEDSection(new SLColor[] {SLColor.PURPLE, SLColor.GREEN}); + /******************/ /*** NON-STATIC ***/ @@ -58,10 +58,9 @@ public interface LEDInstructions { LEDInstruction PULSE_RED = new LEDPulseColor(SLColor.RED); LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(SLColor.RED, SLColor.BLUE); - LEDInstruction PULSE_PURPLE = new LEDPulseColor(SLColor.PURPLE, .25); LEDInstruction RICHIE = new RichieMode(SLColor.RED); - LEDInstruction PICKUP = new LEDPulseColor(SLColor.WHITE, 0.5); + /********************************************/ /*** LED CONSTANTS TO BE USED IN COMMANDS ***/ @@ -69,7 +68,27 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction ASSIST_FLASH = LEDInstructions.GREEN; + LEDInstruction INTAKE = new LEDPulseColor(SLColor.RED); + + LEDInstruction PICKUP = new LEDPulseColor(SLColor.RED); + + LEDInstruction SPEAKER_ALIGN = new LEDPulseColor(SLColor.GREEN); + + LEDInstruction AMP_ALIGN = new LEDPulseColor(SLColor.GREEN); + + LEDInstruction AMP_SCORE = LEDInstructions.PURPLE; + + LEDInstruction AUTO_SWERVE = new LEDPulseColor(SLColor.GREEN); + + LEDInstruction ATTENTION = new LEDPulseColor(SLColor.YELLOW); + + LEDInstruction CLIMB_UP = new LEDSection(new SLColor[] {SLColor.PURPLE, SLColor.GREEN}, false); + + LEDInstruction TRAP_ALIGN = new LEDSection(new SLColor[] {SLColor.PURPLE, SLColor.GREEN}, true); + + LEDInstruction FERRY = new LEDPulseColor(SLColor.ORANGE); // No Ferry command yet + + LEDInstruction CONTAINS_NOTE = new LEDSingleColor(SLColor.RED); // TO FUTURE USERS, DONT PUT LEDAlign and LEDAutonChooser (any disabled LEDInstructions) inside // the LEDInstructions interface diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a6667b67..b4f02335 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -154,7 +154,7 @@ public interface Swerve { public interface Assist { SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change - double BUZZ_INTENSITY = 0.8; + double BUZZ_INTENSITY = 1; SmartNumber kP = new SmartNumber("SwerveAssist/kP", 2.0); SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); @@ -424,7 +424,7 @@ public interface Rotation { public interface LED { int LED_LENGTH = 15; - SmartNumber BLINK_TIME = new SmartNumber("LED/LED Blink Time", .5); + SmartNumber BLINK_TIME = new SmartNumber("LED/LED Blink Time", .15); SmartNumber TRANSLATION_SPREAD = new SmartNumber("LED/LED Translation Spread (m)", 0.5); SmartNumber ROTATION_SPREAD = new SmartNumber("LED/LED Rotation Spread (deg)", 15); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java index 2112f061..9db6915b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java @@ -88,7 +88,7 @@ public static AutonLEDColors fromName(String name) { } public LEDAutonChooser() { - super(AutonLEDColors.fromName(RobotContainer.getAutonomousCommandNameStatic()).ledColors); + super(AutonLEDColors.fromName(RobotContainer.getAutonomousCommandNameStatic()).ledColors, true); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java index 98b4689c..6f52a0d1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java @@ -6,44 +6,88 @@ package com.stuypulse.robot.subsystems.leds.instructions; +import java.util.Arrays; + import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SLColor; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.AddressableLEDBuffer; public class LEDSection implements LEDInstruction { - public final SLColor[] sections; - public final int[] separatorIndexes; + private final SLColor[] sections; + private final SLColor[] altSections; + private final boolean isBlinking; + private final int[] separatorIndexes; + private final StopWatch stopWatch; - public LEDSection(SLColor[] sections, int[] separatorIndexes) { - if (sections.length - 1 != separatorIndexes.length) { - throw new IllegalArgumentException("Mismatching section and seperating indexes"); + public LEDSection(SLColor[] sections, SLColor[] altSections, int[] separatorIndexes, boolean isBlinking) { + if (sections.length != separatorIndexes.length) { + throw new IllegalArgumentException("Mismatching section and seperating indexes: \n Section Length (" + sections.length +") \n SeparatorIndexes ("+ separatorIndexes.length +")" ); } this.sections = sections; + this.altSections = altSections; this.separatorIndexes = separatorIndexes; + this.isBlinking = isBlinking; + this.stopWatch = new StopWatch(); } - public LEDSection(SLColor[] sections) { - this.sections = sections; + public LEDSection(SLColor[] sections, SLColor[] altSections){ + this(sections, altSections, genSeparators(sections), true); + } + public LEDSection(SLColor[] sections, int[] separatorIndexes) { + this(sections, genEmptyArray(sections.length), separatorIndexes, false); + } + + public LEDSection(SLColor[] sections, boolean isBlinking) { + this(sections, genEmptyArray(sections.length), genSeparators(sections), isBlinking); + } + + @Override + public void setLED(AddressableLEDBuffer ledBuffer) { + if (isBlinking){ + double time = stopWatch.getTime(); + double pulseTime = Settings.LED.BLINK_TIME.get(); + + if (time < pulseTime) { + setColorSections(ledBuffer, this.sections, this.separatorIndexes); + } else if (time < pulseTime * 2) { + setColorSections(ledBuffer, this.altSections, this.separatorIndexes); + } else { + stopWatch.reset(); + } + } else { + setColorSections(ledBuffer, this.sections, this.separatorIndexes); + } + } + + private static SLColor[] genEmptyArray(int length) { + SLColor[] array = new SLColor[length]; + Arrays.fill(array, SLColor.BLACK); + return array; + } + + private static int[] genSeparators(SLColor[] sections){ int totalLEDLength = Settings.LED.LED_LENGTH; int sectionLength = totalLEDLength / sections.length; int extraLEDS = totalLEDLength % sections.length; - this.separatorIndexes = new int[sections.length]; + int[] separators = new int[sections.length]; // extra LEDs get distributed to the first few sections for (int i = 0; i < sections.length; i++) { int offset = Math.min(extraLEDS, i); int extraLEDForCurrentSection = (i < extraLEDS ? 1 : 0); - this.separatorIndexes[i] = (i + 1) * sectionLength + offset + extraLEDForCurrentSection - 1; + separators[i] = (i + 1) * sectionLength + offset + extraLEDForCurrentSection - 1; } + + return separators; } - @Override - public void setLED(AddressableLEDBuffer ledBuffer) { + private void setColorSections(AddressableLEDBuffer ledBuffer, SLColor[] sections, int[] separatorIndexes){ int sectionIndex = 0; for (int ledIndex = 0; ledIndex < ledBuffer.getLength(); ledIndex++) {