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