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.MathUtil;
008import edu.wpi.first.util.WPIUtilJNI;
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_rateLimit;
018  private double m_prevVal;
019  private double m_prevTime;
020
021  /**
022   * Creates a new SlewRateLimiter with the given rate limit and initial value.
023   *
024   * @param rateLimit The rate-of-change limit, in units per second.
025   * @param initialValue The initial value of the input.
026   */
027  public SlewRateLimiter(double rateLimit, double initialValue) {
028    m_rateLimit = rateLimit;
029    m_prevVal = initialValue;
030    m_prevTime = WPIUtilJNI.now() * 1e-6;
031  }
032
033  /**
034   * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
035   *
036   * @param rateLimit The rate-of-change limit, in units per second.
037   */
038  public SlewRateLimiter(double rateLimit) {
039    this(rateLimit, 0);
040  }
041
042  /**
043   * Filters the input to limit its slew rate.
044   *
045   * @param input The input value whose slew rate is to be limited.
046   * @return The filtered value, which will not change faster than the slew rate.
047   */
048  public double calculate(double input) {
049    double currentTime = WPIUtilJNI.now() * 1e-6;
050    double elapsedTime = currentTime - m_prevTime;
051    m_prevVal +=
052        MathUtil.clamp(input - m_prevVal, -m_rateLimit * elapsedTime, m_rateLimit * elapsedTime);
053    m_prevTime = currentTime;
054    return m_prevVal;
055  }
056
057  /**
058   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
059   *
060   * @param value The value to reset to.
061   */
062  public void reset(double value) {
063    m_prevVal = value;
064    m_prevTime = WPIUtilJNI.now() * 1e-6;
065  }
066}