Lifecycle change

This commit is contained in:
Alexander Nozik 2023-10-27 10:57:46 +03:00
parent 1619fdadf2
commit 4f028ccee8
5 changed files with 171 additions and 114 deletions

View File

@ -8,110 +8,56 @@ import kotlinx.coroutines.sync.Mutex
import kotlinx.coroutines.sync.withLock import kotlinx.coroutines.sync.withLock
import kotlinx.datetime.Clock import kotlinx.datetime.Clock
import kotlinx.datetime.Instant 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.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
import kotlin.time.Duration.Companion.milliseconds import kotlin.time.Duration.Companion.milliseconds
import kotlin.time.DurationUnit 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<PidRegulator>() {
val target by property(MetaConverter.double, PidRegulator::target)
val value by doubleProperty { read() }
}
}
/** /**
* * PID controller on top of a [Regulator]
*/ */
class VirtualPid( public class PidRegulator(
context: Context, public val regulator: Regulator,
override val kp: Double, public val kp: Double,
override val ki: Double, public val ki: Double,
override val kd: Double, public val kd: Double,
val mass: Double,
override var target: Double = 0.0,
private val dt: Duration = 0.5.milliseconds, private val dt: Duration = 0.5.milliseconds,
private val clock: Clock = Clock.System, private val clock: Clock = Clock.System,
) : DeviceBySpec<PidRegulator>(PidRegulator, context), PidRegulator { ) : DeviceBySpec<Regulator>(Regulator, regulator.context), Regulator {
private val mutex = Mutex()
override var target: Double = regulator.target
private var lastTime: Instant = clock.now() 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 integral: Double = 0.0
private var updateJob: Job? = null private var updateJob: Job? = null
private val mutex = Mutex()
override suspend fun onStart() { override suspend fun onStart() {
if(regulator.lifecycleState == DeviceLifecycleState.STOPPED){
regulator.start()
}
regulator.start()
updateJob = launch { updateJob = launch {
while (isActive) { while (isActive) {
delay(dt) delay(dt)
mutex.withLock { mutex.withLock {
val realTime = clock.now() val realTime = clock.now()
val delta = target - value val delta = target - position
val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS) val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS)
integral += delta * dtSeconds integral += delta * dtSeconds
val derivative = (value - lastValue) / dtSeconds val derivative = (regulator.target - lastRegulatorTarget) / dtSeconds
//set last time and value to new values //set last time and value to new values
lastTime = realTime lastTime = realTime
lastValue = value lastRegulatorTarget = regulator.target
// compute new value based on velocity and acceleration from the previous step regulator.target = regulator.position + kp * delta + ki * integral + kd * derivative
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"
}
} }
} }
} }
@ -119,28 +65,131 @@ class VirtualPid(
override fun onStop() { override fun onStop() {
updateJob?.cancel() updateJob?.cancel()
super<PidRegulator>.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" } //interface PidRegulator : Device {
target = newTarget // /**
} // * Proportional coefficient
// */
companion object : Factory<Device> { // val kp: Double
override fun build(context: Context, meta: Meta) = VirtualPid( //
context, // /**
meta["kp"].double ?: error("Kp is not defined"), // * Integral coefficient
meta["ki"].double ?: error("Ki is not defined"), // */
meta["kd"].double ?: error("Kd is not defined"), // val ki: Double
meta["m"].double ?: error("Mass is not defined"), //
) // /**
// * Differential coefficient
} // */
} // val kd: Double
//
// /**
// * The target value for PID
// */
// var target: Double
//
// /**
// * Read current value
// */
// suspend fun read(): Double
//
// companion object : DeviceSpec<PidRegulator>() {
// 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>(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<PidRegulator>.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<Device> {
// 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"),
// )
//
// }
//}

View File

@ -13,7 +13,7 @@ import space.kscience.dataforge.meta.transformations.MetaConverter
/** /**
* A single axis drive * A single axis drive
*/ */
public interface Drive : Device { public interface Regulator : Device {
/** /**
* Get or set target value * Get or set target value
*/ */
@ -24,21 +24,21 @@ public interface Drive : Device {
*/ */
public val position: Double public val position: Double
public companion object : DeviceSpec<Drive>() { public companion object : DeviceSpec<Regulator>() {
public val target: DevicePropertySpec<Drive, Double> by property(MetaConverter.double, Drive::target) public val target: DevicePropertySpec<Regulator, Double> by property(MetaConverter.double, Regulator::target)
public val position: DevicePropertySpec<Drive, Double> by doubleProperty { position } public val position: DevicePropertySpec<Regulator, Double> by doubleProperty { position }
} }
} }
/** /**
* Virtual [Drive] with speed limit * Virtual [Regulator] with speed limit
*/ */
public class VirtualDrive( public class VirtualRegulator(
context: Context, context: Context,
value: Double, value: Double,
private val speed: Double, private val speed: Double,
) : DeviceBySpec<Drive>(Drive, context), Drive { ) : DeviceBySpec<Regulator>(Regulator, context), Regulator {
private var moveJob: Job? = null private var moveJob: Job? = null

View File

@ -19,21 +19,22 @@ import space.kscience.dataforge.names.Name
/** /**
* A lifecycle state of a device * A lifecycle state of a device
*/ */
public enum class DeviceLifecycleState{ public enum class DeviceLifecycleState {
/** /**
* Device is initializing * Device is initializing
*/ */
INIT, STARTING,
/** /**
* The Device is initialized and running * The Device is initialized and running
*/ */
OPEN, STARTED,
/** /**
* The Device is closed * The Device is closed
*/ */
CLOSED STOPPED
} }
/** /**
@ -136,5 +137,8 @@ public fun Device.getAllProperties(): Meta = Meta {
/** /**
* Subscribe on property changes for the whole device * 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<PropertyChangedMessage>().onEach(callback).launchIn(scope) messageFlow.filterIsInstance<PropertyChangedMessage>().onEach(callback).launchIn(scope)

View File

@ -189,7 +189,7 @@ public abstract class DeviceBase<D : Device>(
} }
@DFExperimental @DFExperimental
override var lifecycleState: DeviceLifecycleState = DeviceLifecycleState.INIT override var lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STOPPED
protected set protected set
protected open suspend fun onStart() { protected open suspend fun onStart() {
@ -198,10 +198,14 @@ public abstract class DeviceBase<D : Device>(
@OptIn(DFExperimental::class) @OptIn(DFExperimental::class)
final override suspend fun start() { final override suspend fun start() {
super.start() if(lifecycleState == DeviceLifecycleState.STOPPED) {
lifecycleState = DeviceLifecycleState.INIT super.start()
onStart() lifecycleState = DeviceLifecycleState.STARTING
lifecycleState = DeviceLifecycleState.OPEN onStart()
lifecycleState = DeviceLifecycleState.STARTED
} else {
logger.debug { "Device $this is already started" }
}
} }
protected open fun onStop() { protected open fun onStop() {
@ -211,7 +215,7 @@ public abstract class DeviceBase<D : Device>(
@OptIn(DFExperimental::class) @OptIn(DFExperimental::class)
final override fun stop() { final override fun stop() {
onStop() onStop()
lifecycleState = DeviceLifecycleState.CLOSED lifecycleState = DeviceLifecycleState.STOPPED
super.stop() super.stop()
} }

View File

@ -99,7 +99,7 @@ public class DeviceClient(
} }
@DFExperimental @DFExperimental
override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.OPEN override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STARTED
} }
/** /**