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.controller;
006
007import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
008import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
009import edu.wpi.first.util.protobuf.ProtobufSerializable;
010import edu.wpi.first.util.struct.StructSerializable;
011
012/**
013 * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
014 * against the force of gravity).
015 */
016public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
017  /** The static gain, in volts. */
018  private final double ks;
019
020  /** The gravity gain, in volts. */
021  private final double kg;
022
023  /** The velocity gain, in V/(m/s). */
024  private final double kv;
025
026  /** The acceleration gain, in V/(m/s²). */
027  private final double ka;
028
029  /** The period, in seconds. */
030  private final double m_dt;
031
032  /**
033   * Creates a new ElevatorFeedforward with the specified gains and period.
034   *
035   * @param ks The static gain in volts.
036   * @param kg The gravity gain in volts.
037   * @param kv The velocity gain in V/(m/s).
038   * @param ka The acceleration gain in V/(m/s²).
039   * @param dtSeconds The period in seconds.
040   * @throws IllegalArgumentException for kv < zero.
041   * @throws IllegalArgumentException for ka < zero.
042   * @throws IllegalArgumentException for period ≤ zero.
043   */
044  public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
045    this.ks = ks;
046    this.kg = kg;
047    this.kv = kv;
048    this.ka = ka;
049    if (kv < 0.0) {
050      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
051    }
052    if (ka < 0.0) {
053      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
054    }
055    if (dtSeconds <= 0.0) {
056      throw new IllegalArgumentException(
057          "period must be a positive number, got " + dtSeconds + "!");
058    }
059    m_dt = dtSeconds;
060  }
061
062  /**
063   * Creates a new ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms.
064   *
065   * @param ks The static gain in volts.
066   * @param kg The gravity gain in volts.
067   * @param kv The velocity gain in V/(m/s).
068   * @param ka The acceleration gain in V/(m/s²).
069   * @throws IllegalArgumentException for kv &lt; zero.
070   * @throws IllegalArgumentException for ka &lt; zero.
071   */
072  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
073    this(ks, kg, kv, ka, 0.020);
074  }
075
076  /**
077   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
078   * zero. The period is defaulted to 20 ms.
079   *
080   * @param ks The static gain in volts.
081   * @param kg The gravity gain in volts.
082   * @param kv The velocity gain in V/(m/s).
083   * @throws IllegalArgumentException for kv &lt; zero.
084   */
085  public ElevatorFeedforward(double ks, double kg, double kv) {
086    this(ks, kg, kv, 0);
087  }
088
089  /**
090   * Returns the static gain in volts.
091   *
092   * @return The static gain in volts.
093   */
094  public double getKs() {
095    return ks;
096  }
097
098  /**
099   * Returns the gravity gain in volts.
100   *
101   * @return The gravity gain in volts.
102   */
103  public double getKg() {
104    return kg;
105  }
106
107  /**
108   * Returns the velocity gain in V/(m/s).
109   *
110   * @return The velocity gain.
111   */
112  public double getKv() {
113    return kv;
114  }
115
116  /**
117   * Returns the acceleration gain in V/(m/s²).
118   *
119   * @return The acceleration gain.
120   */
121  public double getKa() {
122    return ka;
123  }
124
125  /**
126   * Returns the period in seconds.
127   *
128   * @return The period in seconds.
129   */
130  public double getDt() {
131    return m_dt;
132  }
133
134  /**
135   * Calculates the feedforward from the gains and setpoints assuming continuous control.
136   *
137   * @param velocity The velocity setpoint.
138   * @param acceleration The acceleration setpoint.
139   * @return The computed feedforward.
140   */
141  @SuppressWarnings("removal")
142  @Deprecated(forRemoval = true, since = "2025")
143  public double calculate(double velocity, double acceleration) {
144    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
145  }
146
147  /**
148   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
149   * (acceleration is assumed to be zero).
150   *
151   * @param velocity The velocity setpoint.
152   * @return The computed feedforward.
153   */
154  public double calculate(double velocity) {
155    return calculate(velocity, 0);
156  }
157
158  /**
159   * Calculates the feedforward from the gains and setpoints assuming discrete control.
160   *
161   * <p>Note this method is inaccurate when the velocity crosses 0.
162   *
163   * @param currentVelocity The current velocity setpoint in meters per second.
164   * @param nextVelocity The next velocity setpoint in meters per second.
165   * @return The computed feedforward in volts.
166   */
167  public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
168    // See wpimath/algorithms.md#Elevator_feedforward for derivation
169    if (ka == 0.0) {
170      return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
171    } else {
172      double A = -kv / ka;
173      double B = 1.0 / ka;
174      double A_d = Math.exp(A * m_dt);
175      double B_d = 1.0 / A * (A_d - 1.0) * B;
176      return kg
177          + ks * Math.signum(currentVelocity)
178          + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
179    }
180  }
181
182  // Rearranging the main equation from the calculate() method yields the
183  // formulas for the methods below:
184
185  /**
186   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
187   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
188   * simultaneously achievable - enter the acceleration constraint, and this will give you a
189   * simultaneously-achievable velocity constraint.
190   *
191   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
192   * @param acceleration The acceleration of the elevator, in (m/s²).
193   * @return The maximum possible velocity in (m/s) at the given acceleration.
194   */
195  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
196    // Assume max velocity is positive
197    return (maxVoltage - ks - kg - acceleration * ka) / kv;
198  }
199
200  /**
201   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
202   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
203   * simultaneously achievable - enter the acceleration constraint, and this will give you a
204   * simultaneously-achievable velocity constraint.
205   *
206   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
207   * @param acceleration The acceleration of the elevator, in (m/s²).
208   * @return The maximum possible velocity in (m/s) at the given acceleration.
209   */
210  public double minAchievableVelocity(double maxVoltage, double acceleration) {
211    // Assume min velocity is negative, ks flips sign
212    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
213  }
214
215  /**
216   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
217   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
218   * simultaneously achievable - enter the velocity constraint, and this will give you a
219   * simultaneously-achievable acceleration constraint.
220   *
221   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
222   * @param velocity The velocity of the elevator, in (m/s)
223   * @return The maximum possible acceleration in (m/s²) at the given velocity.
224   */
225  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
226    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
227  }
228
229  /**
230   * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
231   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
232   * simultaneously achievable - enter the velocity constraint, and this will give you a
233   * simultaneously-achievable acceleration constraint.
234   *
235   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
236   * @param velocity The velocity of the elevator, in (m/s)
237   * @return The maximum possible acceleration in (m/s²) at the given velocity.
238   */
239  public double minAchievableAcceleration(double maxVoltage, double velocity) {
240    return maxAchievableAcceleration(-maxVoltage, velocity);
241  }
242
243  /** ElevatorFeedforward struct for serialization. */
244  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
245
246  /** ElevatorFeedforward protobuf for serialization. */
247  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
248}