reset hardware objects after every opmode

hardware/build.gradle.kts

@@ -25,6 +25,9 @@ dependencies {
   implementation(libs.sloth)
   compileOnly(libs.blaze)
 
+  // RobotController implements an SDK interface, so it must be loadable in unit tests
+  testImplementation(libs.ftc.robot.core) { isTransitive = false }
+  testImplementation(libs.ftc.common) { isTransitive = false }
   testImplementation(libs.bundles.kotest)
   testImplementation(libs.mockk)
 }

hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt

@@ -12,21 +12,34 @@ import android.annotation.SuppressLint
 import android.content.Context
 import com.qualcomm.ftccommon.FtcEventLoop
 import com.qualcomm.hardware.lynx.LynxModule
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier
 import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.hardware.configuration.LynxConstants
 import dev.frozenmilk.sinister.sdk.apphooks.OnCreateEventLoop
 import dev.nextftc.hardware.lynx.NextLynxModule
+import dev.nextftc.hardware.util.LazyHardware
 import dev.nextftc.units.celsius
 import dev.nextftc.units.measuretypes.Temperature
 import dev.nextftc.units.measuretypes.Voltage
 import dev.nextftc.units.volts
 import org.firstinspires.ftc.robotcore.external.navigation.TempUnit
 import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit
+import java.util.Collections
+import java.util.WeakHashMap
 
 /**
  * Centralized access to FTC hardware/runtime context and Lynx hub telemetry.
  */
