Skip to content

Commit

Permalink
New lights
Browse files Browse the repository at this point in the history
  • Loading branch information
@bryceroethel
bryceroethel committed Apr 26, 2024
1 parent be2cbeb commit 43c1807
Show file tree
Hide file tree
Showing 5 changed files with 87 additions and 66 deletions.
1 change: 1 addition & 0 deletions .vscode/settings.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@
"GRRDashboard",
"GSON",
"Holonomic",
"Intaking",
"Integ",
"Interp",
"Interpolatable",
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/team340/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,7 @@ public static final class RollerConfigs {

public static final class LightsConstants {

public static final int LENGTH = 126;
public static final int LENGTH = 56;
}

public static final class PivotConstants {
Expand Down
9 changes: 7 additions & 2 deletions src/main/java/org/team340/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.team340.robot;

import static edu.wpi.first.wpilibj2.command.Commands.*;

import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import org.team340.lib.controller.Controller2;
Expand Down Expand Up @@ -74,7 +76,7 @@ public static void init() {
private static void configBindings() {
// Set default commands.
intake.setDefaultCommand(intake.maintainPosition());
lights.setDefaultCommand(lights.defaultCommand(intake::hasNote, feeder::hasNote));
lights.setDefaultCommand(lights.idle());
pivot.setDefaultCommand(pivot.maintainPosition());
swerve.setDefaultCommand(swerve.drive(RobotContainer::getDriveX, RobotContainer::getDriveY, RobotContainer::getDriveRotate, true));

Expand Down Expand Up @@ -121,7 +123,10 @@ private static void configBindings() {
driver.leftBumper().and(driver.rightBumper().negate()).whileTrue(pivot.driveManual(RobotContainer::getPivotManual));

// Right Bumper => Shoot with manual speed adjustment (Hold)
driver.rightBumper().and(driver.leftBumper().negate()).whileTrue(shooter.driveManual(RobotContainer::getShooterManual));
driver
.rightBumper()
.and(driver.leftBumper().negate())
.whileTrue(parallel(shooter.driveManual(RobotContainer::getShooterManual), lights.flames()));

// Start => Toggle feed through
driver.start().toggleOnTrue(Routines.feedThrough(RobotContainer::getShooterManual));
Expand Down
36 changes: 23 additions & 13 deletions src/main/java/org/team340/robot/commands/Routines.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,8 @@ private Routines() {
public static Command intake() {
return parallel(
pivot.goTo(PivotConstants.INTAKE_SAFE_POSITION_CLEARED).unless(pivot::isSafeForIntake),
sequence(waitUntil(pivot::isSafeForIntake), intake.downPosition(), race(feeder.receive(), intake.intake()), feeder.seat())
sequence(waitUntil(pivot::isSafeForIntake), intake.downPosition(), race(feeder.receive(), intake.intake()), feeder.seat()),
lights.intaking()
)
.withName("Routines.intake()");
}
Expand All @@ -38,11 +39,14 @@ public static Command finishIntake() {
* Intakes from the human player.
*/
public static Command intakeHuman() {
return sequence(
pivot.goTo(PivotConstants.INTAKE_SAFE_POSITION_CLEARED).unless(pivot::isSafeForIntake),
intake.uprightPosition(),
pivot.goTo(PivotConstants.MAX_POS),
deadline(waitUntil(feeder::hasNote).andThen(waitSeconds(0.1)), shooter.intakeHuman(), feeder.intakeHuman())
return parallel(
sequence(
pivot.goTo(PivotConstants.INTAKE_SAFE_POSITION_CLEARED).unless(pivot::isSafeForIntake),
intake.uprightPosition(),
pivot.goTo(PivotConstants.MAX_POS),
deadline(waitUntil(feeder::hasNote).andThen(waitSeconds(0.1)), shooter.intakeHuman(), feeder.intakeHuman())
),
lights.intaking()
)
.withName("Routines.intakeHuman()");
}
Expand All @@ -63,9 +67,12 @@ public static Command finishIntakeHuman() {
* Barfs the note forwards out of the intake.
*/
public static Command barfForward() {
return sequence(
parallel(pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), intake.barfPosition()).withTimeout(0.5),
parallel(pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), feeder.barfForward(), intake.barf())
return parallel(
sequence(
parallel(pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), intake.barfPosition()).withTimeout(0.5),
parallel(pivot.goTo(PivotConstants.BARF_FORWARD_POSITION), feeder.barfForward(), intake.barf())
),
lights.flames()
)
.withName("Routines.barfForward()");
}
Expand All @@ -79,7 +86,8 @@ public static Command barfBackward() {
sequence(
parallel(waitSeconds(0.35), pivot.goTo(PivotConstants.DOWN_POSITION), intake.downPosition()),
parallel(feeder.barfBackward(), intake.intake())
)
),
lights.flames()
)
.withName("Routines.barfBackward()");
}
Expand All @@ -88,14 +96,15 @@ public static Command barfBackward() {
* Prepares to poop the note forwards out of the intake.
*/
public static Command prepPoop() {
return sequence(handoff(), intake.poopPosition()).withName("Routines.prepPoop()");
return parallel(sequence(handoff(), intake.poopPosition()), lights.flames()).withName("Routines.prepPoop()");
}

/**
* Poops the note out of the intake.
*/
public static Command poop() {
return deadline(sequence(waitUntil(() -> !intake.hasNote()), waitSeconds(0.15)), intake.poop()).withName("Routines.poop()");
return deadline(sequence(waitUntil(() -> !intake.hasNote()), waitSeconds(0.15)), intake.poop(), lights.flames())
.withName("Routines.poop()");
}

/**
Expand All @@ -117,7 +126,8 @@ public static Command handoff() {
* @param rampSpeed The speed to ramp the shooter by in percent duty cycle / second.
*/
public static Command feedThrough(Supplier<Double> rampSpeed) {
return parallel(intake.intake(), feeder.shoot(true), shooter.driveManual(rampSpeed)).withName("Routines.feedThrough()");
return parallel(intake.intake(), feeder.shoot(true), shooter.driveManual(rampSpeed), lights.flames())
.withName("Routines.feedThrough()");
}

/**
Expand Down
105 changes: 55 additions & 50 deletions src/main/java/org/team340/robot/subsystems/Lights.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,7 @@
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;
Expand Down Expand Up @@ -67,64 +65,71 @@ private void setMirrored(int i, int r, int g, int b) {
buffer.setRGB(buffer.getLength() - i - 1, r, g, b);
}

/**
* The default command for the lights.
*/
public Command defaultCommand(Supplier<Boolean> intakeNote, Supplier<Boolean> feederNote) {
public Command intaking() {
return commandBuilder()
.onExecute(() -> {
double time = timer.get();
for (int i = 0; i < buffer.getLength() / 2; i++) {
double v = (Math.cos((time * 22.0) + ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) / 2.0;
setMirrored(i, (int) (v * 255.0), (int) (v * 10.0), 0);
}
apply();
})
.onEnd(() -> {
set(0, 0, 0);
apply();
})
.ignoringDisable(true)
.withName("lights.intaking()");
}

public Command flames() {
int[] state = new int[buffer.getLength()];
for (int i = 0; i < state.length; i++) {
state[i] = 0;
}

return commandBuilder()
.onExecute(() -> {
if (DriverStation.isTeleopEnabled()) {
if (feederNote.get()) {
if (intakeNote.get()) {
double time = timer.get();
for (int i = 0; i < buffer.getLength() / 2; i++) {
int v = (int) ((Math.cos((time * 60.0) + ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) * 100.0);
setMirrored(i, v, v, v);
}
} else {
double time = timer.get();
for (int i = 0; i < buffer.getLength() / 2; i++) {
double v = (Math.cos((time * 20.0) - ((i / (buffer.getLength() / 2.0)) * Math2.TWO_PI)) + 1.0) / 2.0;
setMirrored(i, (int) (v * 255.0), (int) (v * 24.0), (int) (v * 2.0));
}
}
for (int i = 0; i < buffer.getLength() / 2; i++) {
state[i] = (int) Math.max(0.0, state[i] - (Math2.random((0.5 + (i / (buffer.getLength() * 0.25))) * 5.0) + 28.0));
}
for (int i = (buffer.getLength() / 2) - 1; i >= 2; i--) {
state[i] = (state[i - 1] + state[i - 2] + state[i - 2]) / 3;
}
if (Math.random() < 0.5) {
int i = (int) Math2.random(5.0);
state[i] = (int) (state[i] + Math2.random(160.0, 255.0));
}
for (int i = 0; i < buffer.getLength() / 2; i++) {
int heat = (int) ((state[i] / 255.0) * 191.0);
int ramp = (heat & 63) << 2;
if (heat > 180) {
setMirrored(i, 255, 255, ramp);
} else if (heat > 60) {
setMirrored(i, 255, ramp, 0);
} else {
int v = (int) (((Math.cos(timer.get() * 8.5) + 1.0) * 87.5) + 25.0);
if (Alliance.isBlue()) {
set(0, 0, v);
} else {
set(v, 0, 0);
}
}
} else if (DriverStation.isAutonomousEnabled()) {
for (int i = 0; i < buffer.getLength() / 2; i++) {
state[i] = (int) Math.max(0.0, state[i] - Math2.random((0.5 + (i / (buffer.getLength() * 0.25))) * 70.0) + 4.0);
}
for (int i = (buffer.getLength() / 2) - 1; i >= 2; i--) {
state[i] = (state[i - 1] + state[i - 2] + state[i - 2]) / 3;
}
if (Math.random() < 0.5) {
int i = (int) Math2.random(8.0);
state[i] = (int) (state[i] + Math2.random(160.0, 255.0));
}
for (int i = 0; i < buffer.getLength() / 2; i++) {
int heat = (int) ((state[i] / 255.0) * 191.0);
int ramp = (heat & 63) << 2;
if (heat > 180) {
setMirrored(i, 255, 255, ramp);
} else if (heat > 60) {
setMirrored(i, 255, ramp, 0);
} else {
setMirrored(i, ramp, 0, 0);
}
setMirrored(i, ramp, 0, 0);
}
}
apply();
})
.onEnd(() -> {
set(0, 0, 0);
apply();
})
.ignoringDisable(true)
.withName("lights.flames()");
}

public Command idle() {
return commandBuilder()
.onExecute(() -> {
if (DriverStation.isTeleopEnabled()) {
double v = (((Math.cos(timer.get() * 8.5) + 1.0) / 4.0) + 0.5);
set((int) (v * 255.0), (int) (v * 10.0), 0);
} else if (DriverStation.isDisabled()) {
if (Alliance.isBlue()) set(0, 0, 150); else set(150, 0, 0);
set(255, 10, 0);
} else {
set(0, 0, 0);
}
Expand Down

0 comments on commit 43c1807

Please sign in to comment.