diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index c98f46c8ad..2e9a8d8434 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -10,7 +10,7 @@ package edu.wpi.first.math.controller; */ public class ArmFeedforward { public final double ks; - public final double kcos; + public final double kg; public final double kv; public final double ka; @@ -19,13 +19,13 @@ public class ArmFeedforward { * units of the computed feedforward. * * @param ks The static gain. - * @param kcos The gravity gain. + * @param kg The gravity gain. * @param kv The velocity gain. * @param ka The acceleration gain. */ - public ArmFeedforward(double ks, double kcos, double kv, double ka) { + public ArmFeedforward(double ks, double kg, double kv, double ka) { this.ks = ks; - this.kcos = kcos; + this.kg = kg; this.kv = kv; this.ka = ka; } @@ -35,11 +35,11 @@ public class ArmFeedforward { * Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. - * @param kcos The gravity gain. + * @param kg The gravity gain. * @param kv The velocity gain. */ - public ArmFeedforward(double ks, double kcos, double kv) { - this(ks, kcos, kv, 0); + public ArmFeedforward(double ks, double kg, double kv) { + this(ks, kg, kv, 0); } /** @@ -55,7 +55,7 @@ public class ArmFeedforward { public double calculate( double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) { return ks * Math.signum(velocityRadPerSec) - + kcos * Math.cos(positionRadians) + + kg * Math.cos(positionRadians) + kv * velocityRadPerSec + ka * accelRadPerSecSquared; } @@ -92,7 +92,7 @@ public class ArmFeedforward { */ public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) { // Assume max velocity is positive - return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / kv; + return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv; } /** @@ -110,7 +110,7 @@ public class ArmFeedforward { */ public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) { // Assume min velocity is negative, ks flips sign - return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / kv; + return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv; } /** @@ -127,7 +127,7 @@ public class ArmFeedforward { * @return The maximum possible acceleration at the given velocity. */ public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { - return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / ka; + return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka; } /**