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.MatBuilder; 008import edu.wpi.first.math.Nat; 009import edu.wpi.first.math.controller.proto.ElevatorFeedforwardProto; 010import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct; 011import edu.wpi.first.math.system.plant.LinearSystemId; 012 013/** 014 * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting 015 * against the force of gravity). 016 */ 017public class ElevatorFeedforward { 018 public final double ks; 019 public final double kg; 020 public final double kv; 021 public final double ka; 022 023 public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto(); 024 public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct(); 025 026 /** 027 * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will 028 * dictate units of the computed feedforward. 029 * 030 * @param ks The static gain. 031 * @param kg The gravity gain. 032 * @param kv The velocity gain. 033 * @param ka The acceleration gain. 034 */ 035 public ElevatorFeedforward(double ks, double kg, double kv, double ka) { 036 this.ks = ks; 037 this.kg = kg; 038 this.kv = kv; 039 this.ka = ka; 040 } 041 042 /** 043 * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to 044 * zero. Units of the gain values will dictate units of the computed feedforward. 045 * 046 * @param ks The static gain. 047 * @param kg The gravity gain. 048 * @param kv The velocity gain. 049 */ 050 public ElevatorFeedforward(double ks, double kg, double kv) { 051 this(ks, kg, kv, 0); 052 } 053 054 /** 055 * Calculates the feedforward from the gains and setpoints. 056 * 057 * @param velocity The velocity setpoint. 058 * @param acceleration The acceleration setpoint. 059 * @return The computed feedforward. 060 */ 061 public double calculate(double velocity, double acceleration) { 062 return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; 063 } 064 065 /** 066 * Calculates the feedforward from the gains and setpoints. 067 * 068 * <p>Note this method is inaccurate when the velocity crosses 0. 069 * 070 * @param currentVelocity The current velocity setpoint. 071 * @param nextVelocity The next velocity setpoint. 072 * @param dtSeconds Time between velocity setpoints in seconds. 073 * @return The computed feedforward. 074 */ 075 public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { 076 // Discretize the affine model. 077 // 078 // dx/dt = Ax + Bu + c 079 // dx/dt = Ax + B(u + B⁺c) 080 // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) 081 // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) 082 // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ 083 // 084 // Solve for uₖ. 085 // 086 // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ 087 // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) 088 // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ 089 // 090 // For an elevator with the model 091 // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), 092 // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B 093 // assuming sgn(x) is a constant for the duration of the step. 094 // 095 // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) 096 // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) 097 // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) 098 var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); 099 var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); 100 101 var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity); 102 var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity); 103 104 return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); 105 } 106 107 /** 108 * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be 109 * zero). 110 * 111 * @param velocity The velocity setpoint. 112 * @return The computed feedforward. 113 */ 114 public double calculate(double velocity) { 115 return calculate(velocity, 0); 116 } 117 118 // Rearranging the main equation from the calculate() method yields the 119 // formulas for the methods below: 120 121 /** 122 * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration. 123 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 124 * simultaneously achievable - enter the acceleration constraint, and this will give you a 125 * simultaneously-achievable velocity constraint. 126 * 127 * @param maxVoltage The maximum voltage that can be supplied to the elevator. 128 * @param acceleration The acceleration of the elevator. 129 * @return The maximum possible velocity at the given acceleration. 130 */ 131 public double maxAchievableVelocity(double maxVoltage, double acceleration) { 132 // Assume max velocity is positive 133 return (maxVoltage - ks - kg - acceleration * ka) / kv; 134 } 135 136 /** 137 * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration. 138 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 139 * simultaneously achievable - enter the acceleration constraint, and this will give you a 140 * simultaneously-achievable velocity constraint. 141 * 142 * @param maxVoltage The maximum voltage that can be supplied to the elevator. 143 * @param acceleration The acceleration of the elevator. 144 * @return The minimum possible velocity at the given acceleration. 145 */ 146 public double minAchievableVelocity(double maxVoltage, double acceleration) { 147 // Assume min velocity is negative, ks flips sign 148 return (-maxVoltage + ks - kg - acceleration * ka) / kv; 149 } 150 151 /** 152 * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity. 153 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 154 * simultaneously achievable - enter the velocity constraint, and this will give you a 155 * simultaneously-achievable acceleration constraint. 156 * 157 * @param maxVoltage The maximum voltage that can be supplied to the elevator. 158 * @param velocity The velocity of the elevator. 159 * @return The maximum possible acceleration at the given velocity. 160 */ 161 public double maxAchievableAcceleration(double maxVoltage, double velocity) { 162 return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka; 163 } 164 165 /** 166 * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity. 167 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 168 * simultaneously achievable - enter the velocity constraint, and this will give you a 169 * simultaneously-achievable acceleration constraint. 170 * 171 * @param maxVoltage The maximum voltage that can be supplied to the elevator. 172 * @param velocity The velocity of the elevator. 173 * @return The minimum possible acceleration at the given velocity. 174 */ 175 public double minAchievableAcceleration(double maxVoltage, double velocity) { 176 return maxAchievableAcceleration(-maxVoltage, velocity); 177 } 178}