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; 009import edu.wpi.first.math.jni.ArmFeedforwardJNI; 010import edu.wpi.first.util.protobuf.ProtobufSerializable; 011import edu.wpi.first.util.struct.StructSerializable; 012 013/** 014 * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting 015 * against the force of gravity on a beam suspended at an angle). 016 */ 017public class ArmFeedforward implements ProtobufSerializable, StructSerializable { 018 /** The static gain, in volts. */ 019 private double ks; 020 021 /** The gravity gain, in volts. */ 022 private double kg; 023 024 /** The velocity gain, in V/(rad/s). */ 025 private double kv; 026 027 /** The acceleration gain, in V/(rad/s²). */ 028 private double ka; 029 030 /** The period, in seconds. */ 031 private final double m_dt; 032 033 /** 034 * Creates a new ArmFeedforward with the specified gains and period. 035 * 036 * @param ks The static gain in volts. 037 * @param kg The gravity gain in volts. 038 * @param kv The velocity gain in V/(rad/s). 039 * @param ka The acceleration gain in V/(rad/s²). 040 * @param dt The period in seconds. 041 * @throws IllegalArgumentException for kv < zero. 042 * @throws IllegalArgumentException for ka < zero. 043 * @throws IllegalArgumentException for period ≤ zero. 044 */ 045 public ArmFeedforward(double ks, double kg, double kv, double ka, double dt) { 046 this.ks = ks; 047 this.kg = kg; 048 this.kv = kv; 049 this.ka = ka; 050 if (kv < 0.0) { 051 throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); 052 } 053 if (ka < 0.0) { 054 throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!"); 055 } 056 if (dt <= 0.0) { 057 throw new IllegalArgumentException("period must be a positive number, got " + dt + "!"); 058 } 059 m_dt = dt; 060 } 061 062 /** 063 * Creates a new ArmFeedforward with the specified gains. The period is defaulted to 20 ms. 064 * 065 * @param ks The static gain in volts. 066 * @param kg The gravity gain in volts. 067 * @param kv The velocity gain in V/(rad/s). 068 * @param ka The acceleration gain in V/(rad/s²). 069 * @throws IllegalArgumentException for kv < zero. 070 * @throws IllegalArgumentException for ka < zero. 071 */ 072 public ArmFeedforward(double ks, double kg, double kv, double ka) { 073 this(ks, kg, kv, ka, 0.020); 074 } 075 076 /** 077 * Creates a new ArmFeedforward with the specified gains. 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/(rad/s). 082 * @throws IllegalArgumentException for kv < zero. 083 */ 084 public ArmFeedforward(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/(rad/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/(rad/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/(rad/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/(rad/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 position The position setpoint in radians. This angle should be measured from the 174 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 175 * your encoder does not follow this convention, an offset should be added. 176 * @param velocity The velocity setpoint in radians per second. 177 * @return The computed feedforward. 178 */ 179 public double calculate(double position, double velocity) { 180 return ks * Math.signum(velocity) + kg * Math.cos(position) + kv * velocity; 181 } 182 183 /** 184 * Calculates the feedforward from the gains and setpoints assuming continuous control. 185 * 186 * @param currentAngle The current angle in radians. This angle should be measured from the 187 * horizontal (i.e. if the provided angle is 0, the arm should be parallel to the floor). If 188 * your encoder does not follow this convention, an offset should be added. 189 * @param currentVelocity The current velocity setpoint in radians per second. 190 * @param nextVelocity The next velocity setpoint in radians per second. 191 * @return The computed feedforward in volts. 192 */ 193 public double calculate(double currentAngle, double currentVelocity, double nextVelocity) { 194 return ArmFeedforwardJNI.calculate( 195 ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, m_dt); 196 } 197 198 // Rearranging the main equation from the calculate() method yields the 199 // formulas for the methods below: 200 201 /** 202 * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an 203 * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 204 * profile are simultaneously achievable - enter the acceleration constraint, and this will give 205 * you a simultaneously-achievable velocity constraint. 206 * 207 * @param maxVoltage The maximum voltage that can be supplied to the arm. 208 * @param angle The angle of the arm, in radians. This angle should be measured from the 209 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 210 * your encoder does not follow this convention, an offset should be added. 211 * @param acceleration The acceleration of the arm, in (rad/s²). 212 * @return The maximum possible velocity in (rad/s) at the given acceleration and angle. 213 */ 214 public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) { 215 // Assume max velocity is positive 216 return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv; 217 } 218 219 /** 220 * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an 221 * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 222 * profile are simultaneously achievable - enter the acceleration constraint, and this will give 223 * you a simultaneously-achievable velocity constraint. 224 * 225 * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts. 226 * @param angle The angle of the arm, in radians. This angle should be measured from the 227 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 228 * your encoder does not follow this convention, an offset should be added. 229 * @param acceleration The acceleration of the arm, in (rad/s²). 230 * @return The minimum possible velocity in (rad/s) at the given acceleration and angle. 231 */ 232 public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) { 233 // Assume min velocity is negative, ks flips sign 234 return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv; 235 } 236 237 /** 238 * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and 239 * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 240 * profile are 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 arm, in volts. 244 * @param angle The angle of the arm, in radians. This angle should be measured from the 245 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 246 * your encoder does not follow this convention, an offset should be added. 247 * @param velocity The velocity of the elevator, in (rad/s) 248 * @return The maximum possible acceleration in (rad/s²) at the given velocity. 249 */ 250 public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { 251 return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka; 252 } 253 254 /** 255 * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and 256 * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 257 * profile are simultaneously achievable - enter the velocity constraint, and this will give you a 258 * simultaneously-achievable acceleration constraint. 259 * 260 * @param maxVoltage The maximum voltage that can be supplied to the arm, in volts. 261 * @param angle The angle of the arm, in radians. This angle should be measured from the 262 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 263 * your encoder does not follow this convention, an offset should be added. 264 * @param velocity The velocity of the elevator, in (rad/s) 265 * @return The maximum possible acceleration in (rad/s²) at the given velocity. 266 */ 267 public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { 268 return maxAchievableAcceleration(-maxVoltage, angle, velocity); 269 } 270 271 /** Arm feedforward struct for serialization. */ 272 public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct(); 273 274 /** Arm feedforward protobuf for serialization. */ 275 public static final ArmFeedforwardProto proto = new ArmFeedforwardProto(); 276}