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.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<PidRegulator>() {
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>(PidRegulator, context), PidRegulator {
private val mutex = Mutex()
) : DeviceBySpec<Regulator>(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<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" }
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"),
)
}
}
//
//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() }
// }
//}
//
///**
// *
// */
//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
*/
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<Drive>() {
public val target: DevicePropertySpec<Drive, Double> by property(MetaConverter.double, Drive::target)
public companion object : DeviceSpec<Regulator>() {
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,
value: Double,
private val speed: Double,
) : DeviceBySpec<Drive>(Drive, context), Drive {
) : DeviceBySpec<Regulator>(Regulator, context), Regulator {
private var moveJob: Job? = null

View File

@ -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<PropertyChangedMessage>().onEach(callback).launchIn(scope)

View File

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

View File

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