Skip to content

Commit

Permalink
feat: add triaging
Browse files Browse the repository at this point in the history
  • Loading branch information
rutmanz committed Feb 6, 2024
1 parent 2fba8a2 commit 102fd7b
Show file tree
Hide file tree
Showing 12 changed files with 182 additions and 86 deletions.
86 changes: 48 additions & 38 deletions src/main/java/org/team1540/robot2024/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,21 +3,21 @@
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
import org.team1540.robot2024.Constants.Elevator.ElevatorState;
import org.team1540.robot2024.commands.FeedForwardCharacterization;
import org.team1540.robot2024.commands.SwerveDriveCommand;
import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
import org.team1540.robot2024.commands.indexer.IntakeCommand;
import org.team1540.robot2024.commands.shooter.ShootSequence;
import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
import org.team1540.robot2024.subsystems.drive.*;
import org.team1540.robot2024.subsystems.elevator.Elevator;
import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
Expand All @@ -28,6 +28,7 @@
import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
import org.team1540.robot2024.subsystems.led.Leds;
import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
import org.team1540.robot2024.subsystems.shooter.*;
import org.team1540.robot2024.subsystems.tramp.Tramp;
import org.team1540.robot2024.subsystems.tramp.TrampIO;
Expand Down Expand Up @@ -64,7 +65,7 @@ public class RobotContainer {
public final CommandXboxController copilot = new CommandXboxController(1);

// Dashboard inputs
// public final LoggedDashboardChooser<Command> autoChooser;
public final LoggedDashboardChooser<Command> autoChooser;

public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);

Expand All @@ -75,29 +76,29 @@ public class RobotContainer {
*/
public RobotContainer() {
switch (Constants.currentMode) {
// case REAL:
// // Real robot, instantiate hardware IO implementations
// drivetrain =
// new Drivetrain(
// new GyroIOPigeon2(odometrySignalRefresher),
// new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
// new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
// new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
// new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
// tramp = new Tramp(new TrampIOSparkMax());
// shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
// elevator = new Elevator(new ElevatorIOTalonFX());
// indexer =
// new Indexer(
// new IndexerIOSparkMax()
// );
// aprilTagVision = new AprilTagVision(
// new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
// new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
// drivetrain::addVisionMeasurement,
// () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
// new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
// break;
case REAL:
// Real robot, instantiate hardware IO implementations
drivetrain =
new Drivetrain(
new GyroIOPigeon2(odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
tramp = new Tramp(new TrampIOSparkMax());
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
elevator = new Elevator(new ElevatorIOTalonFX());
indexer =
new Indexer(
new IndexerIOSparkMax()
);
aprilTagVision = new AprilTagVision(
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
drivetrain::addVisionMeasurement,
() -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
break;
case SIM:
// Sim robot, instantiate physics sim IO implementations
drivetrain =
Expand Down Expand Up @@ -150,22 +151,31 @@ public RobotContainer() {


// Set up auto routines
// autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
//
// // Set up FF characterization routines
// autoChooser.addOption(
// "Drive FF Characterization",
// new FeedForwardCharacterization(
// drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
// autoChooser.addOption(
// "Flywheels FF Characterization",
// new FeedForwardCharacterization(
// shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());

// Set up FF characterization routines
autoChooser.addOption(
"Drive FF Characterization",
new FeedForwardCharacterization(
drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
autoChooser.addOption(
"Flywheels FF Characterization",
new FeedForwardCharacterization(
shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));

// Configure the button bindings
// configureButtonBindings();
configureButtonBindings();
configureLedBindings();
}

private void configureLedBindings() {
leds.setFatalPattern(new LedPatternFlame());
new Trigger(DriverStation::isDSAttached)
.onTrue(Commands.runOnce(leds::clearFatalPattern)
.ignoringDisable(true))
.onFalse(Commands.runOnce(() -> leds.setFatalPattern(new LedPatternFlame()))
.ignoringDisable(true));
}
/**
* Use this method to define your button->command mappings. Buttons can be created by
* instantiating a {@link GenericHID} or one of its subclasses ({@link
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
package org.team1540.robot2024.subsystems.led;

import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow;


public class LedTriager {
private final LedPattern[] patterns = new LedPattern[Leds.CRITICALITY_COUNT];
private final LedPattern defaultPattern = new LedPatternRainbow(1);
private boolean isNew = true;
public LedPattern getPattern() {
for (int i = patterns.length -1; i >= 0; i--) {
if (patterns[i] != null) {
return patterns[i];
}
}
return defaultPattern;
}

public boolean shouldRefresh() {
final boolean val = isNew || getPattern().isDynamic();
isNew = false;
return val;
}

public void clearPattern(Leds.PatternCriticality criticality) {
patterns[criticality.ordinal()] = null;
isNew = true;
}

public boolean addPattern(LedPattern pattern, Leds.PatternCriticality criticality) {
patterns[criticality.ordinal()] = pattern;
isNew = true;
return getPattern() == pattern;
}

}
47 changes: 40 additions & 7 deletions src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,16 @@
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import org.team1540.robot2024.Constants;
import org.team1540.robot2024.subsystems.led.patterns.LedPattern;


import static org.team1540.robot2024.Constants.LED_STRIP_PORT_PWM;

public class Leds extends SubsystemBase {

private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.LED_STRIP_LENGTH);
private final AddressableLED strip = new AddressableLED(LED_STRIP_PORT_PWM);
private final ZonedAddressableLEDBuffer[] buffers = new ZonedAddressableLEDBuffer[ZONE_COUNT];
private final LedPattern[] patterns = new LedPattern[ZONE_COUNT];
private final LedTriager[] patterns = new LedTriager[ZONE_COUNT];

public Leds() {
strip.setLength(ledBuffer.getLength());
Expand All @@ -22,29 +22,62 @@ public Leds() {

buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 40, false);
buffers[Zone.ZONE2.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 40, 80, false);
for (int i = 0; i < ZONE_COUNT;i++) {
patterns[i] = new LedTriager();
}
}

@Override
public void periodic() {
for (int i = 0; i < ZONE_COUNT;i++) {
if (patterns[i].isDynamic()) {
patterns[i].apply(buffers[i]);
if (patterns[i].shouldRefresh()) {
patterns[i].getPattern().apply(buffers[i]);
}
}
strip.setData(ledBuffer);
}

public void setPattern(Zone zone, LedPattern pattern, PatternCriticality criticality) {
patterns[zone.ordinal()].addPattern(pattern, criticality);
pattern.setLength(buffers[zone.ordinal()].getLength());
}

public void setPattern(Zone zone, LedPattern pattern) {
patterns[zone.ordinal()] = pattern;
if (!pattern.isDynamic()) {
pattern.apply(buffers[zone.ordinal()]);
setPattern(zone, pattern, PatternCriticality.INFO);
}

public void clearPattern(Zone zone, PatternCriticality criticality) {
patterns[zone.ordinal()].clearPattern(criticality);
}

public void setFatalPattern(LedPattern pattern) {
System.out.println("Setting criticality");
for (int i = 0; i<ZONE_COUNT;i++) {
patterns[i].addPattern(pattern, PatternCriticality.FATAL);
pattern.setLength(buffers[i].getLength());
}
}

public void clearFatalPattern() {
System.out.println("Clearing criticality");
for (int i = 0; i<ZONE_COUNT;i++) {
patterns[i].clearPattern(PatternCriticality.FATAL);
}

}


private static final int ZONE_COUNT=Zone.values().length;
public enum Zone {
ZONE1,
ZONE2;
}
static final int CRITICALITY_COUNT=PatternCriticality.values().length;
public enum PatternCriticality {
DECORATION,
INFO,
CRITICAL,
FATAL
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,10 @@
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;

class ZonedAddressableLEDBuffer {
public final boolean isInverted;
public final int start;
public final int length;
public class ZonedAddressableLEDBuffer {
private final boolean isInverted;
private final int start;
private final int length;
private final AddressableLEDBuffer buffer;

public ZonedAddressableLEDBuffer(AddressableLEDBuffer buffer, int start, int end, boolean isInverted) {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.team1540.robot2024.subsystems.led;
package org.team1540.robot2024.subsystems.led.patterns;

import edu.wpi.first.wpilibj.util.Color;
import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;

public abstract class LedPattern {
private final boolean isDynamic;
Expand All @@ -9,11 +10,12 @@ protected LedPattern(boolean isDynamic) {
this.isDynamic = isDynamic;
}

final boolean isDynamic() {
public final boolean isDynamic() {
return isDynamic;
}

abstract void apply(ZonedAddressableLEDBuffer buffer);
public abstract void apply(ZonedAddressableLEDBuffer buffer);
public void setLength(int length) {}

protected static int getHue(Color color) {
final int red = (int) color.red * 255;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.team1540.robot2024.subsystems.led;
package org.team1540.robot2024.subsystems.led.patterns;

import edu.wpi.first.wpilibj.util.Color;
import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;

public class LedPatternBreathing extends LedPattern {
private final int speed;
Expand All @@ -15,7 +16,7 @@ public LedPatternBreathing(int speed, Color color) {
}

@Override
void apply(ZonedAddressableLEDBuffer buffer) {
public void apply(ZonedAddressableLEDBuffer buffer) {
if (saturation > 255) {
saturation = 255;
isReversed = !isReversed;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,16 +1,19 @@
package org.team1540.robot2024.subsystems.led;
package org.team1540.robot2024.subsystems.led.patterns;


import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;

import java.awt.*;
import java.util.Random;

public class LedPatternFlame extends LedPattern {
private static final Random generator = new Random();
private static final boolean reverseDirection = true;

private final int cooling;
private final int sparking = 123;
private final boolean reverseDirection = true;
private final int[] temperatures = new int[40];

private int[] temperatures;


public LedPatternFlame(int cooling) {
Expand All @@ -22,9 +25,13 @@ public LedPatternFlame() {
this(62);
}

@Override
public void setLength(int length) {
this.temperatures = new int[length];
}

@Override
void apply(ZonedAddressableLEDBuffer buffer) {
public void apply(ZonedAddressableLEDBuffer buffer) {

for (int i = 0; i < buffer.getLength(); i++) {
temperatures[i] = bit8Subtraction(temperatures[i], getRandomInt(0, ((cooling * 10) / buffer.getLength()) + 2));
Expand Down
Loading

0 comments on commit 102fd7b

Please sign in to comment.