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 double ks;
019
020  /** The gravity gain, in volts. */
021  private double kg;
022
023  /** The velocity gain, in V/(m/s). */
024  private double kv;
025
026  /** The acceleration gain, in V/(m/s²). */
027  private 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 dt 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 dt) {
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 (dt <= 0.0) {
056      throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
057    }
058    m_dt = dt;
059  }
060
061  /**
062   * Creates a new ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms.
063   *
064   * @param ks The static gain in volts.
065   * @param kg The gravity gain in volts.
066   * @param kv The velocity gain in V/(m/s).
067   * @param ka The acceleration gain in V/(m/s²).
068   * @throws IllegalArgumentException for kv &lt; zero.
069   * @throws IllegalArgumentException for ka &lt; zero.
070   */
071  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
072    this(ks, kg, kv, ka, 0.020);
073  }
074
075  /**
076   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
077   * zero. The period is defaulted to 20 ms.
078   *
079   * @param ks The static gain in volts.
080   * @param kg The gravity gain in volts.
081   * @param kv The velocity gain in V/(m/s).
082   * @throws IllegalArgumentException for kv &lt; zero.
083   */
084  public ElevatorFeedforward(double ks, double kg, double kv) {
085    this(ks, kg, kv, 0);
086  }
087
088  /**
089   * Sets the static gain.
090   *
091   * @param ks The static gain in volts.
092   */
093  public void setKs(double ks) {
094    this.ks = ks;
095  }
096
097  /**
098   * Sets the gravity gain.
099   *
100   * @param kg The gravity gain in volts.
101   */
102  public void setKg(double kg) {
103    this.kg = kg;
104  }
105
106  /**
107   * Sets the velocity gain.
108   *
109   * @param kv The velocity gain in V/(m/s).
110   */
111  public void setKv(double kv) {
112    this.kv = kv;
113  }
114
115  /**
116   * Sets the acceleration gain.
117   *
118   * @param ka The acceleration gain in V/(m/s²).
119   */
120  public void setKa(double ka) {
121    this.ka = ka;
122  }
123
124  /**
125   * Returns the static gain in volts.
126   *
127   * @return The static gain in volts.
128   */
129  public double getKs() {
130    return ks;
131  }
132
133  /**
134   * Returns the gravity gain in volts.
135   *
136   * @return The gravity gain in volts.
137   */
138  public double getKg() {
139    return kg;
140  }
141
142  /**
143   * Returns the velocity gain in V/(m/s).
144   *
145   * @return The velocity gain.
146   */
147  public double getKv() {
148    return kv;
149  }
150
151  /**
152   * Returns the acceleration gain in V/(m/s²).
153   *
154   * @return The acceleration gain.
155   */
156  public double getKa() {
157    return ka;
158  }
159
160  /**
161   * Returns the period in seconds.
162   *
163   * @return The period in seconds.
164   */
165  public double getDt() {
166    return m_dt;
167  }
168
169  /**
170   * Calculates the feedforward from the gains and velocity setpoint assuming continuous control
171   * (acceleration is assumed to be zero).
172   *
173   * @param velocity The velocity setpoint in meters per second.
174   * @return The computed feedforward.
175   */
176  public double calculate(double velocity) {
177    return calculate(velocity, velocity);
178  }
179
180  /**
181   * Calculates the feedforward from the gains and setpoints assuming discrete control.
182   *
183   * <p>Note this method is inaccurate when the velocity crosses 0.
184   *
185   * @param currentVelocity The current velocity setpoint in meters per second.
186   * @param nextVelocity The next velocity setpoint in meters per second.
187   * @return The computed feedforward.
188   */
189  public double calculate(double currentVelocity, double nextVelocity) {
190    // See wpimath/algorithms.md#Elevator_feedforward for derivation
191    if (ka < 1e-9) {
192      return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
193    } else {
194      double A = -kv / ka;
195      double B = 1.0 / ka;
196      double A_d = Math.exp(A * m_dt);
197      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
198      return kg
199          + ks * Math.signum(currentVelocity)
200          + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
201    }
202  }
203
204  // Rearranging the main equation from the calculate() method yields the
205  // formulas for the methods below:
206
207  /**
208   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
209   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
210   * simultaneously achievable - enter the acceleration constraint, and this will give you a
211   * simultaneously-achievable velocity constraint.
212   *
213   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
214   * @param acceleration The acceleration of the elevator, in (m/s²).
215   * @return The maximum possible velocity in (m/s) at the given acceleration.
216   */
217  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
218    // Assume max velocity is positive
219    return (maxVoltage - ks - kg - acceleration * ka) / kv;
220  }
221
222  /**
223   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
224   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
225   * simultaneously achievable - enter the acceleration constraint, and this will give you a
226   * simultaneously-achievable velocity constraint.
227   *
228   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
229   * @param acceleration The acceleration of the elevator, in (m/s²).
230   * @return The maximum possible velocity in (m/s) at the given acceleration.
231   */
232  public double minAchievableVelocity(double maxVoltage, double acceleration) {
233    // Assume min velocity is negative, ks flips sign
234    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
235  }
236
237  /**
238   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
239   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
240   * simultaneously achievable - enter the velocity constraint, and this will give you a
241   * simultaneously-achievable acceleration constraint.
242   *
243   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
244   * @param velocity The velocity of the elevator, in (m/s)
245   * @return The maximum possible acceleration in (m/s²) at the given velocity.
246   */
247  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
248    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
249  }
250
251  /**
252   * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
253   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
254   * simultaneously achievable - enter the velocity constraint, and this will give you a
255   * simultaneously-achievable acceleration constraint.
256   *
257   * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts.
258   * @param velocity The velocity of the elevator, in (m/s)
259   * @return The maximum possible acceleration in (m/s²) at the given velocity.
260   */
261  public double minAchievableAcceleration(double maxVoltage, double velocity) {
262    return maxAchievableAcceleration(-maxVoltage, velocity);
263  }
264
265  /** ElevatorFeedforward struct for serialization. */
266  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
267
268  /** ElevatorFeedforward protobuf for serialization. */
269  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
270}