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