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