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.MatBuilder;
008import edu.wpi.first.math.Nat;
009import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto;
010import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct;
011import edu.wpi.first.math.system.plant.LinearSystemId;
012import edu.wpi.first.util.protobuf.ProtobufSerializable;
013import edu.wpi.first.util.struct.StructSerializable;
014
015/**
016 * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
017 * against the force of gravity).
018 */
019public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable {
020  /** The static gain. */
021  public final double ks;
022
023  /** The gravity gain. */
024  public final double kg;
025
026  /** The velocity gain. */
027  public final double kv;
028
029  /** The acceleration gain. */
030  public final double ka;
031
032  /** ElevatorFeedforward protobuf for serialization. */
033  public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto();
034
035  /** ElevatorFeedforward struct for serialization. */
036  public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct();
037
038  /**
039   * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
040   * dictate units of the computed feedforward.
041   *
042   * @param ks The static gain.
043   * @param kg The gravity gain.
044   * @param kv The velocity gain.
045   * @param ka The acceleration gain.
046   * @throws IllegalArgumentException for kv < zero.
047   * @throws IllegalArgumentException for ka < zero.
048   */
049  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
050    this.ks = ks;
051    this.kg = kg;
052    this.kv = kv;
053    this.ka = ka;
054    if (kv < 0.0) {
055      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
056    }
057    if (ka < 0.0) {
058      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
059    }
060  }
061
062  /**
063   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
064   * zero. Units of the gain values will dictate units of the computed feedforward.
065   *
066   * @param ks The static gain.
067   * @param kg The gravity gain.
068   * @param kv The velocity gain.
069   */
070  public ElevatorFeedforward(double ks, double kg, double kv) {
071    this(ks, kg, kv, 0);
072  }
073
074  /**
075   * Calculates the feedforward from the gains and setpoints.
076   *
077   * @param velocity The velocity setpoint.
078   * @param acceleration The acceleration setpoint.
079   * @return The computed feedforward.
080   */
081  public double calculate(double velocity, double acceleration) {
082    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
083  }
084
085  /**
086   * Calculates the feedforward from the gains and setpoints.
087   *
088   * <p>Note this method is inaccurate when the velocity crosses 0.
089   *
090   * @param currentVelocity The current velocity setpoint.
091   * @param nextVelocity The next velocity setpoint.
092   * @param dtSeconds Time between velocity setpoints in seconds.
093   * @return The computed feedforward.
094   */
095  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
096    // Discretize the affine model.
097    //
098    //   dx/dt = Ax + Bu + c
099    //   dx/dt = Ax + B(u + B⁺c)
100    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
101    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
102    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
103    //
104    // Solve for uₖ.
105    //
106    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
107    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
108    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
109    //
110    // For an elevator with the model
111    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
112    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
113    // assuming sgn(x) is a constant for the duration of the step.
114    //
115    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
116    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
117    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
118    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
119    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
120
121    var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
122    var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
123
124    return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
125  }
126
127  /**
128   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
129   * zero).
130   *
131   * @param velocity The velocity setpoint.
132   * @return The computed feedforward.
133   */
134  public double calculate(double velocity) {
135    return calculate(velocity, 0);
136  }
137
138  // Rearranging the main equation from the calculate() method yields the
139  // formulas for the methods below:
140
141  /**
142   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
143   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
144   * simultaneously achievable - enter the acceleration constraint, and this will give you a
145   * simultaneously-achievable velocity constraint.
146   *
147   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
148   * @param acceleration The acceleration of the elevator.
149   * @return The maximum possible velocity at the given acceleration.
150   */
151  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
152    // Assume max velocity is positive
153    return (maxVoltage - ks - kg - acceleration * ka) / kv;
154  }
155
156  /**
157   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
158   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
159   * simultaneously achievable - enter the acceleration constraint, and this will give you a
160   * simultaneously-achievable velocity constraint.
161   *
162   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
163   * @param acceleration The acceleration of the elevator.
164   * @return The minimum possible velocity at the given acceleration.
165   */
166  public double minAchievableVelocity(double maxVoltage, double acceleration) {
167    // Assume min velocity is negative, ks flips sign
168    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
169  }
170
171  /**
172   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
173   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
174   * simultaneously achievable - enter the velocity constraint, and this will give you a
175   * simultaneously-achievable acceleration constraint.
176   *
177   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
178   * @param velocity The velocity of the elevator.
179   * @return The maximum possible acceleration at the given velocity.
180   */
181  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
182    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
183  }
184
185  /**
186   * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
187   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
188   * simultaneously achievable - enter the velocity constraint, and this will give you a
189   * simultaneously-achievable acceleration constraint.
190   *
191   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
192   * @param velocity The velocity of the elevator.
193   * @return The minimum possible acceleration at the given velocity.
194   */
195  public double minAchievableAcceleration(double maxVoltage, double velocity) {
196    return maxAchievableAcceleration(-maxVoltage, velocity);
197  }
198}