From 3f1cf3cabe056a2fe601cfc65ee1aefb797b8730 Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 14 May 2026 21:50:38 -0700 Subject: [PATCH] [wpilib] ExHub Follower Fixes (#8892) A chunk of updates to followers on Expansion Hub Fixes followers not implicitly enabling. Allows followers to follow other followers Throws exceptions on construction if a follower cycle is detected. Allows reversing followers. The enable thing will fix https://github.com/wpilibsuite/SystemcoreTesting/discussions/259#discussioncomment-16886195 Closes #8843 Depends on https://github.com/wpilibsuite/scservices/pull/30 --- .../hardware/expansionhub/ExpansionHub.cpp | 59 +++++++++++++++++++ .../expansionhub/ExpansionHubMotor.cpp | 21 ++++++- .../hardware/expansionhub/ExpansionHub.hpp | 3 + .../expansionhub/ExpansionHubMotor.hpp | 16 ++++- .../python/semiwrap/ExpansionHubMotor.yml | 3 + .../cpp/hardware/ExpansionHubMotorTest.cpp | 37 ++++++++++++ .../hardware/expansionhub/ExpansionHub.java | 59 +++++++++++++++++++ .../expansionhub/ExpansionHubMotor.java | 30 +++++++++- .../expansionhub/ExpansionHubMotorTest.java | 37 ++++++++++++ 9 files changed, 260 insertions(+), 5 deletions(-) create mode 100644 wpilibc/src/test/native/cpp/hardware/ExpansionHubMotorTest.cpp create mode 100644 wpilibj/src/test/java/org/wpilib/hardware/expansionhub/ExpansionHubMotorTest.java diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp index cb3d42bf17..dd13933042 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include "wpi/framework/RobotBase.hpp" @@ -50,12 +51,20 @@ class ExpansionHub::DataStore { DataStore& operator=(DataStore&) = delete; DataStore& operator=(DataStore&&) = delete; + void validateFollowerConfiguration(); + void validateRootFollower(int baseChannel, int channel, + std::array& followerVisited); + std::string getFollowerStringCycle( + int baseChannel, std::array& followerVisited); + wpi::nt::BooleanSubscriber m_hubConnectedSubscriber; uint32_t m_reservedMotorMask{0}; uint32_t m_reservedServoMask{0}; wpi::util::mutex m_reservedLock; + std::optional followerConfiguration[NumMotorPorts]; + int m_usbId; }; @@ -124,9 +133,59 @@ void ExpansionHub::UnreserveMotor(int channel) { int mask = 1 << channel; std::scoped_lock lock{m_dataStore->m_reservedLock}; m_dataStore->m_reservedMotorMask &= ~mask; + m_dataStore->followerConfiguration[channel].reset(); } void ExpansionHub::ReportUsage(std::string_view device, std::string_view data) { HAL_ReportUsage( fmt::format("ExpansionHub[{}]/{}", m_dataStore->m_usbId, device), data); } + +std::string ExpansionHub::DataStore::getFollowerStringCycle( + int baseChannel, std::array& followerVisited) { + std::string result = fmt::format("{}", baseChannel); + int current = baseChannel; + while (followerVisited[current] != baseChannel) { + current = followerVisited[current]; + result += fmt::format(" -> {}", current); + } + result += fmt::format(" -> {}", followerVisited[current]); + return result; +} + +void ExpansionHub::DataStore::validateRootFollower( + int baseChannel, int channel, + std::array& followerVisited) { + if (followerVisited[channel] != -1) { + throw WPILIB_MakeError( + err::ParameterOutOfRange, "Follower cycle detected on hub {}: {}", + m_usbId, getFollowerStringCycle(baseChannel, followerVisited)); + } + auto leader = followerConfiguration[channel]; + if (!leader.has_value()) { + return; + } + followerVisited[channel] = *leader; + validateRootFollower(baseChannel, *leader, followerVisited); +} + +void ExpansionHub::DataStore::validateFollowerConfiguration() { + std::array followerVisited; + for (int i = 0; i < NumMotorPorts; i++) { + for (int j = 0; j < NumMotorPorts; j++) { + followerVisited[j] = -1; + } + validateRootFollower(i, i, followerVisited); + } +} + +void ExpansionHub::AddFollower(int leaderChannel, int followerChannel) { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->followerConfiguration[followerChannel] = leaderChannel; + m_dataStore->validateFollowerConfiguration(); +} + +void ExpansionHub::RemoveFollower(int followerChannel) { + std::scoped_lock lock{m_dataStore->m_reservedLock}; + m_dataStore->followerConfiguration[followerChannel].reset(); +} diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp index 49a12e6e5a..266133ce2a 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubMotor.cpp @@ -158,11 +158,28 @@ ExpansionHubPositionConstants& ExpansionHubMotor::GetPositionConstants() { return m_positionConstants; } -void ExpansionHubMotor::Follow(const ExpansionHubMotor& leader) { +void ExpansionHubMotor::Follow(const ExpansionHubMotor& leader, + FollowDirection direction) { if (m_hub.GetUsbId() != leader.m_hub.GetUsbId()) { throw WPILIB_MakeError(err::InvalidParameter, "Cannot follow motor on different hub"); } + if (m_channel == leader.m_channel) { + throw WPILIB_MakeError(err::InvalidParameter, "Cannot follow self"); + } + m_hub.AddFollower(leader.m_channel, m_channel); + SetEnabled(true); m_modePublisher.Set(kFollowerMode); - m_setpointPublisher.Set(leader.m_channel); + if (direction == FollowDirection::Opposed) { + m_setpointPublisher.Set(leader.m_channel + 4); + } else { + m_setpointPublisher.Set(leader.m_channel); + } +} + +void ExpansionHubMotor::Unfollow() { + m_hub.RemoveFollower(m_channel); + SetEnabled(false); + m_modePublisher.Set(kPercentageMode); + m_setpointPublisher.Set(0); } diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp index deded587cc..ae5fd90b28 100644 --- a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHub.hpp @@ -79,6 +79,9 @@ class ExpansionHub { bool CheckAndReserveMotor(int channel); void UnreserveMotor(int channel); + void AddFollower(int leaderChannel, int followerChannel); + void RemoveFollower(int followerChannel); + void ReportUsage(std::string_view device, std::string_view data); class DataStore; diff --git a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp index 58b15c1662..fed8c8b7dd 100644 --- a/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp +++ b/wpilibc/src/main/native/include/wpi/hardware/expansionhub/ExpansionHubMotor.hpp @@ -19,6 +19,14 @@ namespace wpi { * ExpansionHub. */ class ExpansionHubMotor { public: + /** The direction to follow a leader motor in when using the follow method. */ + enum class FollowDirection { + /** Follow the leader motor in the same direction. */ + Aligned, + /** Follow the leader motor in the opposite direction. */ + Opposed + }; + /** * Constructs a servo at the requested channel on a specific USB port. * @@ -144,8 +152,14 @@ class ExpansionHubMotor { * Additionally, the direction of both motors will be the same. * * @param leader The motor to follow + * @param direction The direction to follow the leader */ - void Follow(const ExpansionHubMotor& leader); + void Follow(const ExpansionHubMotor& leader, FollowDirection direction); + + /** + * Stops following the currently set leader motor. + */ + void Unfollow(); private: ExpansionHub m_hub; diff --git a/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml b/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml index e9f030494f..0f4a31068d 100644 --- a/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml +++ b/wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml @@ -18,3 +18,6 @@ classes: Follow: GetVelocityConstants: GetPositionConstants: + Unfollow: + enums: + FollowDirection: diff --git a/wpilibc/src/test/native/cpp/hardware/ExpansionHubMotorTest.cpp b/wpilibc/src/test/native/cpp/hardware/ExpansionHubMotorTest.cpp new file mode 100644 index 0000000000..44cdf2e0f6 --- /dev/null +++ b/wpilibc/src/test/native/cpp/hardware/ExpansionHubMotorTest.cpp @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/hardware/expansionhub/ExpansionHubMotor.hpp" + +#include + +#include "wpi/hal/HAL.h" +#include "wpi/system/Errors.hpp" + +namespace wpi { +TEST(ExpansionHubMotorTest, FollowerLoopDetection) { + HAL_Initialize(500, 0); + + wpi::ExpansionHubMotor motor0{0, 0}; + wpi::ExpansionHubMotor motor1{0, 1}; + wpi::ExpansionHubMotor motor2{0, 2}; + + // Test that a simple loop is detected + motor1.Follow(motor2, wpi::ExpansionHubMotor::FollowDirection::Opposed); + motor2.Follow(motor0, wpi::ExpansionHubMotor::FollowDirection::Opposed); + EXPECT_THROW( + motor0.Follow(motor1, wpi::ExpansionHubMotor::FollowDirection::Opposed), + wpi::RuntimeError); +} +TEST(ExpansionHubMotorTest, Follower) { + HAL_Initialize(500, 0); + + wpi::ExpansionHubMotor motor0{0, 0}; + wpi::ExpansionHubMotor motor1{0, 1}; + wpi::ExpansionHubMotor motor2{0, 2}; + + motor1.Follow(motor2, wpi::ExpansionHubMotor::FollowDirection::Opposed); + motor2.Follow(motor0, wpi::ExpansionHubMotor::FollowDirection::Opposed); +} +} // namespace wpi diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java index f7e931aa26..c76ac1c8b1 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java @@ -4,6 +4,7 @@ package org.wpilib.hardware.expansionhub; +import java.util.OptionalInt; import org.wpilib.framework.RobotBase; import org.wpilib.hardware.hal.HAL; import org.wpilib.networktables.BooleanSubscriber; @@ -20,6 +21,9 @@ public class ExpansionHub implements AutoCloseable { private int m_reservedServoMask; private final Object m_reserveLock = new Object(); + private final OptionalInt[] followerConfiguration = new OptionalInt[4]; + private final int[] followerVisited = new int[4]; + private final BooleanSubscriber m_hubConnectedSubscriber; DataStore(int usbId) { @@ -31,6 +35,10 @@ public class ExpansionHub implements AutoCloseable { m_hubConnectedSubscriber = systemServer.getBooleanTopic("/rhsp/" + usbId + "/connected").subscribe(false); + for (int i = 0; i < followerConfiguration.length; i++) { + followerConfiguration[i] = OptionalInt.empty(); + } + // Wait up to half a second for connected to come up, using a poll loop to // ensure we don't block. if (RobotBase.isReal()) { @@ -150,6 +158,7 @@ public class ExpansionHub implements AutoCloseable { void unreserveMotor(int channel) { int mask = 1 << channel; synchronized (m_dataStore.m_reserveLock) { + m_dataStore.followerConfiguration[channel] = OptionalInt.empty(); m_dataStore.m_reservedMotorMask &= ~mask; } } @@ -203,6 +212,56 @@ public class ExpansionHub implements AutoCloseable { return m_dataStore.m_hubConnectedSubscriber.get(false); } + private String getFollowerStringCycle(int baseChannel, int[] followerVisited) { + StringBuilder sb = new StringBuilder(); + sb.append(baseChannel); + int current = baseChannel; + while (followerVisited[current] != baseChannel) { + current = followerVisited[current]; + sb.append(" -> ").append(current); + } + sb.append(" -> ").append(followerVisited[current]); + return sb.toString(); + } + + private void validateRootFollower(int baseChannel, int channel, int[] followerVisited) { + if (followerVisited[channel] != -1) { + throw new IllegalStateException( + "Follower cycle detected on hub " + + m_dataStore.m_usbId + + ": " + + getFollowerStringCycle(baseChannel, followerVisited)); + } + OptionalInt leader = m_dataStore.followerConfiguration[channel]; + if (leader.isEmpty()) { + return; + } + followerVisited[channel] = leader.getAsInt(); + validateRootFollower(baseChannel, leader.getAsInt(), followerVisited); + } + + private void validateFollowerConfiguration() { + for (int i = 0; i < m_dataStore.followerConfiguration.length; i++) { + for (int j = 0; j < m_dataStore.followerVisited.length; j++) { + m_dataStore.followerVisited[j] = -1; + } + validateRootFollower(i, i, m_dataStore.followerVisited); + } + } + + void addFollower(int leaderChannel, int followerChannel) { + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.followerConfiguration[followerChannel] = OptionalInt.of(leaderChannel); + validateFollowerConfiguration(); + } + } + + void removeFollower(int followerChannel) { + synchronized (m_dataStore.m_reserveLock) { + m_dataStore.followerConfiguration[followerChannel] = OptionalInt.empty(); + } + } + /** * Gets the USB ID of this hub. * diff --git a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java index 911b26c1fe..c61c37c1c5 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHubMotor.java @@ -21,6 +21,14 @@ import org.wpilib.units.measure.Voltage; /** This class controls a specific motor and encoder hooked up to an ExpansionHub. */ public class ExpansionHubMotor implements AutoCloseable { + /** The direction to follow a leader motor in when using the follow method. */ + public enum FollowDirection { + /** Follow the leader motor in the same direction. */ + Aligned, + /** Follow the leader motor in the opposite direction. */ + Opposed + } + private static final int kPercentageMode = 0; private static final int kVoltageMode = 1; private static final int kPositionMode = 2; @@ -299,13 +307,31 @@ public class ExpansionHubMotor implements AutoCloseable { * of both motors will be the same. * * @param leader The motor to follow + * @param direction The direction to follow the leader */ - public void follow(ExpansionHubMotor leader) { + public void follow(ExpansionHubMotor leader, FollowDirection direction) { requireNonNullParam(leader, "leader", "follow"); if (leader.m_hub.getUsbId() != this.m_hub.getUsbId()) { throw new IllegalArgumentException("Leader motor must be on the same hub as the follower"); } + if (leader.m_channel == this.m_channel) { + throw new IllegalArgumentException("Cannot follow self"); + } + m_hub.addFollower(leader.m_channel, this.m_channel); + setEnabled(true); m_modePublisher.set(kFollowerMode); - m_setpointPublisher.set(leader.m_channel); + if (direction == FollowDirection.Opposed) { + m_setpointPublisher.set(leader.m_channel + 4); + } else { + m_setpointPublisher.set(leader.m_channel); + } + } + + /** Stops following the currently set leader motor. */ + public void unfollow() { + m_hub.removeFollower(this.m_channel); + setEnabled(false); + m_modePublisher.set(kPercentageMode); + m_setpointPublisher.set(0); } } diff --git a/wpilibj/src/test/java/org/wpilib/hardware/expansionhub/ExpansionHubMotorTest.java b/wpilibj/src/test/java/org/wpilib/hardware/expansionhub/ExpansionHubMotorTest.java new file mode 100644 index 0000000000..02f7cb3929 --- /dev/null +++ b/wpilibj/src/test/java/org/wpilib/hardware/expansionhub/ExpansionHubMotorTest.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.expansionhub; + +import static org.junit.jupiter.api.Assertions.assertThrows; + +import org.junit.jupiter.api.Test; +import org.wpilib.hardware.expansionhub.ExpansionHubMotor.FollowDirection; +import org.wpilib.hardware.hal.HAL; + +class ExpansionHubMotorTest { + @Test + void testFollower() { + HAL.initialize(500, 0); + try (ExpansionHubMotor motor0 = new ExpansionHubMotor(0, 0); + ExpansionHubMotor motor1 = new ExpansionHubMotor(0, 1); + ExpansionHubMotor motor2 = new ExpansionHubMotor(0, 2); ) { + motor1.follow(motor2, FollowDirection.Opposed); + motor2.follow(motor0, FollowDirection.Opposed); + } + } + + @Test + void testFollowerCycle() { + HAL.initialize(500, 0); + try (ExpansionHubMotor motor0 = new ExpansionHubMotor(0, 0); + ExpansionHubMotor motor1 = new ExpansionHubMotor(0, 1); + ExpansionHubMotor motor2 = new ExpansionHubMotor(0, 2); ) { + motor1.follow(motor2, FollowDirection.Opposed); + motor2.follow(motor0, FollowDirection.Opposed); + assertThrows( + IllegalStateException.class, () -> motor0.follow(motor1, FollowDirection.Opposed)); + } + } +}