[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;