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 arm (modeled as a motor acting
009 * against the force of gravity on a beam suspended at an angle).
010 */
011public class ArmFeedforward {
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 ArmFeedforward with the specified gains. Units of the gain values will dictate
019   * 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 ArmFeedforward(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 ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero.
035   * 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 ArmFeedforward(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 positionRadians The position (angle) setpoint. This angle should be measured from the
049   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
050   *     your encoder does not follow this convention, an offset should be added.
051   * @param velocityRadPerSec The velocity setpoint.
052   * @param accelRadPerSecSquared The acceleration setpoint.
053   * @return The computed feedforward.
054   */
055  public double calculate(
056      double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
057    return ks * Math.signum(velocityRadPerSec)
058        + kg * Math.cos(positionRadians)
059        + kv * velocityRadPerSec
060        + ka * accelRadPerSecSquared;
061  }
062
063  /**
064   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
065   * zero).
066   *
067   * @param positionRadians The position (angle) setpoint. This angle should be measured from the
068   *     horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If
069   *     your encoder does not follow this convention, an offset should be added.
070   * @param velocity The velocity setpoint.
071   * @return The computed feedforward.
072   */
073  public double calculate(double positionRadians, double velocity) {
074    return calculate(positionRadians, velocity, 0);
075  }
076
077  // Rearranging the main equation from the calculate() method yields the
078  // formulas for the methods below:
079
080  /**
081   * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an
082   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
083   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
084   * you a simultaneously-achievable velocity constraint.
085   *
086   * @param maxVoltage The maximum voltage that can be supplied to the arm.
087   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
088   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
089   *     not follow this convention, an offset should be added.
090   * @param acceleration The acceleration of the arm.
091   * @return The maximum possible velocity at the given acceleration and angle.
092   */
093  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
094    // Assume max velocity is positive
095    return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv;
096  }
097
098  /**
099   * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an
100   * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
101   * profile are simultaneously achievable - enter the acceleration constraint, and this will give
102   * you a simultaneously-achievable velocity constraint.
103   *
104   * @param maxVoltage The maximum voltage that can be supplied to the arm.
105   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
106   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
107   *     not follow this convention, an offset should be added.
108   * @param acceleration The acceleration of the arm.
109   * @return The minimum possible velocity at the given acceleration and angle.
110   */
111  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
112    // Assume min velocity is negative, ks flips sign
113    return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv;
114  }
115
116  /**
117   * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and
118   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
119   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
120   * simultaneously-achievable acceleration constraint.
121   *
122   * @param maxVoltage The maximum voltage that can be supplied to the arm.
123   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
124   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
125   *     not follow this convention, an offset should be added.
126   * @param velocity The velocity of the arm.
127   * @return The maximum possible acceleration at the given velocity.
128   */
129  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
130    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka;
131  }
132
133  /**
134   * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and
135   * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal
136   * profile are simultaneously achievable - enter the velocity constraint, and this will give you a
137   * simultaneously-achievable acceleration constraint.
138   *
139   * @param maxVoltage The maximum voltage that can be supplied to the arm.
140   * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if
141   *     the provided angle is 0, the arm should be parallel with the floor). If your encoder does
142   *     not follow this convention, an offset should be added.
143   * @param velocity The velocity of the arm.
144   * @return The minimum possible acceleration at the given velocity.
145   */
146  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
147    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
148  }
149}