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
007/**
008 * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
009 * against the force of gravity).
010 */
011@SuppressWarnings("MemberName")
012public class ElevatorFeedforward {
013  public final double ks;
014  public final double kg;
015  public final double kv;
016  public final double ka;
017
018  /**
019   * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
020   * dictate units of the computed feedforward.
021   *
022   * @param ks The static gain.
023   * @param kg The gravity gain.
024   * @param kv The velocity gain.
025   * @param ka The acceleration gain.
026   */
027  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
028    this.ks = ks;
029    this.kg = kg;
030    this.kv = kv;
031    this.ka = ka;
032  }
033
034  /**
035   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
036   * zero. Units of the gain values will dictate units of the computed feedforward.
037   *
038   * @param ks The static gain.
039   * @param kg The gravity gain.
040   * @param kv The velocity gain.
041   */
042  public ElevatorFeedforward(double ks, double kg, double kv) {
043    this(ks, kg, kv, 0);
044  }
045
046  /**
047   * Calculates the feedforward from the gains and setpoints.
048   *
049   * @param velocity The velocity setpoint.
050   * @param acceleration The acceleration setpoint.
051   * @return The computed feedforward.
052   */
053  public double calculate(double velocity, double acceleration) {
054    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
055  }
056
057  /**
058   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
059   * zero).
060   *
061   * @param velocity The velocity setpoint.
062   * @return The computed feedforward.
063   */
064  public double calculate(double velocity) {
065    return calculate(velocity, 0);
066  }
067
068  // Rearranging the main equation from the calculate() method yields the
069  // formulas for the methods below:
070
071  /**
072   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
073   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
074   * simultaneously achievable - enter the acceleration constraint, and this will give you a
075   * simultaneously-achievable velocity constraint.
076   *
077   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
078   * @param acceleration The acceleration of the elevator.
079   * @return The maximum possible velocity at the given acceleration.
080   */
081  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
082    // Assume max velocity is positive
083    return (maxVoltage - ks - kg - acceleration * ka) / kv;
084  }
085
086  /**
087   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
088   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
089   * simultaneously achievable - enter the acceleration constraint, and this will give you a
090   * simultaneously-achievable velocity constraint.
091   *
092   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
093   * @param acceleration The acceleration of the elevator.
094   * @return The minimum possible velocity at the given acceleration.
095   */
096  public double minAchievableVelocity(double maxVoltage, double acceleration) {
097    // Assume min velocity is negative, ks flips sign
098    return (-maxVoltage + ks - kg - acceleration * ka) / kv;
099  }
100
101  /**
102   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
103   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
104   * simultaneously achievable - enter the velocity constraint, and this will give you a
105   * simultaneously-achievable acceleration constraint.
106   *
107   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
108   * @param velocity The velocity of the elevator.
109   * @return The maximum possible acceleration at the given velocity.
110   */
111  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
112    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka;
113  }
114
115  /**
116   * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
117   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
118   * simultaneously achievable - enter the velocity constraint, and this will give you a
119   * simultaneously-achievable acceleration constraint.
120   *
121   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
122   * @param velocity The velocity of the elevator.
123   * @return The minimum possible acceleration at the given velocity.
124   */
125  public double minAchievableAcceleration(double maxVoltage, double velocity) {
126    return maxAchievableAcceleration(-maxVoltage, velocity);
127  }
128}