[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

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