diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt index aee2f48d..b1900965 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt @@ -40,6 +40,10 @@ object RobotController : OnCreateEventLoop, OpModeManagerNotifier.Notifications hardwareObjects += hardware } + private fun resetHardware() { + synchronized(hardwareObjects) { hardwareObjects.toList() }.forEach { it.reset() } + } + /** * Application context captured when the event loop is created. * @@ -145,7 +149,9 @@ object RobotController : OnCreateEventLoop, OpModeManagerNotifier.Notifications ftcEventLoop.opModeManager.registerListener(this) } - override fun onOpModePreInit(opMode: OpMode?) {} + override fun onOpModePreInit(opMode: OpMode?) { + resetHardware() + } override fun onOpModePreStart(opMode: OpMode?) {} @@ -153,6 +159,6 @@ object RobotController : OnCreateEventLoop, OpModeManagerNotifier.Notifications * Discards all cached hardware objects so they are re-initialized in the next op mode. */ override fun onOpModePostStop(opMode: OpMode?) { - synchronized(hardwareObjects) { hardwareObjects.toList() }.forEach { it.reset() } + resetHardware() } } diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt index 5f7c344e..733a253c 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt @@ -52,13 +52,19 @@ class NextLynxModule internal constructor(initializer: () -> LynxModule, @JvmFie } /** Tracks used ports. Resets each OpMode. */ - private val usedPorts by LazyHardware { HashSet>() } + private val usedPorts by LazyHardware { HashMap, Throwable>() } /** Throws if this port is already used on this hub. */ private fun claim(kind: PortKind, port: Int) { - check(usedPorts.add(kind to port)) { - "Port conflict on $type: ${kind.name.lowercase()} port $port is used by more than one device" + val key = kind to port + val first = usedPorts[key] + if (first != null) { + throw IllegalStateException( + "Port conflict on $type: ${kind.name.lowercase()} port $port is used by more than one device", + first, + ) } + usedPorts[key] = Throwable("Port first claimed here") } /** Current module temperature. */