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.Matrix;
008import edu.wpi.first.math.Nat;
009import edu.wpi.first.math.system.plant.LinearSystemId;
010
011/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
012@SuppressWarnings("MemberName")
013public class SimpleMotorFeedforward {
014  public final double ks;
015  public final double kv;
016  public final double ka;
017
018  /**
019   * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values will
020   * dictate units of the computed feedforward.
021   *
022   * @param ks The static gain.
023   * @param kv The velocity gain.
024   * @param ka The acceleration gain.
025   */
026  public SimpleMotorFeedforward(double ks, double kv, double ka) {
027    this.ks = ks;
028    this.kv = kv;
029    this.ka = ka;
030  }
031
032  /**
033   * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
034   * to zero. Units of the gain values will dictate units of the computed feedforward.
035   *
036   * @param ks The static gain.
037   * @param kv The velocity gain.
038   */
039  public SimpleMotorFeedforward(double ks, double kv) {
040    this(ks, kv, 0);
041  }
042
043  /**
044   * Calculates the feedforward from the gains and setpoints.
045   *
046   * @param velocity The velocity setpoint.
047   * @param acceleration The acceleration setpoint.
048   * @return The computed feedforward.
049   */
050  public double calculate(double velocity, double acceleration) {
051    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
052  }
053
054  /**
055   * Calculates the feedforward from the gains and setpoints.
056   *
057   * @param currentVelocity The current velocity setpoint.
058   * @param nextVelocity The next velocity setpoint.
059   * @param dtSeconds Time between velocity setpoints in seconds.
060   * @return The computed feedforward.
061   */
062  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
063    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
064    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
065
066    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
067    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
068
069    return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
070  }
071
072  // Rearranging the main equation from the calculate() method yields the
073  // formulas for the methods below:
074
075  /**
076   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
077   * zero).
078   *
079   * @param velocity The velocity setpoint.
080   * @return The computed feedforward.
081   */
082  public double calculate(double velocity) {
083    return calculate(velocity, 0);
084  }
085
086  /**
087   * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
088   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
089   * simultaneously achievable - enter the acceleration constraint, and this will give you a
090   * simultaneously-achievable velocity constraint.
091   *
092   * @param maxVoltage The maximum voltage that can be supplied to the motor.
093   * @param acceleration The acceleration of the motor.
094   * @return The maximum possible velocity at the given acceleration.
095   */
096  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
097    // Assume max velocity is positive
098    return (maxVoltage - ks - acceleration * ka) / kv;
099  }
100
101  /**
102   * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
103   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
104   * simultaneously achievable - enter the acceleration constraint, and this will give you a
105   * simultaneously-achievable velocity constraint.
106   *
107   * @param maxVoltage The maximum voltage that can be supplied to the motor.
108   * @param acceleration The acceleration of the motor.
109   * @return The minimum possible velocity at the given acceleration.
110   */
111  public double minAchievableVelocity(double maxVoltage, double acceleration) {
112    // Assume min velocity is negative, ks flips sign
113    return (-maxVoltage + ks - acceleration * ka) / kv;
114  }
115
116  /**
117   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
118   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
119   * 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 motor.
123   * @param velocity The velocity of the motor.
124   * @return The maximum possible acceleration at the given velocity.
125   */
126  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
127    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
128  }
129
130  /**
131   * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
132   * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are
133   * simultaneously achievable - enter the velocity constraint, and this will give you a
134   * simultaneously-achievable acceleration constraint.
135   *
136   * @param maxVoltage The maximum voltage that can be supplied to the motor.
137   * @param velocity The velocity of the motor.
138   * @return The minimum possible acceleration at the given velocity.
139   */
140  public double minAchievableAcceleration(double maxVoltage, double velocity) {
141    return maxAchievableAcceleration(-maxVoltage, velocity);
142  }
143}