Add gravity compensation publishers and methods for lift, arm, and arm ratio

Author: zachwaffle4

expansionhub/PositionController.cpp

@@ -20,20 +20,20 @@ double PositionController::Compute(double setpoint, double measurement) {
 
     feedForward.SetKs(units::volt_t{sSubscriber.Get(0)});
 
-    const double gLift = gLiftSubscriber.Get(0);
-    const double gArm = gArmSubscriber.Get(0);
-    const double gArmRatio = gArmRatioSubscriber.Get(0);
+    const double g = gSubscriber.Get(0);
+    const double cos = cosSubscriber.Get(0);
+    const double cosRatio = cosRatioSubscriber.Get(0);
     constexpr double kGravityCompensationZeroTolerance = 1e-9;
 
-    // Precedence rule: kgLift wins when it is configured to a nonzero value.
-    // Only when kgLift is effectively zero do we fall back to arm-style
-    // gravity compensation based on kgArm and kgArmRatio.
-    double gravityCompensation = gLift;
+    // Precedence rule: g wins when it is configured to a nonzero value.
+    // Only when g is effectively zero do we fall back to arm-style
+    // gravity compensation based on cos and cosRatio.
+    double gravityCompensation = g;
     if (std::abs(gravityCompensation) <= kGravityCompensationZeroTolerance &&
-        std::abs(gArm) > kGravityCompensationZeroTolerance) {
+        std::abs(cos) > kGravityCompensationZeroTolerance) {
         const double armAngleRadians =
-            measurement * gArmRatio * 2.0 * std::numbers::pi;
-        gravityCompensation = gArm * std::cos(armAngleRadians);
+            measurement * cosRatio * 2.0 * std::numbers::pi;
+        gravityCompensation = cos * std::cos(armAngleRadians);
     }
 
     return (feedForward.Calculate(
@@ -84,21 +84,21 @@ void PositionController::Initialize(
                             "/constants/position/continuousMaximum")
             .Subscribe(false, options);
 
-    gLiftSubscriber =
+    gSubscriber =
         instance
             .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" + motorNum +
-                            "/constants/position/kgLift")
+                            "/constants/position/kg")
             .Subscribe(0, options);
 
-    gArmSubscriber =
+    cosSubscriber =
         instance
             .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" + motorNum +
-                            "/constants/position/kgArm")
+                            "/constants/position/kcos")
             .Subscribe(0, options);
 
-    gArmRatioSubscriber =
+    cosRatioSubscriber =
         instance
             .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" + motorNum +
-                            "/constants/position/kgArmRatio")
+                            "/constants/position/kcosRatio")
             .Subscribe(0, options);
 }

expansionhub/PositionController.h

@@ -25,9 +25,9 @@ struct PositionController {
     wpi::nt::DoubleSubscriber continuousMinimumSubscriber;
     wpi::nt::DoubleSubscriber continuousMaximumSubscriber;
 
-    wpi::nt::DoubleSubscriber gLiftSubscriber;
-    wpi::nt::DoubleSubscriber gArmSubscriber;
-    wpi::nt::DoubleSubscriber gArmRatioSubscriber;
+    wpi::nt::DoubleSubscriber gSubscriber;
+    wpi::nt::DoubleSubscriber cosSubscriber;
+    wpi::nt::DoubleSubscriber cosRatioSubscriber;
 
     wpi::math::PIDController pidController{0, 0, 0, eh::Period};
     wpi::math::SimpleMotorFeedforward<wpi::units::meter> feedForward{eh::Ks, 0_V / 1_mps, 0_V / 1_mps_sq, eh::Period};