diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/PidRegulator.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/PidRegulator.kt index 8dbe6ba..cd38c93 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/PidRegulator.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/PidRegulator.kt @@ -8,110 +8,56 @@ import kotlinx.coroutines.sync.Mutex import kotlinx.coroutines.sync.withLock import kotlinx.datetime.Clock import kotlinx.datetime.Instant -import space.kscience.controls.api.Device +import space.kscience.controls.api.DeviceLifecycleState import space.kscience.controls.spec.DeviceBySpec -import space.kscience.controls.spec.DeviceSpec -import space.kscience.controls.spec.doubleProperty -import space.kscience.dataforge.context.Context -import space.kscience.dataforge.context.Factory -import space.kscience.dataforge.meta.Meta -import space.kscience.dataforge.meta.double -import space.kscience.dataforge.meta.get -import space.kscience.dataforge.meta.transformations.MetaConverter -import kotlin.math.pow import kotlin.time.Duration import kotlin.time.Duration.Companion.milliseconds import kotlin.time.DurationUnit - -interface PidRegulator : Device { - /** - * Proportional coefficient - */ - val kp: Double - - /** - * Integral coefficient - */ - val ki: Double - - /** - * Differential coefficient - */ - val kd: Double - - /** - * The target value for PID - */ - var target: Double - - /** - * Read current value - */ - suspend fun read(): Double - - companion object : DeviceSpec() { - val target by property(MetaConverter.double, PidRegulator::target) - val value by doubleProperty { read() } - } -} - /** - * + * PID controller on top of a [Regulator] */ -class VirtualPid( - context: Context, - override val kp: Double, - override val ki: Double, - override val kd: Double, - val mass: Double, - override var target: Double = 0.0, +public class PidRegulator( + public val regulator: Regulator, + public val kp: Double, + public val ki: Double, + public val kd: Double, private val dt: Duration = 0.5.milliseconds, private val clock: Clock = Clock.System, -) : DeviceBySpec(PidRegulator, context), PidRegulator { - - private val mutex = Mutex() +) : DeviceBySpec(Regulator, regulator.context), Regulator { + override var target: Double = regulator.target private var lastTime: Instant = clock.now() - private var lastValue: Double = target + private var lastRegulatorTarget: Double = target - private var value: Double = target - private var velocity: Double = 0.0 - private var acceleration: Double = 0.0 private var integral: Double = 0.0 private var updateJob: Job? = null + private val mutex = Mutex() + override suspend fun onStart() { + if(regulator.lifecycleState == DeviceLifecycleState.STOPPED){ + regulator.start() + } + regulator.start() updateJob = launch { while (isActive) { delay(dt) mutex.withLock { val realTime = clock.now() - val delta = target - value + val delta = target - position val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS) integral += delta * dtSeconds - val derivative = (value - lastValue) / dtSeconds + val derivative = (regulator.target - lastRegulatorTarget) / dtSeconds //set last time and value to new values lastTime = realTime - lastValue = value + lastRegulatorTarget = regulator.target - // compute new value based on velocity and acceleration from the previous step - value += velocity * dtSeconds + acceleration * dtSeconds.pow(2) / 2 - - // compute new velocity based on acceleration on the previous step - velocity += acceleration * dtSeconds - - //compute force for the next step based on current values - acceleration = (kp * delta + ki * integral + kd * derivative) / mass - - - check(value.isFinite() && velocity.isFinite()) { - "Value $value is not finite" - } + regulator.target = regulator.position + kp * delta + ki * integral + kd * derivative } } } @@ -119,28 +65,131 @@ class VirtualPid( override fun onStop() { updateJob?.cancel() - super.stop() } - override suspend fun read(): Double = value + override val position: Double get() = regulator.position - suspend fun readVelocity(): Double = velocity +} - suspend fun readAcceleration(): Double = acceleration - suspend fun write(newTarget: Double) = mutex.withLock { - require(newTarget.isFinite()) { "Value $newTarget is not valid" } - target = newTarget - } - - companion object : Factory { - override fun build(context: Context, meta: Meta) = VirtualPid( - context, - meta["kp"].double ?: error("Kp is not defined"), - meta["ki"].double ?: error("Ki is not defined"), - meta["kd"].double ?: error("Kd is not defined"), - meta["m"].double ?: error("Mass is not defined"), - ) - - } -} \ No newline at end of file +// +//interface PidRegulator : Device { +// /** +// * Proportional coefficient +// */ +// val kp: Double +// +// /** +// * Integral coefficient +// */ +// val ki: Double +// +// /** +// * Differential coefficient +// */ +// val kd: Double +// +// /** +// * The target value for PID +// */ +// var target: Double +// +// /** +// * Read current value +// */ +// suspend fun read(): Double +// +// companion object : DeviceSpec() { +// val target by property(MetaConverter.double, PidRegulator::target) +// val value by doubleProperty { read() } +// } +//} +// +///** +// * +// */ +//class VirtualPid( +// context: Context, +// override val kp: Double, +// override val ki: Double, +// override val kd: Double, +// val mass: Double, +// override var target: Double = 0.0, +// private val dt: Duration = 0.5.milliseconds, +// private val clock: Clock = Clock.System, +//) : DeviceBySpec(PidRegulator, context), PidRegulator { +// +// private val mutex = Mutex() +// +// +// private var lastTime: Instant = clock.now() +// private var lastValue: Double = target +// +// private var value: Double = target +// private var velocity: Double = 0.0 +// private var acceleration: Double = 0.0 +// private var integral: Double = 0.0 +// +// +// private var updateJob: Job? = null +// +// override suspend fun onStart() { +// updateJob = launch { +// while (isActive) { +// delay(dt) +// mutex.withLock { +// val realTime = clock.now() +// val delta = target - value +// val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS) +// integral += delta * dtSeconds +// val derivative = (value - lastValue) / dtSeconds +// +// //set last time and value to new values +// lastTime = realTime +// lastValue = value +// +// // compute new value based on velocity and acceleration from the previous step +// value += velocity * dtSeconds + acceleration * dtSeconds.pow(2) / 2 +// +// // compute new velocity based on acceleration on the previous step +// velocity += acceleration * dtSeconds +// +// //compute force for the next step based on current values +// acceleration = (kp * delta + ki * integral + kd * derivative) / mass +// +// +// check(value.isFinite() && velocity.isFinite()) { +// "Value $value is not finite" +// } +// } +// } +// } +// } +// +// override fun onStop() { +// updateJob?.cancel() +// super.stop() +// } +// +// override suspend fun read(): Double = value +// +// suspend fun readVelocity(): Double = velocity +// +// suspend fun readAcceleration(): Double = acceleration +// +// suspend fun write(newTarget: Double) = mutex.withLock { +// require(newTarget.isFinite()) { "Value $newTarget is not valid" } +// target = newTarget +// } +// +// companion object : Factory { +// override fun build(context: Context, meta: Meta) = VirtualPid( +// context, +// meta["kp"].double ?: error("Kp is not defined"), +// meta["ki"].double ?: error("Ki is not defined"), +// meta["kd"].double ?: error("Kd is not defined"), +// meta["m"].double ?: error("Mass is not defined"), +// ) +// +// } +//} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/Drive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/Regulator.kt similarity index 65% rename from controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/Drive.kt rename to controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/Regulator.kt index ede628a..2db263d 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/Drive.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/Regulator.kt @@ -13,7 +13,7 @@ import space.kscience.dataforge.meta.transformations.MetaConverter /** * A single axis drive */ -public interface Drive : Device { +public interface Regulator : Device { /** * Get or set target value */ @@ -24,21 +24,21 @@ public interface Drive : Device { */ public val position: Double - public companion object : DeviceSpec() { - public val target: DevicePropertySpec by property(MetaConverter.double, Drive::target) + public companion object : DeviceSpec() { + public val target: DevicePropertySpec by property(MetaConverter.double, Regulator::target) - public val position: DevicePropertySpec by doubleProperty { position } + public val position: DevicePropertySpec by doubleProperty { position } } } /** - * Virtual [Drive] with speed limit + * Virtual [Regulator] with speed limit */ -public class VirtualDrive( +public class VirtualRegulator( context: Context, value: Double, private val speed: Double, -) : DeviceBySpec(Drive, context), Drive { +) : DeviceBySpec(Regulator, context), Regulator { private var moveJob: Job? = null diff --git a/controls-core/src/commonMain/kotlin/space/kscience/controls/api/Device.kt b/controls-core/src/commonMain/kotlin/space/kscience/controls/api/Device.kt index dad1d90..a0268c2 100644 --- a/controls-core/src/commonMain/kotlin/space/kscience/controls/api/Device.kt +++ b/controls-core/src/commonMain/kotlin/space/kscience/controls/api/Device.kt @@ -19,21 +19,22 @@ import space.kscience.dataforge.names.Name /** * A lifecycle state of a device */ -public enum class DeviceLifecycleState{ +public enum class DeviceLifecycleState { + /** * Device is initializing - */ - INIT, + */ + STARTING, /** * The Device is initialized and running */ - OPEN, + STARTED, /** * The Device is closed */ - CLOSED + STOPPED } /** @@ -136,5 +137,8 @@ public fun Device.getAllProperties(): Meta = Meta { /** * Subscribe on property changes for the whole device */ -public fun Device.onPropertyChange(scope: CoroutineScope = this, callback: suspend PropertyChangedMessage.() -> Unit): Job = +public fun Device.onPropertyChange( + scope: CoroutineScope = this, + callback: suspend PropertyChangedMessage.() -> Unit, +): Job = messageFlow.filterIsInstance().onEach(callback).launchIn(scope) diff --git a/controls-core/src/commonMain/kotlin/space/kscience/controls/spec/DeviceBase.kt b/controls-core/src/commonMain/kotlin/space/kscience/controls/spec/DeviceBase.kt index b1544e0..3b6bce0 100644 --- a/controls-core/src/commonMain/kotlin/space/kscience/controls/spec/DeviceBase.kt +++ b/controls-core/src/commonMain/kotlin/space/kscience/controls/spec/DeviceBase.kt @@ -189,7 +189,7 @@ public abstract class DeviceBase( } @DFExperimental - override var lifecycleState: DeviceLifecycleState = DeviceLifecycleState.INIT + override var lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STOPPED protected set protected open suspend fun onStart() { @@ -198,10 +198,14 @@ public abstract class DeviceBase( @OptIn(DFExperimental::class) final override suspend fun start() { - super.start() - lifecycleState = DeviceLifecycleState.INIT - onStart() - lifecycleState = DeviceLifecycleState.OPEN + if(lifecycleState == DeviceLifecycleState.STOPPED) { + super.start() + lifecycleState = DeviceLifecycleState.STARTING + onStart() + lifecycleState = DeviceLifecycleState.STARTED + } else { + logger.debug { "Device $this is already started" } + } } protected open fun onStop() { @@ -211,7 +215,7 @@ public abstract class DeviceBase( @OptIn(DFExperimental::class) final override fun stop() { onStop() - lifecycleState = DeviceLifecycleState.CLOSED + lifecycleState = DeviceLifecycleState.STOPPED super.stop() } diff --git a/controls-magix/src/commonMain/kotlin/space/kscience/controls/client/DeviceClient.kt b/controls-magix/src/commonMain/kotlin/space/kscience/controls/client/DeviceClient.kt index 64e8a9e..2fcecff 100644 --- a/controls-magix/src/commonMain/kotlin/space/kscience/controls/client/DeviceClient.kt +++ b/controls-magix/src/commonMain/kotlin/space/kscience/controls/client/DeviceClient.kt @@ -99,7 +99,7 @@ public class DeviceClient( } @DFExperimental - override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.OPEN + override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STARTED } /**