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.
042   *
043   * @param rateLimit The rate-of-change limit, in units per second.
044   */
045  public SlewRateLimiter(double rateLimit) {
046    this(rateLimit, -rateLimit, 0);
047  }
048
049  /**
050   * Filters the input to limit its slew rate.
051   *
052   * @param input The input value whose slew rate is to be limited.
053   * @return The filtered value, which will not change faster than the slew rate.
054   */
055  public double calculate(double input) {
056    double currentTime = MathSharedStore.getTimestamp();
057    double elapsedTime = currentTime - m_prevTime;
058    m_prevVal +=
059        MathUtil.clamp(
060            input - m_prevVal,
061            m_negativeRateLimit * elapsedTime,
062            m_positiveRateLimit * elapsedTime);
063    m_prevTime = currentTime;
064    return m_prevVal;
065  }
066
067  /**
068   * Returns the value last calculated by the SlewRateLimiter.
069   *
070   * @return The last value.
071   */
072  public double lastValue() {
073    return m_prevVal;
074  }
075
076  /**
077   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
078   *
079   * @param value The value to reset to.
080   */
081  public void reset(double value) {
082    m_prevVal = value;
083    m_prevTime = MathSharedStore.getTimestamp();
084  }
085}