-object RobotController : OnCreateEventLoop {
+object RobotController : OnCreateEventLoop, OpModeManagerNotifier.Notifications {
+  // weakly held so lazies created inside an op mode can be garbage collected
+  private val hardwareObjects: MutableSet<LazyHardware<*>> =
+    Collections.synchronizedSet(Collections.newSetFromMap(WeakHashMap()))
+
+  internal fun register(hardware: LazyHardware<*>) {
+    hardwareObjects += hardware
+  }
+
   /**
    * Application context captured when the event loop is created.
    *
@@ -129,5 +142,17 @@ object RobotController : OnCreateEventLoop {
   override fun onCreateEventLoop(context: Context, ftcEventLoop: FtcEventLoop) {
     appContext = context
     eventLoop = ftcEventLoop
+    ftcEventLoop.opModeManager.registerListener(this)
+  }
+
+  override fun onOpModePreInit(opMode: OpMode?) {}
+
+  override fun onOpModePreStart(opMode: OpMode?) {}
+
+  /**
+   * 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() }
   }
 }

hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt

@@ -68,15 +68,22 @@ open class NextCRServo @JvmOverloads constructor(
   private val lazyServo = LazyHardware(initializer)
   private val servo by lazyServo
 
-  /**
-   * Power applied to the servo, in the range [-1.0, 1.0].
-   */
-  var power: Double by Caching(cacheTolerance) {
+  private val powerCache = Caching(cacheTolerance) {
     if (it != null) {
       servo.power = it
     }
   }
 
+  /**
+   * Power applied to the servo, in the range [-1.0, 1.0].
+   */
+  var power: Double by powerCache
+
+  init {
+    // a re-created servo does not hold the previously written power
+    lazyServo.applyAfterInit("powerCache") { powerCache.reset() }
+  }
+
   /**
    * Direction of the servo. Setting this to [NextMotor.Direction.REVERSE]
    * causes positive [power] values to spin the servo the opposite way,
@@ -85,11 +92,7 @@ open class NextCRServo @JvmOverloads constructor(
   var direction: NextMotor.Direction = NextMotor.Direction.FORWARD
     set(value) {
       field = value
-      if (lazyServo.isInitialized) {
-        servo.direction = value.sdkDirection
-      } else {
-        lazyServo.applyAfterInit { it.direction = value.sdkDirection }
-      }
+      lazyServo.applyAfterInit("direction") { it.direction = value.sdkDirection }
     }
 
   /**

hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt

@@ -119,6 +119,22 @@ class NextMotor @JvmOverloads constructor(
   private val lazyMotor = LazyHardware(initializer)
   private val motor by lazyMotor
 
+  private val powerCache = Caching(cacheTolerance) {
+    if (it != null) {
+      if (RobotController.blazeEnabled) {
+        val port = this.motor.portNumber
+        BlazeFTC.setMotorPower(hubId, port, it)
+      } else {
+        motor.power = it
+      }
+    }
+  }
+
+  init {
+    // a re-created motor does not hold the previously written power
+    lazyMotor.applyAfterInit("powerCache") { powerCache.reset() }
+  }
+
   /**
    * Position control constants (PID and feedforward gains).
    *
@@ -175,16 +191,7 @@ class NextMotor @JvmOverloads constructor(
    * This backing field is managed by the caching delegate to reduce
    * redundant hardware writes.
    */
-  private var power by Caching(cacheTolerance) {
-    if (it != null) {
-      if (RobotController.blazeEnabled) {
-        val port = this.motor.portNumber
-        BlazeFTC.setMotorPower(hubId, port, it)
-      } else {
-        motor.power = it
-      }
-    }
-  }
+  private var power by powerCache
 
   /**
    * Motor rotation direction (FORWARD or REVERSE).
@@ -194,11 +201,7 @@ class NextMotor @JvmOverloads constructor(
   var direction = Direction.FORWARD
     set(value) {
       field = value
-      if (lazyMotor.isInitialized) {
-        motor.direction = value.sdkDirection
-      } else {
-        lazyMotor.applyAfterInit { it.direction = value.sdkDirection }
-      }
+      lazyMotor.applyAfterInit("direction") { it.direction = value.sdkDirection }
     }
 
   /**
@@ -209,11 +212,7 @@ class NextMotor @JvmOverloads constructor(
   var zeroPowerBehavior = ZeroPowerBehavior.FLOAT
     set(value) {
       field = value
-      if (lazyMotor.isInitialized) {
-        motor.zeroPowerBehavior = value.sdkZeroPowerBehavior
-      } else {
-        lazyMotor.applyAfterInit { it.zeroPowerBehavior = value.sdkZeroPowerBehavior }
-      }
+      lazyMotor.applyAfterInit("zeroPowerBehavior") { it.zeroPowerBehavior = value.sdkZeroPowerBehavior }
     }
 
   /**
@@ -233,11 +232,7 @@ class NextMotor @JvmOverloads constructor(
   var currentAlert: Current
     get() = motor.getCurrentAlert(CurrentUnit.AMPS).amperes
     set(value) {
-      if (lazyMotor.isInitialized) {
-        motor.setCurrentAlert(value.magnitude, CurrentUnit.AMPS)
-      } else {
-        lazyMotor.applyAfterInit { it.setCurrentAlert(value.magnitude, CurrentUnit.AMPS) }
-      }
+      lazyMotor.applyAfterInit("currentAlert") { it.setCurrentAlert(value.magnitude, CurrentUnit.AMPS) }
     }
 
   /**

hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt

@@ -94,10 +94,21 @@ open class NextServo @JvmOverloads constructor(
   )
 
   private val lazyServo = LazyHardware(initializer).apply {
-    applyAfterInit { it.direction = direction.servoDirection }
+    applyAfterInit("direction") { it.direction = direction.servoDirection }
   }
   private val servo by lazyServo
 
+  private val positionCache = Caching(cacheTolerance) {
+    if (it != null) {
+      servo.position = it
+    }
+  }
+
+  init {
+    // a re-created servo does not hold the previously written position
+    lazyServo.applyAfterInit("positionCache") { positionCache.reset() }
+  }
+
   /**
    * Allows user to change servo's direction configuration
    *
@@ -110,11 +121,7 @@ open class NextServo @JvmOverloads constructor(
    */
   var direction: NextMotor.Direction = direction
     set(direction) {
-      if (lazyServo.isInitialized) {
-        servo.direction = direction.servoDirection
-      } else {
-        lazyServo.applyAfterInit { it.direction = direction.servoDirection }
-      }
+      lazyServo.applyAfterInit("direction") { it.direction = direction.servoDirection }
       field = direction
     }
 
@@ -124,11 +131,7 @@ open class NextServo @JvmOverloads constructor(
    * Assigning a value writes through to the backing [ServoImplEx], while reads
    * are handled by the [Caching] delegate.
    */
-  var position: Double by Caching(cacheTolerance) {
-    if (it != null) {
-      servo.position = it
-    }
-  }
+  var position: Double by positionCache
 
   /**
    * Provides access to the servo's PWM range configuration.
@@ -143,11 +146,7 @@ open class NextServo @JvmOverloads constructor(
   var pwmRange: PwmControl.PwmRange
     get() = servo.pwmRange
     set(range) {
-      if (lazyServo.isInitialized) {
-        servo.pwmRange = range
-      } else {
-        lazyServo.applyAfterInit { it.pwmRange = range }
-      }
+      lazyServo.applyAfterInit("pwmRange") { it.pwmRange = range }
     }
 
   /**

hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt

@@ -124,11 +124,7 @@ class NextColorDistanceSensor @JvmOverloads constructor(
   var gain: Float
     get() = colorSensor.gain
     set(gain) {
-      if (lazySensor.isInitialized) {
-        colorSensor.gain = gain
-      } else {
-        lazySensor.applyAfterInit { it.gain = gain }
-      }
+      lazySensor.applyAfterInit("gain") { it.gain = gain }
     }
 
   /** Reads the color sensor (and distance sensor, if present) and refreshes the cache. Call this once per loop, before reading any properties. */

hardware/src/main/kotlin/dev/nextftc/hardware/util/Caching.kt

@@ -14,10 +14,19 @@ class Caching(private val cacheTolerance: Double, private val whenSet: (Double?)
 
   override fun setValue(thisRef: Any?, property: KProperty<*>, value: Double) {
     if (cachedValue.isNaN() || abs(cachedValue - value) > cacheTolerance) {
-      cachedValue = value
+      // written first: the write may initialize the hardware, which resets this cache
       whenSet(value)
+      cachedValue = value
     } else {
       whenSet(null)
     }
   }
+
+  /**
+   * Forgets the cached value so the next set is always written to the hardware. Call this
+   * whenever the underlying hardware object is re-created.
+   */
+  fun reset() {
+    cachedValue = Double.NaN
+  }
 }

hardware/src/main/kotlin/dev/nextftc/hardware/util/LazyHardware.kt

@@ -1,24 +1,36 @@
 package dev.nextftc.hardware.util
 
 import android.util.Log
-import com.qualcomm.robotcore.eventloop.opmode.OpMode
 import dev.nextftc.functionalInterfaces.Configurator
 import dev.nextftc.hardware.RobotController
 import kotlin.properties.ReadOnlyProperty
 import kotlin.reflect.KProperty
 
+/**
+ * Lazily initializes a hardware object on first access.
+ *
+ * The cached object is discarded when an OpMode stops (see [RobotController]), so the next
+ * access re-runs the initializer against the new hardware map. Blocks passed to [applyAfterInit]
+ * are re-applied on every initialization.
+ */
 class LazyHardware<T>(private val initializer: () -> T) : ReadOnlyProperty<Any?, T> {
 
   private var value: T? = null
   internal val isInitialized: Boolean
     get() = value != null
 
+  private val onInit = LinkedHashMap<Any, Configurator<T>>()
+
+  init {
+    RobotController.register(this)
+  }
+
   override fun getValue(thisRef: Any?, property: KProperty<*>): T {
     if (value != null) return value!!
 
     return initializer.invoke().also { hardwareObject ->
       value = hardwareObject
-      onInit.forEach { block -> block.configure(hardwareObject) }
+      onInit.values.toList().forEach { block -> block.configure(hardwareObject) }
       Log.d(
         "NextFTC",
         "Initialized lazy $hardwareObject in property ${property.name} in class ${thisRef?.let {
@@ -28,13 +40,25 @@ class LazyHardware<T>(private val initializer: () -> T) : ReadOnlyProperty<Any?,
     }
   }
 
-  private val onInit = mutableListOf<Configurator<T>>()
+  /**
+   * Runs [block] on the hardware object now if it is initialized, and again after every
+   * (re-)initialization.
+   */
+  fun applyAfterInit(block: Configurator<T>) = applyAfterInit(Any(), block)
 
-  fun applyAfterInit(block: Configurator<T>) {
-    if (value != null) {
-      block.configure(value)
-    } else {
-      onInit += block
-    }
+  /**
+   * Like [applyAfterInit], but replaces any earlier block registered with the same [key], so
+   * repeatedly updating one setting does not accumulate blocks.
+   */
+  fun applyAfterInit(key: Any, block: Configurator<T>) {
+    onInit[key] = block
+    value?.let { block.configure(it) }
+  }
+
+  /**
+   * Discards the cached object so the next access re-initializes it.
+   */
+  internal fun reset() {
+    value = null
   }
 }