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}