Source code

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