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