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}