add gravity feedforward to exhub motors by zachwaffle4 · Pull Request #29 · wpilibsuite/scservices

zachwaffle4 · 2026-05-12T18:34:55Z

…m ratio

# Conflicts: # expansionhub/PositionController.cpp

Copilot

Pull request overview

Adds gravity-feedforward support (lift, arm, and arm-ratio constants) to expansion hub position-mode motors. New kgLift/kgArm/kgArmRatio NT entries are published from the Java and Python client APIs and consumed by the C++ PositionController to add a gravity-compensation voltage term on top of the existing PID + simple feedforward output. The Python side also renames internal attributes (*PidConstants → *Constants), and ExpansionHubServo.java is migrated to the new org.wpilib.* package paths (matching the linked allwpilib PR).

Changes:

New setGLift / setGArm / setGArmRatio setters and corresponding NT publishers in both Java (ExpansionHubPositionConstants) and Python (expansion_hub.py).
PositionController (C++) adds three new subscribers and computes a gravity-compensation voltage (kgLift, or kgArm * cos(measurement * kgArmRatio * 2π)), added to the PID + feedforward output.
ExpansionHubServo.java imports migrated from edu.wpi.first.* to org.wpilib.*.

Reviewed changes

Copilot reviewed 5 out of 5 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
expansionhub/wpilib/python/expansion_hub.py	Adds `kgLift`/`kgArm`/`kgArmRatio` publishers and setters; renames `PidConstants` attributes to `Constants`.
expansionhub/wpilib/java/ExpansionHubServo.java	Migrates imports to `org.wpilib.*` package namespace.
expansionhub/wpilib/java/ExpansionHubPositionConstants.java	Adds gravity-compensation publishers, setters, and `close()` cleanup.
expansionhub/PositionController.h	Declares three new gravity-compensation NT subscribers.
expansionhub/PositionController.cpp	Subscribes to the new constants and adds a gravity-compensation voltage term to the controller output.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

Copilot

Pull request overview

Copilot reviewed 5 out of 5 changed files in this pull request and generated 7 comments.

+    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: 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(cos) > kGravityCompensationZeroTolerance) {
+        const double armAngleRadians =
+            measurement * cosRatio * 2.0 * std::numbers::pi;
+        gravityCompensation = cos * std::cos(armAngleRadians);
+    }


+    gSubscriber =
+        instance
+            .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" + motorNum +
+                            "/constants/position/kg")
+            .Subscribe(0, options);
+
+    cosSubscriber =
+        instance
+            .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" + motorNum +
+                            "/constants/position/kcos")
+            .Subscribe(0, options);
+
+    cosRatioSubscriber =
+        instance
+            .GetDoubleTopic("/rhsp/" + busIdStr + "/motor" + motorNum +
+                            "/constants/position/kcosRatio")
+            .Subscribe(0, options);


+import org.wpilib.math.util.MathUtil;
+import org.wpilib.networktables.BooleanPublisher;
+import org.wpilib.networktables.BooleanSubscriber;
+import org.wpilib.networktables.IntegerPublisher;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.system.SystemServer;


+        self.velocityConstants = ExpansionHubVelocityConstants(hubNumber, motorNumber)
+        self.positionConstants = ExpansionHubPositionConstants(hubNumber, motorNumber)


    def getVelocityPidConstants(self) -> ExpansionHubVelocityConstants:
-        return self.velocityPidConstants
+        return self.velocityConstants

    def getPositionPidConstants(self) -> ExpansionHubPositionConstants:
-        return self.positionPidConstants
+        return self.positionConstants


+    const double cos = cosSubscriber.Get(0);
+    const double cosRatio = cosRatioSubscriber.Get(0);


