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.ElevatorFeedforwardProto; 008import edu.wpi.first.math.controller.struct.ElevatorFeedforwardStruct; 009import edu.wpi.first.util.protobuf.ProtobufSerializable; 010import edu.wpi.first.util.struct.StructSerializable; 011 012/** 013 * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting 014 * against the force of gravity). 015 */ 016public class ElevatorFeedforward implements ProtobufSerializable, StructSerializable { 017 /** The static gain, in volts. */ 018 private double ks; 019 020 /** The gravity gain, in volts. */ 021 private double kg; 022 023 /** The velocity gain, in V/(m/s). */ 024 private double kv; 025 026 /** The acceleration gain, in V/(m/s²). */ 027 private double ka; 028 029 /** The period, in seconds. */ 030 private final double m_dt; 031 032 /** 033 * Creates a new ElevatorFeedforward with the specified gains and period. 034 * 035 * @param ks The static gain in volts. 036 * @param kg The gravity gain in volts. 037 * @param kv The velocity gain in V/(m/s). 038 * @param ka The acceleration gain in V/(m/s²). 039 * @param dt The period in seconds. 040 * @throws IllegalArgumentException for kv < zero. 041 * @throws IllegalArgumentException for ka < zero. 042 * @throws IllegalArgumentException for period ≤ zero. 043 */ 044 public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dt) { 045 this.ks = ks; 046 this.kg = kg; 047 this.kv = kv; 048 this.ka = ka; 049 if (kv < 0.0) { 050 throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); 051 } 052 if (ka < 0.0) { 053 throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); 054 } 055 if (dt <= 0.0) { 056 throw new IllegalArgumentException("period must be a positive number, got " + dt + "!"); 057 } 058 m_dt = dt; 059 } 060 061 /** 062 * Creates a new ElevatorFeedforward with the specified gains. The period is defaulted to 20 ms. 063 * 064 * @param ks The static gain in volts. 065 * @param kg The gravity gain in volts. 066 * @param kv The velocity gain in V/(m/s). 067 * @param ka The acceleration gain in V/(m/s²). 068 * @throws IllegalArgumentException for kv < zero. 069 * @throws IllegalArgumentException for ka < zero. 070 */ 071 public ElevatorFeedforward(double ks, double kg, double kv, double ka) { 072 this(ks, kg, kv, ka, 0.020); 073 } 074 075 /** 076 * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to 077 * zero. The period is defaulted to 20 ms. 078 * 079 * @param ks The static gain in volts. 080 * @param kg The gravity gain in volts. 081 * @param kv The velocity gain in V/(m/s). 082 * @throws IllegalArgumentException for kv < zero. 083 */ 084 public ElevatorFeedforward(double ks, double kg, double kv) { 085 this(ks, kg, kv, 0); 086 } 087 088 /** 089 * Sets the static gain. 090 * 091 * @param ks The static gain in volts. 092 */ 093 public void setKs(double ks) { 094 this.ks = ks; 095 } 096 097 /** 098 * Sets the gravity gain. 099 * 100 * @param kg The gravity gain in volts. 101 */ 102 public void setKg(double kg) { 103 this.kg = kg; 104 } 105 106 /** 107 * Sets the velocity gain. 108 * 109 * @param kv The velocity gain in V/(m/s). 110 */ 111 public void setKv(double kv) { 112 this.kv = kv; 113 } 114 115 /** 116 * Sets the acceleration gain. 117 * 118 * @param ka The acceleration gain in V/(m/s²). 119 */ 120 public void setKa(double ka) { 121 this.ka = ka; 122 } 123 124 /** 125 * Returns the static gain in volts. 126 * 127 * @return The static gain in volts. 128 */ 129 public double getKs() { 130 return ks; 131 } 132 133 /** 134 * Returns the gravity gain in volts. 135 * 136 * @return The gravity gain in volts. 137 */ 138 public double getKg() { 139 return kg; 140 } 141 142 /** 143 * Returns the velocity gain in V/(m/s). 144 * 145 * @return The velocity gain. 146 */ 147 public double getKv() { 148 return kv; 149 } 150 151 /** 152 * Returns the acceleration gain in V/(m/s²). 153 * 154 * @return The acceleration gain. 155 */ 156 public double getKa() { 157 return ka; 158 } 159 160 /** 161 * Returns the period in seconds. 162 * 163 * @return The period in seconds. 164 */ 165 public double getDt() { 166 return m_dt; 167 } 168 169 /** 170 * Calculates the feedforward from the gains and velocity setpoint assuming continuous control 171 * (acceleration is assumed to be zero). 172 * 173 * @param velocity The velocity setpoint in meters per second. 174 * @return The computed feedforward. 175 */ 176 public double calculate(double velocity) { 177 return calculate(velocity, velocity); 178 } 179 180 /** 181 * Calculates the feedforward from the gains and setpoints assuming discrete control. 182 * 183 * <p>Note this method is inaccurate when the velocity crosses 0. 184 * 185 * @param currentVelocity The current velocity setpoint in meters per second. 186 * @param nextVelocity The next velocity setpoint in meters per second. 187 * @return The computed feedforward. 188 */ 189 public double calculate(double currentVelocity, double nextVelocity) { 190 // See wpimath/algorithms.md#Elevator_feedforward for derivation 191 if (ka < 1e-9) { 192 return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity; 193 } else { 194 double A = -kv / ka; 195 double B = 1.0 / ka; 196 double A_d = Math.exp(A * m_dt); 197 double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B; 198 return kg 199 + ks * Math.signum(currentVelocity) 200 + 1.0 / B_d * (nextVelocity - A_d * currentVelocity); 201 } 202 } 203 204 // Rearranging the main equation from the calculate() method yields the 205 // formulas for the methods below: 206 207 /** 208 * Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration. 209 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 210 * simultaneously achievable - enter the acceleration constraint, and this will give you a 211 * simultaneously-achievable velocity constraint. 212 * 213 * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. 214 * @param acceleration The acceleration of the elevator, in (m/s²). 215 * @return The maximum possible velocity in (m/s) at the given acceleration. 216 */ 217 public double maxAchievableVelocity(double maxVoltage, double acceleration) { 218 // Assume max velocity is positive 219 return (maxVoltage - ks - kg - acceleration * ka) / kv; 220 } 221 222 /** 223 * Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration. 224 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 225 * simultaneously achievable - enter the acceleration constraint, and this will give you a 226 * simultaneously-achievable velocity constraint. 227 * 228 * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. 229 * @param acceleration The acceleration of the elevator, in (m/s²). 230 * @return The maximum possible velocity in (m/s) at the given acceleration. 231 */ 232 public double minAchievableVelocity(double maxVoltage, double acceleration) { 233 // Assume min velocity is negative, ks flips sign 234 return (-maxVoltage + ks - kg - acceleration * ka) / kv; 235 } 236 237 /** 238 * Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity. 239 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 240 * simultaneously achievable - enter the velocity constraint, and this will give you a 241 * simultaneously-achievable acceleration constraint. 242 * 243 * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. 244 * @param velocity The velocity of the elevator, in (m/s) 245 * @return The maximum possible acceleration in (m/s²) at the given velocity. 246 */ 247 public double maxAchievableAcceleration(double maxVoltage, double velocity) { 248 return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / ka; 249 } 250 251 /** 252 * Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity. 253 * Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are 254 * simultaneously achievable - enter the velocity constraint, and this will give you a 255 * simultaneously-achievable acceleration constraint. 256 * 257 * @param maxVoltage The maximum voltage that can be supplied to the elevator, in volts. 258 * @param velocity The velocity of the elevator, in (m/s) 259 * @return The maximum possible acceleration in (m/s²) at the given velocity. 260 */ 261 public double minAchievableAcceleration(double maxVoltage, double velocity) { 262 return maxAchievableAcceleration(-maxVoltage, velocity); 263 } 264 265 /** ElevatorFeedforward struct for serialization. */ 266 public static final ElevatorFeedforwardStruct struct = new ElevatorFeedforwardStruct(); 267 268 /** ElevatorFeedforward protobuf for serialization. */ 269 public static final ElevatorFeedforwardProto proto = new ElevatorFeedforwardProto(); 270}