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.util.protobuf.ProtobufSerializable;
010import edu.wpi.first.util.struct.StructSerializable;
011
012/**
013 * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting
014 * against the force of gravity on a beam suspended at an angle).
015 */
016public class ArmFeedforward implements ProtobufSerializable, StructSerializable {
017  /** The static gain, in volts. */
018  public final double ks;
019
020  /** The gravity gain, in volts. */
021  public final double kg;
022
023  /** The velocity gain, in volt seconds per radian. */
024  public final double kv;
025
026  /** The acceleration gain, in volt seconds² per radian. */
027  public final double ka;
028
029  /** Arm feedforward protobuf for serialization. */
030  public static final ArmFeedforwardProto proto = new ArmFeedforwardProto();
031
032  /** Arm feedforward struct for serialization. */
033  public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct();
034
035  /**
036   * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate
037   * units of the computed feedforward.
038   *
039   * @param ks The static gain.
040   * @param kg The gravity gain.
041   * @param kv The velocity gain.
042   * @param ka The acceleration gain.
043   * @throws IllegalArgumentException for kv < zero.
044   * @throws IllegalArgumentException for ka < zero.
045   */
046  public ArmFeedforward(double ks, double kg, double kv, double ka) {
047    this.ks = ks;
048    this.kg = kg;
049    this.kv = kv;
050    this.ka = ka;
051    if (kv < 0.0) {
052      throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!");
053    }
054    if (ka < 0.0) {
055      throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
056    }
057  }
058
059  /**
060   * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero.
061   * Units of the gain values will dictate units of the computed feedforward.
062   *
063   * @param ks The static gain.
064   * @param kg The gravity gain.
065   * @param kv The velocity gain.
066   */
067  public ArmFeedforward(double ks, double kg, double kv) {
068    this(ks, kg, kv, 0);
069  }
070
071  /**
072   * Calculates the feedforward from the gains and setpoints.
073   *
074   * @param positionRadians The position (angle) setpoint. This angle should be measured from the
075   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
076   *     your encoder does not follow this convention, an offset should be added.
077   * @param velocityRadPerSec The velocity setpoint.
078   * @param accelRadPerSecSquared The acceleration setpoint.
079   * @return The computed feedforward.
080   */
081  public double calculate(
082      double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
083    return ks * Math.signum(velocityRadPerSec)
084        + kg * Math.cos(positionRadians)
085        + kv * velocityRadPerSec
086        + ka * accelRadPerSecSquared;
087  }
088
089  /**
090   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
091   * zero).
092   *
093   * @param positionRadians The position (angle) setpoint. This angle should be measured from the
094   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
095   *     your encoder does not follow this convention, an offset should be added.
096   * @param velocity The velocity setpoint.
097   * @return The computed feedforward.
098   */
099  public double calculate(double positionRadians, double velocity) {
100    return calculate(positionRadians, velocity, 0);
101  }
102
103  // Rearranging the main equation from the calculate() method yields the
104  // formulas for the methods below:
105
106  /**
107   * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an
108   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
109   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
110   * you a simultaneously-achievable velocity constraint.
111   *
112   * @param maxVoltage The maximum voltage that can be supplied to the arm.
113   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
114   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
115   *     not follow this convention, an offset should be added.
116   * @param acceleration The acceleration of the arm.
117   * @return The maximum possible velocity at the given acceleration and angle.
118   */
119  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
120    // Assume max velocity is positive
121    return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv;
122  }
123
124  /**
125   * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an
126   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
127   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
128   * you a simultaneously-achievable velocity constraint.
129   *
130   * @param maxVoltage The maximum voltage that can be supplied to the arm.
131   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
132   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
133   *     not follow this convention, an offset should be added.
134   * @param acceleration The acceleration of the arm.
135   * @return The minimum possible velocity at the given acceleration and angle.
136   */
137  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
138    // Assume min velocity is negative, ks flips sign
139    return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv;
140  }
141
142  /**
143   * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and
144   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
145   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
146   * simultaneously-achievable acceleration constraint.
147   *
148   * @param maxVoltage The maximum voltage that can be supplied to the arm.
149   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
150   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
151   *     not follow this convention, an offset should be added.
152   * @param velocity The velocity of the arm.
153   * @return The maximum possible acceleration at the given velocity.
154   */
155  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
156    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
157  }
158
159  /**
160   * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and
161   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
162   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
163   * simultaneously-achievable acceleration constraint.
164   *
165   * @param maxVoltage The maximum voltage that can be supplied to the arm.
166   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
167   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
168   *     not follow this convention, an offset should be added.
169   * @param velocity The velocity of the arm.
170   * @return The minimum possible acceleration at the given velocity.
171   */
172  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
173    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
174  }
175}