001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.filter;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.MathUtil;
009
010/**
011 * A class that limits the rate of change of an input value. Useful for implementing voltage,
012 * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
013 * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
014 * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
015 */
016public class SlewRateLimiter {
017  private final double m_positiveRateLimit;
018  private final double m_negativeRateLimit;
019  private double m_prevVal;
020  private double m_prevTime;
021
022  /**
023   * Creates a new SlewRateLimiter with the given positive and negative rate limits and initial
024   * value.
025   *
026   * @param positiveRateLimit The rate-of-change limit in the positive direction, in units per
027   *     second. This is expected to be positive.
028   * @param negativeRateLimit The rate-of-change limit in the negative direction, in units per
029   *     second. This is expected to be negative.
030   * @param initialValue The initial value of the input.
031   */
032  public SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double initialValue) {
033    m_positiveRateLimit = positiveRateLimit;
034    m_negativeRateLimit = negativeRateLimit;
035    m_prevVal = initialValue;
036    m_prevTime = MathSharedStore.getTimestamp();
037  }
038
039  /**
040   * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
041   * -rateLimit and initial value.
042   *
043   * @param rateLimit The rate-of-change limit, in units per second.
044   * @param initalValue The initial value of the input.
045   * @deprecated Use SlewRateLimiter(double positiveRateLimit, double negativeRateLimit, double
046   *     initalValue) instead.
047   */
048  @Deprecated(since = "2023", forRemoval = true)
049  public SlewRateLimiter(double rateLimit, double initalValue) {
050    this(rateLimit, -rateLimit, initalValue);
051  }
052
053  /**
054   * Creates a new SlewRateLimiter with the given positive rate limit and negative rate limit of
055   * -rateLimit.
056   *
057   * @param rateLimit The rate-of-change limit, in units per second.
058   */
059  public SlewRateLimiter(double rateLimit) {
060    this(rateLimit, -rateLimit, 0);
061  }
062
063  /**
064   * Filters the input to limit its slew rate.
065   *
066   * @param input The input value whose slew rate is to be limited.
067   * @return The filtered value, which will not change faster than the slew rate.
068   */
069  public double calculate(double input) {
070    double currentTime = MathSharedStore.getTimestamp();
071    double elapsedTime = currentTime - m_prevTime;
072    m_prevVal +=
073        MathUtil.clamp(
074            input - m_prevVal,
075            m_negativeRateLimit * elapsedTime,
076            m_positiveRateLimit * elapsedTime);
077    m_prevTime = currentTime;
078    return m_prevVal;
079  }
080
081  /**
082   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
083   *
084   * @param value The value to reset to.
085   */
086  public void reset(double value) {
087    m_prevVal = value;
088    m_prevTime = MathSharedStore.getTimestamp();
089  }
090}