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.ArmFeedforwardProto;
008import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
009import edu.wpi.first.math.jni.ArmFeedforwardJNI;
010import edu.wpi.first.util.protobuf.ProtobufSerializable;
011import edu.wpi.first.util.struct.StructSerializable;
012
013/**
014 * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting
015 * against the force of gravity on a beam suspended at an angle).
016 */
017public class ArmFeedforward implements ProtobufSerializable, StructSerializable {
018  /** The static gain, in volts. */
019  private double ks;
020
021  /** The gravity gain, in volts. */
022  private double kg;
023
024  /** The velocity gain, in V/(rad/s). */
025  private double kv;
026
027  /** The acceleration gain, in V/(rad/s²). */
028  private double ka;
029
030  /** The period, in seconds. */
031  private final double m_dt;
032
033  /**
034   * Creates a new ArmFeedforward with the specified gains and period.
035   *
036   * @param ks The static gain in volts.
037   * @param kg The gravity gain in volts.
038   * @param kv The velocity gain in V/(rad/s).
039   * @param ka The acceleration gain in V/(rad/s²).
040   * @param dt The period in seconds.
041   * @throws IllegalArgumentException for kv < zero.
042   * @throws IllegalArgumentException for ka < zero.
043   * @throws IllegalArgumentException for period ≤ zero.
044   */
045  public ArmFeedforward(double ks, double kg, double kv, double ka, double dt) {
046    this.ks = ks;
047    this.kg = kg;
048    this.kv = kv;
049    this.ka = ka;
050    if (kv < 0.0) {
051      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
052    }
053    if (ka < 0.0) {
054      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
055    }
056    if (dt <= 0.0) {
057      throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
058    }
059    m_dt = dt;
060  }
061
062  /**
063   * Creates a new ArmFeedforward 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/(rad/s).
068   * @param ka The acceleration gain in V/(rad/s²).
069   * @throws IllegalArgumentException for kv &lt; zero.
070   * @throws IllegalArgumentException for ka &lt; zero.
071   */
072  public ArmFeedforward(double ks, double kg, double kv, double ka) {
073    this(ks, kg, kv, ka, 0.020);
074  }
075
076  /**
077   * Creates a new ArmFeedforward with the specified gains. 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/(rad/s).
082   * @throws IllegalArgumentException for kv &lt; zero.
083   */
084  public ArmFeedforward(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/(rad/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/(rad/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/(rad/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/(rad/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 position The position setpoint in radians. This angle should be measured from the
174   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
175   *     your encoder does not follow this convention, an offset should be added.
176   * @param velocity The velocity setpoint in radians per second.
177   * @return The computed feedforward.
178   */
179  public double calculate(double position, double velocity) {
180    return ks * Math.signum(velocity) + kg * Math.cos(position) + kv * velocity;
181  }
182
183  /**
184   * Calculates the feedforward from the gains and setpoints assuming continuous control.
185   *
186   * @param currentAngle The current angle in radians. This angle should be measured from the
187   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If
188   *     your encoder does not follow this convention, an offset should be added.
189   * @param currentVelocity The current velocity setpoint in radians per second.
190   * @param nextVelocity The next velocity setpoint in radians per second.
191   * @return The computed feedforward in volts.
192   */
193  public double calculate(double currentAngle, double currentVelocity, double nextVelocity) {
194    return ArmFeedforwardJNI.calculate(
195        ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt);
196  }
197
198  // Rearranging the main equation from the calculate() method yields the
199  // formulas for the methods below:
200
201  /**
202   * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an
203   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
204   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
205   * you a simultaneously-achievable velocity constraint.
206   *
207   * @param maxVoltage The maximum voltage that can be supplied to the arm.
208   * @param angle The angle of the arm, in radians. This angle should be measured from the
209   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
210   *     your encoder does not follow this convention, an offset should be added.
211   * @param acceleration The acceleration of the arm, in (rad/s²).
212   * @return The maximum possible velocity in (rad/s) at the given acceleration and angle.
213   */
214  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
215    // Assume max velocity is positive
216    return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv;
217  }
218
219  /**
220   * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an
221   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
222   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
223   * you a simultaneously-achievable velocity constraint.
224   *
225   * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
226   * @param angle The angle of the arm, in radians. This angle should be measured from the
227   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
228   *     your encoder does not follow this convention, an offset should be added.
229   * @param acceleration The acceleration of the arm, in (rad/s²).
230   * @return The minimum possible velocity in (rad/s) at the given acceleration and angle.
231   */
232  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
233    // Assume min velocity is negative, ks flips sign
234    return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv;
235  }
236
237  /**
238   * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and
239   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
240   * profile are 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 arm, in volts.
244   * @param angle The angle of the arm, in radians. This angle should be measured from the
245   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
246   *     your encoder does not follow this convention, an offset should be added.
247   * @param velocity The velocity of the elevator, in (rad/s)
248   * @return The maximum possible acceleration in (rad/s²) at the given velocity.
249   */
250  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
251    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
252  }
253
254  /**
255   * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and
256   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
257   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
258   * simultaneously-achievable acceleration constraint.
259   *
260   * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts.
261   * @param angle The angle of the arm, in radians. This angle should be measured from the
262   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
263   *     your encoder does not follow this convention, an offset should be added.
264   * @param velocity The velocity of the elevator, in (rad/s)
265   * @return The maximum possible acceleration in (rad/s²) at the given velocity.
266   */
267  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
268    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
269  }
270
271  /** Arm feedforward struct for serialization. */
272  public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
273
274  /** Arm feedforward protobuf for serialization. */
275  public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
276}