diff --git a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp index 4d049b1fa3..9730ef1880 100644 --- a/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp +++ b/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp @@ -4,7 +4,9 @@ #include "wpi/hardware/expansionhub/ExpansionHub.hpp" +#include #include +#include #include "wpi/hal/UsageReporting.hpp" #include "wpi/hardware/expansionhub/ExpansionHubMotor.hpp" @@ -12,6 +14,7 @@ #include "wpi/nt/BooleanTopic.hpp" #include "wpi/system/Errors.hpp" #include "wpi/system/SystemServer.hpp" +#include "wpi/system/Timer.hpp" using namespace wpi; @@ -26,6 +29,16 @@ class ExpansionHub::DataStore { m_hubConnectedSubscriber = systemServer.GetBooleanTopic(fmt::format("/rhsp/{}/connected", usbId)) .Subscribe(false); + + // Wait up to half a second for connected to come up, using a poll loop to + // ensure we don't block. + auto startTime = Timer::GetMonotonicTimestamp(); + while (Timer::GetMonotonicTimestamp() - startTime < 0.5_s) { + if (m_hubConnectedSubscriber.Get(false)) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } } DataStore(DataStore&) = delete; 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 c205505984..f17102093b 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java @@ -8,6 +8,7 @@ import org.wpilib.hardware.hal.HAL; import org.wpilib.networktables.BooleanSubscriber; import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.system.SystemServer; +import org.wpilib.system.Timer; /** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */ public class ExpansionHub implements AutoCloseable { @@ -28,6 +29,16 @@ public class ExpansionHub implements AutoCloseable { m_hubConnectedSubscriber = systemServer.getBooleanTopic("/rhsp/" + usbId + "/connected").subscribe(false); + + // Wait up to half a second for connected to come up, using a poll loop to + // ensure we don't block. + double startTime = Timer.getMonotonicTimestamp(); + while (Timer.getMonotonicTimestamp() - startTime < 0.5) { + if (m_hubConnectedSubscriber.get(false)) { + break; + } + Timer.delay(0.01); + } } @Override