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