diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp index 8618aaaa0f..c5fb3b2e96 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp @@ -15,7 +15,7 @@ ArmSubsystem::ArmSubsystem() kP, 0, 0, {kMaxVelocity, kMaxAcceleration})), m_motor(kMotorPort), m_encoder(kEncoderPorts[0], kEncoderPorts[1]), - m_feedforward(kS, kCos, kV, kA) { + m_feedforward(kS, kG, kV, kA) { m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value()); // Start arm in neutral position SetGoal(State{kArmOffset, 0_rad_per_s}); diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h index 16a4e00d29..bd432eea05 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h @@ -46,7 +46,7 @@ constexpr double kP = 1; // These are fake gains; in actuality these must be determined individually for // each robot constexpr auto kS = 1_V; -constexpr auto kCos = 1_V; +constexpr auto kG = 1_V; constexpr auto kV = 0.5_V * 1_s / 1_rad; constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp index f80970aa16..7557b5ae58 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp @@ -13,7 +13,7 @@ ArmSubsystem::ArmSubsystem() : frc2::TrapezoidProfileSubsystem( {kMaxVelocity, kMaxAcceleration}, kArmOffset), m_motor(kMotorPort), - m_feedforward(kS, kCos, kV, kA) { + m_feedforward(kS, kG, kV, kA) { m_motor.SetPID(kP, 0, 0); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h index 16a4e00d29..bd432eea05 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h @@ -46,7 +46,7 @@ constexpr double kP = 1; // These are fake gains; in actuality these must be determined individually for // each robot constexpr auto kS = 1_V; -constexpr auto kCos = 1_V; +constexpr auto kG = 1_V; constexpr auto kV = 0.5_V * 1_s / 1_rad; constexpr auto kA = 0.1_V * 1_s * 1_s / 1_rad; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java index c7f23de1ca..fe72d3db69 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java @@ -38,7 +38,7 @@ public final class Constants { // These are fake gains; in actuality these must be determined individually for each robot public static final double kSVolts = 1; - public static final double kCosVolts = 1; + public static final double kGVolts = 1; public static final double kVVoltSecondPerRad = 0.5; public static final double kAVoltSecondSquaredPerRad = 0.1; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java index 298b2d52f3..43beedc0a2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java @@ -19,7 +19,7 @@ public class ArmSubsystem extends ProfiledPIDSubsystem { new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]); private final ArmFeedforward m_feedforward = new ArmFeedforward( - ArmConstants.kSVolts, ArmConstants.kCosVolts, + ArmConstants.kSVolts, ArmConstants.kGVolts, ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad); /** Create a new ArmSubsystem. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java index 4fd17f191b..fb003aaeb5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java @@ -38,7 +38,7 @@ public final class Constants { // These are fake gains; in actuality these must be determined individually for each robot public static final double kSVolts = 1; - public static final double kCosVolts = 1; + public static final double kGVolts = 1; public static final double kVVoltSecondPerRad = 0.5; public static final double kAVoltSecondSquaredPerRad = 0.1; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java index 152700e367..9e70402735 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java @@ -16,7 +16,7 @@ public class ArmSubsystem extends TrapezoidProfileSubsystem { new ExampleSmartMotorController(ArmConstants.kMotorPort); private final ArmFeedforward m_feedforward = new ArmFeedforward( - ArmConstants.kSVolts, ArmConstants.kCosVolts, + ArmConstants.kSVolts, ArmConstants.kGVolts, ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad); /** Create a new ArmSubsystem. */ diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index eb7cb76bbd..1f0e4077a3 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -35,14 +35,14 @@ class WPILIB_DLLEXPORT ArmFeedforward { * Creates a new ArmFeedforward with the specified gains. * * @param kS The static gain, in volts. - * @param kCos The gravity gain, in volts. + * @param kG The gravity gain, in volts. * @param kV The velocity gain, in volt seconds per radian. * @param kA The acceleration gain, in volt seconds^2 per radian. */ constexpr ArmFeedforward( - units::volt_t kS, units::volt_t kCos, units::unit_t kV, + units::volt_t kS, units::volt_t kG, units::unit_t kV, units::unit_t kA = units::unit_t(0)) - : kS(kS), kCos(kCos), kV(kV), kA(kA) {} + : kS(kS), kG(kG), kV(kV), kA(kA) {} /** * Calculates the feedforward from the gains and setpoints. @@ -56,7 +56,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::unit_t velocity, units::unit_t acceleration = units::unit_t(0)) const { - return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) + + return kS * wpi::sgn(velocity) + kG * units::math::cos(angle) + kV * velocity + kA * acceleration; } @@ -79,7 +79,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::volt_t maxVoltage, units::unit_t angle, units::unit_t acceleration) { // Assume max velocity is positive - return (maxVoltage - kS - kCos * units::math::cos(angle) - + return (maxVoltage - kS - kG * units::math::cos(angle) - kA * acceleration) / kV; } @@ -100,7 +100,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::volt_t maxVoltage, units::unit_t angle, units::unit_t acceleration) { // Assume min velocity is negative, ks flips sign - return (-maxVoltage + kS - kCos * units::math::cos(angle) - + return (-maxVoltage + kS - kG * units::math::cos(angle) - kA * acceleration) / kV; } @@ -121,7 +121,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { units::volt_t maxVoltage, units::unit_t angle, units::unit_t velocity) { return (maxVoltage - kS * wpi::sgn(velocity) - - kCos * units::math::cos(angle) - kV * velocity) / + kG * units::math::cos(angle) - kV * velocity) / kA; } @@ -144,7 +144,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { } units::volt_t kS{0}; - units::volt_t kCos{0}; + units::volt_t kG{0}; units::unit_t kV{0}; units::unit_t kA{0}; };