+        std::abs(cos) > kGravityCompensationZeroTolerance) {
+        const double armAngleRadians =
+            measurement * cosRatio * 2.0 * std::numbers::pi;
+        gravityCompensation = cos * std::cos(armAngleRadians);


ThadHouse · 2026-06-02T20:19:31Z

@codex review

…m ratio

chatgpt-codex-connector

💡 Codex Review

Here are some automated review suggestions for this pull request.

Reviewed commit: 63d80bd6d0

ℹ️ About Codex in GitHub

Your team has set up Codex to review pull requests in this repo. Reviews are triggered when you

Open a pull request for review
Mark a draft as ready
Comment "@codex review".

If Codex has suggestions, it will comment; otherwise it will react with 👍.

Codex can also answer questions or update the PR. Try commenting "@codex address that feedback".

chatgpt-codex-connector · 2026-06-02T20:21:31Z

+    m_gLiftPublisher =
+        systemServer
+            .getDoubleTopic(
+                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/position" + "/kgLift")


Align gravity constant topics with daemon subscribers

When callers use the new Java/Python setGLift()/setGArm() APIs, these publishers write to /kgLift, /kgArm, and /kgArmRatio, but the daemon added in this commit subscribes to /constants/position/kg, /kcos, and /kcosRatio in PositionController::Initialize. Because NetworkTables topics must match exactly, the new feedforward constants stay at their default 0, so gravity compensation is never applied for normal client usage.

Useful? React with 👍 / 👎.

this is why I don't like having the java and python classes randomly copied in here; they are correct in the corresponding allwpilib pr

You can delete them from here. Its not important to have anymore now that this is in WPILib.

there we go

zachwaffle4 · 2026-06-02T20:36:25Z

@codex review

chatgpt-codex-connector · 2026-06-02T20:36:31Z

To use Codex here, create a Codex account and connect to github.

zachwaffle4 · 2026-06-02T20:38:37Z

@codex review (take 2)

chatgpt-codex-connector · 2026-06-02T20:38:43Z

To use Codex here, create a Codex account and connect to github.

zachwaffle4 · 2026-06-02T20:38:57Z

:(

zachwaffle4 · 2026-06-02T20:40:14Z

@codex review please

chatgpt-codex-connector · 2026-06-02T20:43:15Z

Codex Review: Didn't find any major issues. Hooray!

ℹ️ About Codex in GitHub

Your team has set up Codex to review pull requests in this repo. Reviews are triggered when you

Open a pull request for review
Mark a draft as ready
Comment "@codex review".

If Codex has suggestions, it will comment; otherwise it will react with 👍.

Codex can also answer questions or update the PR. Try commenting "@codex address that feedback".

zachwaffle4 added 2 commits May 12, 2026 14:33

Add gravity compensation publishers and methods for lift, arm, and ar…

000ae50

…m ratio

Merge remote-tracking branch 'upstream/main' into exhub-feedforward

8243c2b

# Conflicts: # expansionhub/PositionController.cpp

zachwaffle4 mentioned this pull request May 12, 2026

[wpilib] add gravity compensation methods to expansion hub position constants wpilibsuite/allwpilib#8890

Open

ThadHouse requested a review from Copilot May 15, 2026 05:00

Copilot started reviewing on behalf of ThadHouse May 15, 2026 05:00 View session

Copilot AI reviewed May 15, 2026

View reviewed changes

Comment thread expansionhub/wpilib/java/ExpansionHubServo.java Outdated

Comment thread expansionhub/wpilib/java/ExpansionHubPositionConstants.java Outdated

Comment thread expansionhub/PositionController.cpp Outdated

Potential fix for pull request finding

3202ad2

Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>

zachwaffle4 requested a review from Copilot June 2, 2026 20:17

Copilot AI reviewed Jun 2, 2026

View reviewed changes

Add gravity compensation publishers and methods for lift, arm, and ar…

05c4a50

…m ratio

zachwaffle4 force-pushed the exhub-feedforward branch from 63d80bd to 05c4a50 Compare June 2, 2026 20:21

chatgpt-codex-connector Bot reviewed Jun 2, 2026

View reviewed changes

delete the java and python files

00a85fa

		self.velocityConstants = ExpansionHubVelocityConstants(hubNumber, motorNumber)
		self.positionConstants = ExpansionHubPositionConstants(hubNumber, motorNumber)

		const double cos = cosSubscriber.Get(0);
		const double cosRatio = cosRatioSubscriber.Get(0);

Uh oh!

Conversation

zachwaffle4 commented May 12, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Uh oh!

ThadHouse commented Jun 2, 2026

Uh oh!

chatgpt-codex-connector Bot left a comment

Choose a reason for hiding this comment

💡 Codex Review

Uh oh!

chatgpt-codex-connector Bot Jun 2, 2026

Choose a reason for hiding this comment

Uh oh!

zachwaffle4 Jun 2, 2026

Choose a reason for hiding this comment

Uh oh!

ThadHouse Jun 2, 2026

Choose a reason for hiding this comment

Uh oh!

zachwaffle4 Jun 2, 2026

Choose a reason for hiding this comment

Uh oh!

zachwaffle4 commented Jun 2, 2026

Uh oh!

chatgpt-codex-connector Bot commented Jun 2, 2026

Uh oh!

zachwaffle4 commented Jun 2, 2026

Uh oh!

chatgpt-codex-connector Bot commented Jun 2, 2026

Uh oh!

zachwaffle4 commented Jun 2, 2026

Uh oh!

zachwaffle4 commented Jun 2, 2026

Uh oh!

chatgpt-codex-connector Bot commented Jun 2, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants