LED Subsystem with Trapezoid and Physics Object

LEDSubsystem.java

package frc.robot;

import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.InchesPerSecond;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import edu.wpi.first.wpilibj.LEDPattern;
import edu.wpi.first.wpilibj.LEDPattern.GradientType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.LEDConstants;

class Trapezoid {
    private int a, b, c;
    public Trapezoid(int a, int b, int c) {
        this.a = a;
        this.b = b;
        this.c = c;
    }

    public double getValue(double x, double offset) {
        double newX = MathUtil.inputModulus(x - offset, 0.0, LEDConstants.numLeds);
        return getValue(newX);
    }
    public double getValue(double x) {
        if (0.0 < x && x < a) {
            return x/a;
        } else if (a <= x && x <= (a+b)) {
            return 1.0;
        } else if ((a+b) < x && x < (a+b+c)) {
            return (-1.0/c)*(x - a - b - c);
        }
        return 0;
    }
}

class PhysicsObject {
    private static final double DT = 0.020;
    private static final double ACCEL = -20.0;
    private static final double BOUNCE = 0.5;

    private double position, velocity, ip, iv;
    private double n = 0;
    private double acceleration = ACCEL;

    public PhysicsObject(double initialPosition, double initialVelocity) {
        position = initialPosition;
        velocity = initialVelocity;
        ip = initialPosition;
        iv = initialVelocity;
    }

    public double getPosition() {
        return position;
    }

    public void reset() {
        n = 0;
        position = ip;
        velocity = iv;
        acceleration = ACCEL;
    }

    public void update() {
        if (n == 250)
            acceleration = 10.0;
        if (n >= 500)
            reset();

        if ((position + velocity * DT) < 0.0)
            velocity *= -BOUNCE;

        position += velocity * DT;
        velocity += acceleration * DT;

        n++;
    }
}

public class LEDSubsystem extends SubsystemBase {
    private final AddressableLED led = new AddressableLED(LEDConstants.port);
    private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(LEDConstants.numLeds);
    private final PhysicsObject object = new PhysicsObject(10.0, -5.0);

    private final Trapezoid trapezoid = new Trapezoid(1, 3, 1);
    private final LEDPattern myPattern = (reader, writer) -> {
        for (int i = 0; i < LEDConstants.numLeds; i++) {
            Color c = Color.lerpRGB(Color.kGreen, Color.kWhite, trapezoid.getValue(i, object.getPosition() - 0.5));
            writer.setLED(i, Color.lerpRGB(Color.kBlack, c, 0.1));
        }

        object.update();
    };


    public LEDSubsystem() {
        led.setLength(LEDConstants.numLeds);
        led.setData(ledBuffer);
        led.start();
    }

    @Override
    public void periodic() {
        myPattern.applyTo(ledBuffer);
        led.setData(ledBuffer);
    }


    // public void setColor(Color color) {
    //     LEDPattern.solid(color).applyTo(ledBuffer);
    //     led.setData(ledBuffer);
    // }
    // public void doThing() {
    //     LEDPattern.gradient(GradientType.kContinuous, Color.kGreen, Color.kPurple)
    //         .applyTo(ledBuffer);
    // }
    // public Command runRainbow() {
    //     LEDPattern rainbow = LEDPattern.rainbow(255, 100)
    //         .scrollAtAbsoluteSpeed(InchesPerSecond.of(10.0), Inches.of(1.0));
        
    //     return run(() -> rainbow.applyTo(ledBuffer));
    // }
}