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