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.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"),
|
||||
// )
|
||||
//
|
||||
// }
|
||||
//}
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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() {
|
||||
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<D : Device>(
|
||||
@OptIn(DFExperimental::class)
|
||||
final override fun stop() {
|
||||
onStop()
|
||||
lifecycleState = DeviceLifecycleState.CLOSED
|
||||
lifecycleState = DeviceLifecycleState.STOPPED
|
||||
super.stop()
|
||||
}
|
||||
|
||||
|
||||
@ -99,7 +99,7 @@ public class DeviceClient(
|
||||
}
|
||||
|
||||
@DFExperimental
|
||||
override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.OPEN
|
||||
override val lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STARTED
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Loading…
Reference in New Issue
Block a user