Lifecycle change
This commit is contained in:
parent
1619fdadf2
commit
4f028ccee8
@ -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"),
|
||||||
|
// )
|
||||||
|
//
|
||||||
|
// }
|
||||||
|
//}
|
@ -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
|
||||||
|
|
@ -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)
|
||||||
|
@ -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()
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -99,7 +99,7 @@ public class DeviceClient(
|
|||||||
}
|
}
|
||||||
|
|
||||||
@DFExperimental
|
@DFExperimental
|
||||||
override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.OPEN
|
override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STARTED
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
Loading…
Reference in New Issue
Block a user