Skip to content

Commit

Permalink
Schemes and machinations
Browse files Browse the repository at this point in the history 
Co-Authored-By: Jack Jewell <[email protected]>
  • Loading branch information
@bryceroethel @JackJ09
bryceroethel and JackJ09 committed Mar 10, 2024
1 parent b439e27 commit 124b842
Show file tree
Hide file tree
Showing 5 changed files with 56 additions and 38 deletions.
6 changes: 3 additions & 3 deletions src/main/java/org/team340/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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)
Expand Down
8 changes: 3 additions & 5 deletions src/main/java/org/team340/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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));
Expand Down
5 changes: 1 addition & 4 deletions src/main/java/org/team340/robot/commands/Routines.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()");
}
}
73 changes: 48 additions & 25 deletions src/main/java/org/team340/robot/subsystems/Lights.java
View file
Original file line number Diff line number Diff line change
@@ -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;

Expand All @@ -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.
Expand All @@ -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();
}

/**
Expand All @@ -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<Boolean> 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()");
}
}
2 changes: 1 addition & 1 deletion src/main/java/org/team340/robot/subsystems/Swerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
import org.team340.robot.Constants.FieldPositions;
import org.team340.robot.Constants.SwerveConstants;

/**\[]
/**
* The swerve subsystem.
*/
public class Swerve extends SwerveBase {
Expand Down

0 comments on commit 124b842

Please sign in to comment.