diff --git a/hardware/build.gradle.kts b/hardware/build.gradle.kts index 9f79b175..c7071b58 100644 --- a/hardware/build.gradle.kts +++ b/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) } diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt index 2b6ed20f..aee2f48d 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/RobotController.kt +++ b/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> = + 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() } } } diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt index 73c791b6..05f4a083 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt +++ b/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 } } /** diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt index 4448c6f0..a9c569c0 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt +++ b/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) } } /** diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt index a4bd455c..a0bb343a 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt +++ b/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 } } /** diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt index 8b0ba560..30919157 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt +++ b/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. */ diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/util/Caching.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/util/Caching.kt index b8210b99..0440a08b 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/util/Caching.kt +++ b/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 + } } diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/util/LazyHardware.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/util/LazyHardware.kt index ef3fa859..96ac34ee 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/util/LazyHardware.kt +++ b/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(private val initializer: () -> T) : ReadOnlyProperty { private var value: T? = null internal val isInitialized: Boolean get() = value != null + private val onInit = LinkedHashMap>() + + 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(private val initializer: () -> T) : ReadOnlyProperty>() + /** + * Runs [block] on the hardware object now if it is initialized, and again after every + * (re-)initialization. + */ + fun applyAfterInit(block: Configurator) = applyAfterInit(Any(), block) - fun applyAfterInit(block: Configurator) { - 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) { + onInit[key] = block + value?.let { block.configure(it) } + } + + /** + * Discards the cached object so the next access re-initializes it. + */ + internal fun reset() { + value = null } } diff --git a/hardware/src/test/kotlin/dev/nextftc/hardware/LazyHardwareTest.kt b/hardware/src/test/kotlin/dev/nextftc/hardware/LazyHardwareTest.kt index 0ba9f39a..49d3ea37 100644 --- a/hardware/src/test/kotlin/dev/nextftc/hardware/LazyHardwareTest.kt +++ b/hardware/src/test/kotlin/dev/nextftc/hardware/LazyHardwareTest.kt @@ -9,6 +9,7 @@ package dev.nextftc.hardware import dev.nextftc.functionalInterfaces.Configurator +import dev.nextftc.hardware.util.Caching import dev.nextftc.hardware.util.LazyHardware import io.kotest.core.spec.style.FunSpec import io.kotest.matchers.shouldBe @@ -80,6 +81,56 @@ class LazyHardwareTest : configured shouldBe "device" } + test("op mode stop re-initializes the value and replays applyAfterInit callbacks") { + var calls = 0 + val configured = mutableListOf() + lateinit var lazyRef: LazyHardware + class Holder { + val value by LazyHardware { ++calls }.also { lazyRef = it } + } + val holder = Holder() + lazyRef.applyAfterInit(Configurator { configured += it }) + + holder.value shouldBe 1 + RobotController.onOpModePostStop(null) + holder.value shouldBe 2 + holder.value shouldBe 2 + + calls shouldBe 2 + configured shouldBe listOf(1, 2) + } + + test("a keyed applyAfterInit replaces the earlier block with the same key") { + var calls = 0 + val configured = mutableListOf() + lateinit var lazyRef: LazyHardware + class Holder { + val value by LazyHardware { ++calls }.also { lazyRef = it } + } + val holder = Holder() + + lazyRef.applyAfterInit("setting", Configurator { configured += "first" }) + holder.value + lazyRef.applyAfterInit("setting", Configurator { configured += "second" }) + RobotController.onOpModePostStop(null) + holder.value + + configured shouldBe listOf("first", "second", "second") + } + + test("Caching writes again after reset even for an unchanged value") { + val writes = mutableListOf() + val cache = Caching(0.01) { writes += it } + var power by cache + + power = 0.5 + power = 0.5 + cache.reset() + power = 0.5 + + writes shouldBe listOf(0.5, null, 0.5) + } + test("multiple queued callbacks all run once the value is initialized") { lateinit var lazyRef: LazyHardware class Holder {