[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

wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp

@@ -6,6 +6,7 @@
 
 #include <chrono>
 #include <memory>
+#include <string>
 #include <thread>
 
 #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<int, NumMotorPorts>& followerVisited);
+  std::string getFollowerStringCycle(
+      int baseChannel, std::array<int, NumMotorPorts>& followerVisited);
+
   wpi::nt::BooleanSubscriber m_hubConnectedSubscriber;
 
   uint32_t m_reservedMotorMask{0};
   uint32_t m_reservedServoMask{0};
   wpi::util::mutex m_reservedLock;
 
+  std::optional<int> 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<int, NumMotorPorts>& 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<int, NumMotorPorts>& 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<int, NumMotorPorts> 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();
+}

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);
 }

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;

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;

wpilibc/src/main/python/semiwrap/ExpansionHubMotor.yml

@@ -18,3 +18,6 @@ classes:
       Follow:
       GetVelocityConstants:
       GetPositionConstants:
+      Unfollow:
+    enums:
+      FollowDirection:

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 <gtest/gtest.h>
+
+#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

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.
    *

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);
   }
 }

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));
+    }
+  }
+}