public static final TrapezoidProfile.Constraints ARM_CONSTRAINTS =
    new TrapezoidProfile.Constraints(
        90.0,   // maxVelocity: degrees per second
        180.0  // maxAcceleration: degrees per second squared
    );
// At these limits: from 0° to 90° takes about 1.2 seconds.
// If the arm slams into stops or trips current limits during profiling,
// reduce these values — the mechanism is telling you its actual limits.

// The arm is currently at 0 degrees, not moving
TrapezoidProfile.State currentState = new TrapezoidProfile.State(0, 0);

// We want to reach 90 degrees with zero final velocity (stopped at target)
TrapezoidProfile.State goalState = new TrapezoidProfile.State(90, 0);

// A non-zero final velocity is valid if this motion feeds into another profile.
// For scoring positions, always target zero — you want the arm still, not coasting.

private TrapezoidProfile m_profile = new TrapezoidProfile(ArmConstants.ARM_CONSTRAINTS);
private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();

@Override
public void periodic() {
    // dt = 0.02s for the standard 50Hz WPILib loop
    m_setpoint = m_profile.calculate(0.02, m_setpoint, m_goal);

    // Use the setpoint's velocity for feedforward, position for PID
    double feedforward = m_feedforward.calculate(m_setpoint.velocity);
    double pidOutput   = m_pid.calculate(getAngleDegrees(), m_setpoint.position);
    m_motor.setVoltage(feedforward + pidOutput);
}

public void setGoal(double degrees) {
    m_goal = new TrapezoidProfile.State(degrees, 0);
    // Do NOT reset m_setpoint here — we want the profile to continue
    // smoothly from wherever the arm currently is, not jump to zero.
}

import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;

public class ElevatorSubsystem extends SubsystemBase {

    private final TalonFX m_motor = new TalonFX(ElevatorConstants.MOTOR_ID);
    private final Encoder m_encoder = new Encoder(
        ElevatorConstants.ENCODER_A, ElevatorConstants.ENCODER_B);

    private final ElevatorFeedforward m_feedforward =
        new ElevatorFeedforward(
            ElevatorConstants.kS,
            ElevatorConstants.kG,   // gravity compensation — unique to ElevatorFF
            ElevatorConstants.kV,
            ElevatorConstants.kA);

    private final ProfiledPIDController m_controller =
        new ProfiledPIDController(
            ElevatorConstants.kP,
            ElevatorConstants.kI,
            ElevatorConstants.kD,
            new TrapezoidProfile.Constraints(
                ElevatorConstants.MAX_VELOCITY_M_PER_S,
                ElevatorConstants.MAX_ACCEL_M_PER_S2));

    @Override
    public void periodic() {
        double pidOutput = m_controller.calculate(getHeightMeters());

        // getSetpoint() returns the profile's current intermediate state
        double ffOutput = m_feedforward.calculate(
            m_controller.getSetpoint().velocity);

        m_motor.setVoltage(pidOutput + ffOutput);
    }

    public void setGoalHeight(double heightMeters) {
        m_controller.setGoal(heightMeters);
    }

    public boolean atGoal() {
        return m_controller.atGoal();
        // atGoal() is true when the profile is complete AND position is within tolerance.
        // Use this in Commands to know when to call isFinished().
    }

    private double getHeightMeters() {
        return m_encoder.getDistance();
    }
}

public class ElevatorToHeightCommand extends Command {

    private final ElevatorSubsystem m_elevator;
    private final double m_targetMeters;

    public ElevatorToHeightCommand(ElevatorSubsystem elevator, double targetMeters) {
        m_elevator = elevator;
        m_targetMeters = targetMeters;
        addRequirements(elevator);
    }

    @Override
    public void initialize() {
        m_elevator.setGoalHeight(m_targetMeters);
        // That's it. The subsystem's periodic() advances the profile every loop.
    }

    @Override
    public boolean isFinished() {
        return m_elevator.atGoal();
    }

    @Override
    public void end(boolean interrupted) {
        if (interrupted) {
            // If interrupted mid-profile, hold current position rather than going limp
            m_elevator.setGoalHeight(m_elevator.getHeightMeters());
        }
    }
}