Complete rework of PID demo and underlying devices

This commit is contained in:
Alexander Nozik 2024-06-02 17:58:38 +03:00
parent 54e915ef10
commit d0e3faea88
24 changed files with 719 additions and 445 deletions

View File

@ -3,11 +3,13 @@ package space.kscience.controls.constructor
import kotlinx.coroutines.CoroutineScope
import kotlinx.coroutines.Job
import kotlinx.coroutines.flow.*
import kotlinx.datetime.Instant
import space.kscience.controls.api.Device
import space.kscience.controls.manager.ClockManager
import space.kscience.dataforge.context.ContextAware
import space.kscience.dataforge.context.request
import kotlin.time.Duration
import kotlin.time.Duration.Companion.milliseconds
/**
* A binding that is used to describe device functionality
@ -36,7 +38,7 @@ public class ConnectionConstrucorElement(
) : ConstructorElement
public class ModelConstructorElement(
public val model: ModelConstructor
public val model: ModelConstructor,
) : ConstructorElement
@ -77,15 +79,15 @@ public interface StateContainer : ContextAware, CoroutineScope {
/**
* Register a [state] in this container. The state is not registered as a device property if [this] is a [DeviceConstructor]
*/
public fun <T, D : DeviceState<T>> StateContainer.state(state: D): D {
public fun <T, D : DeviceState<T>> StateContainer.registerState(state: D): D {
registerElement(StateConstructorElement(state))
return state
}
/**
* Create a register a [MutableDeviceState] with a given [converter]
* Create a register a [MutableDeviceState]
*/
public fun <T> StateContainer.stateOf(initialValue: T): MutableDeviceState<T> = state(
public fun <T> StateContainer.stateOf(initialValue: T): MutableDeviceState<T> = registerState(
MutableDeviceState(initialValue)
)
@ -97,23 +99,53 @@ public fun <T : ModelConstructor> StateContainer.model(model: T): T {
/**
* Create and register a timer state.
*/
public fun StateContainer.timer(tick: Duration): TimerState = state(TimerState(context.request(ClockManager), tick))
public fun StateContainer.timer(tick: Duration): TimerState =
registerState(TimerState(context.request(ClockManager), tick))
/**
* Register a new timer and perform [block] on its change
*/
public fun StateContainer.onTimer(
tick: Duration,
writes: Collection<DeviceState<*>> = emptySet(),
reads: Collection<DeviceState<*>> = emptySet(),
block: suspend (prev: Instant, next: Instant) -> Unit,
): Job = timer(tick).onChange(writes = writes, reads = reads, onChange = block)
public enum class DefaultTimer(public val duration: Duration){
REALTIME(5.milliseconds),
VERY_FAST(10.milliseconds),
FAST(20.milliseconds),
MEDIUM(50.milliseconds),
SLOW(100.milliseconds),
VERY_SLOW(500.milliseconds),
}
/**
* Perform an action on default timer
*/
public fun StateContainer.onTimer(
defaultTimer: DefaultTimer = DefaultTimer.FAST,
writes: Collection<DeviceState<*>> = emptySet(),
reads: Collection<DeviceState<*>> = emptySet(),
block: suspend (prev: Instant, next: Instant) -> Unit,
): Job = timer(defaultTimer.duration).onChange(writes = writes, reads = reads, onChange = block)
//TODO implement timer pooling
public fun <T, R> StateContainer.mapState(
origin: DeviceState<T>,
transformation: (T) -> R,
): DeviceStateWithDependencies<R> = state(DeviceState.map(origin, transformation))
): DeviceStateWithDependencies<R> = registerState(DeviceState.map(origin, transformation))
public fun <T, R> StateContainer.flowState(
origin: DeviceState<T>,
initialValue: R,
transformation: suspend FlowCollector<R>.(T) -> Unit
transformation: suspend FlowCollector<R>.(T) -> Unit,
): DeviceStateWithDependencies<R> {
val state = MutableDeviceState(initialValue)
origin.valueFlow.transform(transformation).onEach { state.value = it }.launchIn(this)
return state(state.withDependencies(setOf(origin)))
return registerState(state.withDependencies(setOf(origin)))
}
/**
@ -123,7 +155,7 @@ public fun <T1, T2, R> StateContainer.combineState(
first: DeviceState<T1>,
second: DeviceState<T2>,
transformation: (T1, T2) -> R,
): DeviceState<R> = state(DeviceState.combine(first, second, transformation))
): DeviceState<R> = registerState(DeviceState.combine(first, second, transformation))
/**
* Create and start binding between [sourceState] and [targetState]. Changes made to [sourceState] are automatically

View File

@ -6,12 +6,14 @@ import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.launchIn
import kotlinx.coroutines.flow.map
import kotlinx.coroutines.flow.onEach
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.UnitsOfMeasurement
import kotlin.reflect.KProperty
/**
* An observable state of a device
*/
public interface DeviceState<T> {
public interface DeviceState<out T> {
public val value: T
public val valueFlow: Flow<T>
@ -49,7 +51,7 @@ public interface DeviceStateWithDependencies<T> : DeviceState<T> {
}
public fun <T> DeviceState<T>.withDependencies(
dependencies: Collection<DeviceState<*>>
dependencies: Collection<DeviceState<*>>,
): DeviceStateWithDependencies<T> = object : DeviceStateWithDependencies<T>, DeviceState<T> by this {
override val dependencies: Collection<DeviceState<*>> = dependencies
}
@ -72,6 +74,16 @@ public fun <T, R> DeviceState.Companion.map(
public fun <T, R> DeviceState<T>.map(mapper: (T) -> R): DeviceStateWithDependencies<R> = DeviceState.map(this, mapper)
public fun DeviceState<out NumericalValue<out UnitsOfMeasurement>>.values(): DeviceState<Double> = object : DeviceState<Double> {
override val value: Double
get() = this@values.value.value
override val valueFlow: Flow<Double>
get() = this@values.valueFlow.map { it.value }
override fun toString(): String = this@values.toString()
}
/**
* Combine two device states into one read-only [DeviceState]. Only the latest value of each state is used.
*/

View File

@ -0,0 +1,18 @@
package space.kscience.controls.constructor.devices
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.property
import space.kscience.controls.constructor.units.NewtonsMeters
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.numerical
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.meta.MetaConverter
public class Drive(
context: Context,
force: MutableDeviceState<NumericalValue<NewtonsMeters>> = MutableDeviceState(NumericalValue(0)),
) : DeviceConstructor(context) {
public val force: MutableDeviceState<NumericalValue<NewtonsMeters>> by property(MetaConverter.numerical(), force)
}

View File

@ -0,0 +1,20 @@
package space.kscience.controls.constructor.devices
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.property
import space.kscience.controls.constructor.units.Degrees
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.numerical
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.meta.MetaConverter
/**
* An encoder that can read an angle
*/
public class EncoderDevice(
context: Context,
position: DeviceState<NumericalValue<Degrees>>
) : DeviceConstructor(context) {
public val position: DeviceState<NumericalValue<Degrees>> by property(MetaConverter.numerical<Degrees>(), position)
}

View File

@ -1,4 +1,4 @@
package space.kscience.controls.constructor.library
package space.kscience.controls.constructor.devices
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.DeviceState
@ -7,7 +7,6 @@ import space.kscience.controls.constructor.registerAsProperty
import space.kscience.controls.constructor.units.Direction
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.UnitsOfMeasurement
import space.kscience.controls.constructor.units.compareTo
import space.kscience.controls.spec.DevicePropertySpec
import space.kscience.controls.spec.DeviceSpec
import space.kscience.controls.spec.booleanProperty

View File

@ -0,0 +1,38 @@
package space.kscience.controls.constructor.devices
import space.kscience.controls.constructor.*
import space.kscience.controls.constructor.models.PidParameters
import space.kscience.controls.constructor.models.PidRegulator
import space.kscience.controls.constructor.units.Meters
import space.kscience.controls.constructor.units.NewtonsMeters
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.numerical
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.meta.Meta
import space.kscience.dataforge.meta.MetaConverter
public class LinearDrive(
drive: Drive,
start: LimitSwitch,
end: LimitSwitch,
position: DeviceState<NumericalValue<Meters>>,
pidParameters: PidParameters,
context: Context = drive.context,
meta: Meta = Meta.EMPTY,
) : DeviceConstructor(context, meta) {
public val position: DeviceState<NumericalValue<Meters>> by property(MetaConverter.numerical(), position)
public val drive: Drive by device(drive)
public val pid: PidRegulator<Meters, NewtonsMeters> = model(
PidRegulator(
context = context,
position = position,
output = drive.force,
pidParameters = pidParameters
)
)
public val startLimit: LimitSwitch by device(start)
public val endLimit: LimitSwitch by device(end)
}

View File

@ -0,0 +1,61 @@
package space.kscience.controls.constructor.devices
import kotlinx.coroutines.launch
import space.kscience.controls.constructor.*
import space.kscience.controls.constructor.units.Degrees
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.plus
import space.kscience.controls.constructor.units.times
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.meta.MetaConverter
import kotlin.math.max
import kotlin.math.min
import kotlin.math.roundToInt
import kotlin.time.DurationUnit
/**
* A step drive
*
* @param speed ticks per second
* @param target target ticks state
* @param writeTicks a hardware callback
*/
public class StepDrive(
context: Context,
speed: MutableDeviceState<Double>,
target: MutableDeviceState<Int> = MutableDeviceState(0),
private val writeTicks: suspend (ticks: Int, speed: Double) -> Unit,
) : DeviceConstructor(context) {
public val target: MutableDeviceState<Int> by property(MetaConverter.int, target)
public val speed: MutableDeviceState<Double> by property(MetaConverter.double, speed)
private val positionState = stateOf(target.value)
public val position: DeviceState<Int> by property(MetaConverter.int, positionState)
private val ticker = onTimer { prev, next ->
val tickSpeed = speed.value
val timeDelta = (next - prev).toDouble(DurationUnit.SECONDS)
val ticksDelta: Int = target.value - position.value
val steps: Int = when {
ticksDelta > 0 -> min(ticksDelta, (timeDelta * tickSpeed).roundToInt())
ticksDelta < 0 -> max(ticksDelta, (timeDelta * tickSpeed).roundToInt())
else -> return@onTimer
}
launch {
writeTicks(steps, tickSpeed)
positionState.value += steps
}
}
}
/**
* Compute a state using given tick-to-angle transformation
*/
public fun StepDrive.angle(
zero: NumericalValue<Degrees>,
step: NumericalValue<Degrees>,
): DeviceState<NumericalValue<Degrees>> = position.map { zero + it * step }

View File

@ -1,57 +0,0 @@
package space.kscience.controls.constructor.library
import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.map
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.map
/**
* A state describing a [Double] value in the [range]
*/
public open class DoubleInRangeState(
private val input: DeviceState<Double>,
public val range: ClosedFloatingPointRange<Double>,
) : DeviceState<Double> {
override val valueFlow: Flow<Double> get() = input.valueFlow.map { it.coerceIn(range) }
override val value: Double get() = input.value.coerceIn(range)
/**
* A state showing that the range is on its lower boundary
*/
public val atStart: DeviceState<Boolean> = input.map { it <= range.start }
/**
* A state showing that the range is on its higher boundary
*/
public val atEnd: DeviceState<Boolean> = input.map { it >= range.endInclusive }
override fun toString(): String = "DoubleRangeState(value=${value},range=$range)"
}
public class MutableDoubleInRangeState(
private val mutableInput: MutableDeviceState<Double>,
range: ClosedFloatingPointRange<Double>
) : DoubleInRangeState(mutableInput, range), MutableDeviceState<Double> {
override var value: Double
get() = super.value
set(value) {
mutableInput.value = value.coerceIn(range)
}
}
public fun MutableDoubleInRangeState(
initialValue: Double,
range: ClosedFloatingPointRange<Double>
): MutableDoubleInRangeState = MutableDoubleInRangeState(MutableDeviceState(initialValue),range)
public fun DeviceState<Double>.coerceIn(
range: ClosedFloatingPointRange<Double>
): DoubleInRangeState = DoubleInRangeState(this, range)
public fun MutableDeviceState<Double>.coerceIn(
range: ClosedFloatingPointRange<Double>
): MutableDoubleInRangeState = MutableDoubleInRangeState(this, range)

View File

@ -1,102 +0,0 @@
package space.kscience.controls.constructor.library
import kotlinx.coroutines.Job
import kotlinx.coroutines.delay
import kotlinx.coroutines.isActive
import kotlinx.coroutines.launch
import space.kscience.controls.api.Device
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.propertyAsState
import space.kscience.controls.manager.clock
import space.kscience.controls.spec.*
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.context.Factory
import space.kscience.dataforge.meta.MetaConverter
import space.kscience.dataforge.meta.double
import space.kscience.dataforge.meta.get
import kotlin.math.pow
import kotlin.time.Duration.Companion.milliseconds
import kotlin.time.DurationUnit
/**
* A classic drive regulated by force with encoder
*/
public interface Drive : Device {
/**
* Get or set drive force or momentum
*/
public var force: Double
/**
* Current position value
*/
public val position: Double
public companion object : DeviceSpec<Drive>() {
public val force: MutableDevicePropertySpec<Drive, Double> by Drive.mutableProperty(
MetaConverter.double,
Drive::force
)
public val position: DevicePropertySpec<Drive, Double> by doubleProperty { position }
}
}
/**
* A virtual drive
*/
public class VirtualDrive(
context: Context,
private val mass: Double,
public val positionState: MutableDeviceState<Double>,
) : Drive, DeviceBySpec<Drive>(Drive, context) {
private val dt = meta["time.step"].double?.milliseconds ?: 5.milliseconds
private val clock = context.clock
override var force: Double = 0.0
override val position: Double get() = positionState.value
public var velocity: Double = 0.0
private set
private var updateJob: Job? = null
override suspend fun onStart() {
updateJob = launch {
var lastTime = clock.now()
while (isActive) {
delay(dt)
val realTime = clock.now()
val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS)
//set last time and value to new values
lastTime = realTime
// compute new value based on velocity and acceleration from the previous step
positionState.value += velocity * dtSeconds + force / mass * dtSeconds.pow(2) / 2
propertyChanged(Drive.position, positionState.value)
// compute new velocity based on acceleration on the previous step
velocity += force / mass * dtSeconds
}
}
}
override suspend fun onStop() {
updateJob?.cancel()
}
public companion object {
public fun factory(
mass: Double,
positionState: MutableDeviceState<Double>,
): Factory<Drive> = Factory { context, _ ->
VirtualDrive(context, mass, positionState)
}
}
}
public fun Drive.stateOfForce(initialForce: Double = 0.0): MutableDeviceState<Double> =
propertyAsState(Drive.force, initialForce)

View File

@ -1,74 +0,0 @@
package space.kscience.controls.constructor.library
import kotlinx.coroutines.delay
import kotlinx.coroutines.isActive
import kotlinx.coroutines.launch
import kotlinx.coroutines.sync.Mutex
import kotlinx.coroutines.sync.withLock
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.state
import space.kscience.controls.constructor.stateOf
import space.kscience.controls.manager.clock
import space.kscience.dataforge.context.Context
import kotlin.time.Duration
import kotlin.time.DurationUnit
/**
* Pid regulator parameters
*/
public data class PidParameters(
val kp: Double,
val ki: Double,
val kd: Double,
val timeStep: Duration,
)
/**
* A PID regulator
*/
public class PidRegulator(
context: Context,
private val position: DeviceState<Double>,
public var pidParameters: PidParameters, // TODO expose as property
output: MutableDeviceState<Double> = MutableDeviceState(0.0),
) : Regulator<Double>(context) {
override val target: MutableDeviceState<Double> = stateOf(0.0)
override val output: MutableDeviceState<Double> = state(output)
private var lastPosition: Double = target.value
private var integral: Double = 0.0
private val mutex = Mutex()
private var lastTime = clock.now()
private val updateJob = launch {
while (isActive) {
delay(pidParameters.timeStep)
mutex.withLock {
val realTime = clock.now()
val delta = target.value - position.value
val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS)
integral += delta * dtSeconds
val derivative = (position.value - lastPosition) / dtSeconds
//set last time and value to new values
lastTime = realTime
lastPosition = position.value
output.value = pidParameters.kp * delta + pidParameters.ki * integral + pidParameters.kd * derivative
}
}
}
}
//
//public fun DeviceGroup.pid(
// name: String,
// drive: Drive,
// pidParameters: PidParameters,
//): PidRegulator = install(name.parseAsName(), PidRegulator(drive, pidParameters))

View File

@ -1,17 +0,0 @@
package space.kscience.controls.constructor.library
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.dataforge.context.Context
/**
* A regulator with target value and current position
*/
public abstract class Regulator<T>(context: Context) : DeviceConstructor(context) {
public abstract val target: MutableDeviceState<T>
public abstract val output: DeviceState<T>
}

View File

@ -1,33 +0,0 @@
package space.kscience.controls.constructor.library
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.combineState
import space.kscience.controls.constructor.property
import space.kscience.controls.constructor.units.*
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.meta.MetaConverter
/**
* A step drive regulated by [input]
*/
public class StepDrive(
context: Context,
public val step: NumericalValue<Degrees>,
public val zero: NumericalValue<Degrees> = NumericalValue(0.0),
direction: Direction = Direction.UP,
input: MutableDeviceState<Int> = MutableDeviceState(0),
hold: MutableDeviceState<Boolean> = MutableDeviceState(false)
) : Transmission<Int, NumericalValue<Degrees>>(context) {
override val input: MutableDeviceState<Int> by property(MetaConverter.int, input)
public val hold: MutableDeviceState<Boolean> by property(MetaConverter.boolean, hold)
override val output: DeviceState<NumericalValue<Degrees>> = combineState(
input, hold
) { input, hold ->
//TODO use hold parameter
zero + input * direction.coef * step
}
}

View File

@ -1,33 +0,0 @@
package space.kscience.controls.constructor.library
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.flowState
import space.kscience.dataforge.context.Context
/**
* A model for a device that converts one type of physical quantity to another type
*/
public abstract class Transmission<T, R>(context: Context) : DeviceConstructor(context) {
public abstract val input: MutableDeviceState<T>
public abstract val output: DeviceState<R>
public companion object {
/**
* Create a device that is a hard connection between two physical quantities
*/
public suspend fun <T, R> direct(
context: Context,
input: MutableDeviceState<T>,
transform: suspend (T) -> R
): Transmission<T, R> {
val initialValue = transform(input.value)
return object : Transmission<T, R>(context) {
override val input: MutableDeviceState<T> = input
override val output: DeviceState<R> = flowState(input, initialValue) { emit(transform(it)) }
}
}
}
}

View File

@ -0,0 +1,104 @@
package space.kscience.controls.constructor.models
import space.kscience.controls.constructor.*
import space.kscience.controls.constructor.units.*
import space.kscience.dataforge.context.Context
import kotlin.math.pow
import kotlin.time.Duration
import kotlin.time.Duration.Companion.milliseconds
import kotlin.time.DurationUnit
/**
* A model for inertial movement. Both linear and angular
*/
public class Inertia<U : UnitsOfMeasurement, V : UnitsOfMeasurement>(
context: Context,
force: DeviceState<Double>, //TODO add system unit sets
inertia: Double,
public val position: MutableDeviceState<NumericalValue<U>>,
public val velocity: MutableDeviceState<NumericalValue<V>>,
timerPrecision: Duration = 10.milliseconds,
) : ModelConstructor(context) {
init {
registerState(position)
registerState(velocity)
}
private val movementTimer = timer(timerPrecision)
private var currentForce = force.value
private val movement = movementTimer.onChange { prev, next ->
val dtSeconds = (next - prev).toDouble(DurationUnit.SECONDS)
// compute new value based on velocity and acceleration from the previous step
position.value += NumericalValue(velocity.value.value * dtSeconds + currentForce / inertia * dtSeconds.pow(2) / 2)
// compute new velocity based on acceleration on the previous step
velocity.value += NumericalValue(currentForce / inertia * dtSeconds)
currentForce = force.value
}
public companion object {
/**
* Linear inertial model with [force] in newtons and [mass] in kilograms
*/
public fun linear(
context: Context,
force: DeviceState<NumericalValue<Newtons>>,
mass: NumericalValue<Kilograms>,
position: MutableDeviceState<NumericalValue<Meters>>,
velocity: MutableDeviceState<NumericalValue<MetersPerSecond>> = MutableDeviceState(NumericalValue(0.0)),
): Inertia<Meters, MetersPerSecond> = Inertia(
context = context,
force = force.values(),
inertia = mass.value,
position = position,
velocity = velocity
)
//
//
// public fun linear(
// context: Context,
// force: DeviceState<NumericalValue<Newtons>>,
// mass: NumericalValue<Kilograms>,
// initialPosition: NumericalValue<Meters>,
// initialVelocity: NumericalValue<MetersPerSecond> = NumericalValue(0),
// ): Inertia<Meters, MetersPerSecond> = Inertia(
// context = context,
// force = force.values(),
// inertia = mass.value,
// position = MutableDeviceState(initialPosition),
// velocity = MutableDeviceState(initialVelocity)
// )
public fun circular(
context: Context,
force: DeviceState<NumericalValue<NewtonsMeters>>,
momentOfInertia: NumericalValue<KgM2>,
position: MutableDeviceState<NumericalValue<Degrees>>,
velocity: MutableDeviceState<NumericalValue<DegreesPerSecond>> = MutableDeviceState(NumericalValue(0.0)),
): Inertia<Degrees, DegreesPerSecond> = Inertia(
context = context,
force = force.values(),
inertia = momentOfInertia.value,
position = position,
velocity = velocity
)
//
// public fun circular(
// context: Context,
// force: DeviceState<NumericalValue<NewtonsMeters>>,
// momentOfInertia: NumericalValue<KgM2>,
// initialPosition: NumericalValue<Degrees>,
// initialVelocity: NumericalValue<DegreesPerSecond> = NumericalValue(0),
// ): Inertia<Degrees, DegreesPerSecond> = Inertia(
// context = context,
// force = force.values(),
// inertia = momentOfInertia.value,
// position = MutableDeviceState(initialPosition),
// velocity = MutableDeviceState(initialVelocity)
// )
}
}

View File

@ -0,0 +1,73 @@
package space.kscience.controls.constructor.models
import kotlinx.coroutines.delay
import kotlinx.coroutines.isActive
import kotlinx.coroutines.launch
import kotlinx.coroutines.sync.Mutex
import kotlinx.coroutines.sync.withLock
import space.kscience.controls.constructor.*
import space.kscience.controls.constructor.units.*
import space.kscience.controls.manager.clock
import space.kscience.dataforge.context.Context
import kotlin.time.Duration
import kotlin.time.Duration.Companion.milliseconds
import kotlin.time.DurationUnit
/**
* Pid regulator parameters
*/
public data class PidParameters(
val kp: Double,
val ki: Double,
val kd: Double,
val timeStep: Duration = 10.milliseconds,
)
/**
* A PID regulator
*
* @param P units of position values
* @param O units of output values
*/
public class PidRegulator<P : UnitsOfMeasurement, O : UnitsOfMeasurement>(
context: Context,
private val position: DeviceState<NumericalValue<P>>,
public var pidParameters: PidParameters, // TODO expose as property
output: MutableDeviceState<NumericalValue<O>> = MutableDeviceState(NumericalValue(0.0)),
private val convertOutput: (NumericalValue<P>) -> NumericalValue<O> = { NumericalValue(it.value) },
) : ModelConstructor(context) {
public val target: MutableDeviceState<NumericalValue<P>> = stateOf(NumericalValue(0.0))
public val output: MutableDeviceState<NumericalValue<O>> = registerState(output)
private val updateJob = launch {
var lastPosition: NumericalValue<P> = target.value
var integral: NumericalValue<P> = NumericalValue(0.0)
val mutex = Mutex()
val clock = context.clock
var lastTime = clock.now()
while (isActive) {
delay(pidParameters.timeStep)
mutex.withLock {
val realTime = clock.now()
val delta: NumericalValue<P> = target.value - position.value
val dtSeconds = (realTime - lastTime).toDouble(DurationUnit.SECONDS)
integral += delta * dtSeconds
val derivative = (position.value - lastPosition) / dtSeconds
//set last time and value to new values
lastTime = realTime
lastPosition = position.value
output.value =
convertOutput(pidParameters.kp * delta + pidParameters.ki * integral + pidParameters.kd * derivative)
}
}
}
}

View File

@ -0,0 +1,68 @@
package space.kscience.controls.constructor.models
import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.map
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.map
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.UnitsOfMeasurement
/**
* A state describing a [T] value in the [range]
*/
public open class RangeState<T : Comparable<T>>(
private val input: DeviceState<T>,
public val range: ClosedRange<T>,
) : DeviceState<T> {
override val valueFlow: Flow<T> get() = input.valueFlow.map { it.coerceIn(range) }
override val value: T get() = input.value.coerceIn(range)
/**
* A state showing that the range is on its lower boundary
*/
public val atStart: DeviceState<Boolean> = input.map { it <= range.start }
/**
* A state showing that the range is on its higher boundary
*/
public val atEnd: DeviceState<Boolean> = input.map { it >= range.endInclusive }
override fun toString(): String = "DoubleRangeState(value=${value},range=$range)"
}
public class MutableRangeState<T : Comparable<T>>(
private val mutableInput: MutableDeviceState<T>,
range: ClosedRange<T>,
) : RangeState<T>(mutableInput, range), MutableDeviceState<T> {
override var value: T
get() = super.value
set(value) {
mutableInput.value = value.coerceIn(range)
}
}
public fun <T : Comparable<T>> MutableRangeState(
initialValue: T,
range: ClosedRange<T>,
): MutableRangeState<T> = MutableRangeState<T>(MutableDeviceState(initialValue), range)
public fun <U : UnitsOfMeasurement> MutableRangeState(
initialValue: Double,
range: ClosedRange<Double>,
): MutableRangeState<NumericalValue<U>> = MutableRangeState(
initialValue = NumericalValue(initialValue),
range = NumericalValue<U>(range.start)..NumericalValue<U>(range.endInclusive)
)
public fun <T : Comparable<T>> DeviceState<T>.coerceIn(
range: ClosedFloatingPointRange<T>,
): RangeState<T> = RangeState(this, range)
public fun <T : Comparable<T>> MutableDeviceState<T>.coerceIn(
range: ClosedFloatingPointRange<T>,
): MutableRangeState<T> = MutableRangeState(this, range)

View File

@ -0,0 +1,25 @@
package space.kscience.controls.constructor.models
import space.kscience.controls.constructor.*
import space.kscience.controls.constructor.units.Degrees
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.units.times
import space.kscience.dataforge.context.Context
/**
* A reducer device used for simulations only (no public properties)
*/
public class Reducer(
context: Context,
public val ratio: Double,
public val input: DeviceState<NumericalValue<Degrees>>,
public val output: MutableDeviceState<NumericalValue<Degrees>>,
) : ModelConstructor(context) {
init {
registerState(input)
registerState(output)
transformTo(input, output) {
it * ratio
}
}
}

View File

@ -0,0 +1,22 @@
package space.kscience.controls.constructor.models
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.ModelConstructor
import space.kscience.controls.constructor.map
import space.kscience.controls.constructor.units.Meters
import space.kscience.controls.constructor.units.Newtons
import space.kscience.controls.constructor.units.NewtonsMeters
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.dataforge.context.Context
public class ScrewDrive(
context: Context,
public val leverage: NumericalValue<Meters>,
) : ModelConstructor(context) {
public fun transformForce(
stateOfForce: DeviceState<NumericalValue<NewtonsMeters>>,
): DeviceState<NumericalValue<Newtons>> = DeviceState.map(stateOfForce) {
NumericalValue(it.value * leverage.value)
}
}

View File

@ -1,5 +1,8 @@
package space.kscience.controls.constructor.units
import space.kscience.dataforge.meta.Meta
import space.kscience.dataforge.meta.MetaConverter
import space.kscience.dataforge.meta.double
import kotlin.jvm.JvmInline
@ -7,31 +10,46 @@ import kotlin.jvm.JvmInline
* A value without identity coupled to units of measurements.
*/
@JvmInline
public value class NumericalValue<U : UnitsOfMeasurement>(public val value: Double)
public value class NumericalValue<U : UnitsOfMeasurement>(public val value: Double): Comparable<NumericalValue<U>> {
override fun compareTo(other: NumericalValue<U>): Int = value.compareTo(other.value)
public operator fun <U: UnitsOfMeasurement> NumericalValue<U>.compareTo(other: NumericalValue<U>): Int =
value.compareTo(other.value)
}
public fun <U : UnitsOfMeasurement> NumericalValue(
number: Number,
): NumericalValue<U> = NumericalValue(number.toDouble())
public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.plus(
other: NumericalValue<U>
other: NumericalValue<U>,
): NumericalValue<U> = NumericalValue(this.value + other.value)
public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.minus(
other: NumericalValue<U>
other: NumericalValue<U>,
): NumericalValue<U> = NumericalValue(this.value - other.value)
public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.times(
c: Number
c: Number,
): NumericalValue<U> = NumericalValue(this.value * c.toDouble())
public operator fun <U : UnitsOfMeasurement> Number.times(
numericalValue: NumericalValue<U>
numericalValue: NumericalValue<U>,
): NumericalValue<U> = NumericalValue(numericalValue.value * toDouble())
public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.times(
c: Double
c: Double,
): NumericalValue<U> = NumericalValue(this.value * c)
public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.div(
c: Number
c: Number,
): NumericalValue<U> = NumericalValue(this.value / c.toDouble())
private object NumericalValueMetaConverter : MetaConverter<NumericalValue<*>> {
override fun convert(obj: NumericalValue<*>): Meta = Meta(obj.value)
override fun readOrNull(source: Meta): NumericalValue<*>? = source.double?.let { NumericalValue<Nothing>(it) }
}
@Suppress("UNCHECKED_CAST")
public fun <U : UnitsOfMeasurement> MetaConverter.Companion.numerical(): MetaConverter<NumericalValue<U>> =
NumericalValueMetaConverter as MetaConverter<NumericalValue<U>>

View File

@ -30,8 +30,31 @@ public data object Degrees : UnitsOfAngles
/**/
public interface UnitsAngularOfVelocity : UnitsOfMeasurement
public sealed interface UnitsAngularOfVelocity : UnitsOfMeasurement
public data object RadiansPerSecond : UnitsAngularOfVelocity
public data object DegreesPerSecond : UnitsAngularOfVelocity
/**/
public interface UnitsOfForce: UnitsOfMeasurement
public data object Newtons: UnitsOfForce
/**/
public interface UnitsOfTorque: UnitsOfMeasurement
public data object NewtonsMeters: UnitsOfTorque
/**/
public interface UnitsOfMass: UnitsOfMeasurement
public data object Kilograms : UnitsOfMass
/**/
public interface UnitsOfMomentOfInertia: UnitsOfMeasurement
public data object KgM2: UnitsOfMomentOfInertia

View File

@ -0,0 +1,59 @@
package space.kscience.controls.compose
import androidx.compose.foundation.layout.Row
import androidx.compose.material.icons.Icons
import androidx.compose.material.icons.filled.KeyboardArrowLeft
import androidx.compose.material.icons.filled.KeyboardArrowRight
import androidx.compose.material3.*
import androidx.compose.runtime.*
import androidx.compose.ui.Alignment
import androidx.compose.ui.Modifier
import androidx.compose.ui.graphics.Shape
import androidx.compose.ui.text.TextStyle
@Composable
public fun NumberTextField(
value: Number,
onValueChange: (Number) -> Unit,
step: Double = 0.0,
formatter: (Number) -> String = { it.toString() },
modifier: Modifier = Modifier,
enabled: Boolean = true,
textStyle: TextStyle = LocalTextStyle.current,
label: @Composable (() -> Unit)? = null,
supportingText: @Composable (() -> Unit)? = null,
shape: Shape = TextFieldDefaults.shape,
colors: TextFieldColors = TextFieldDefaults.colors(),
) {
var isError by remember { mutableStateOf(false) }
Row (verticalAlignment = Alignment.CenterVertically, modifier = modifier) {
step.takeIf { it > 0.0 }?.let {
IconButton({ onValueChange(value.toDouble() - step) }, enabled = enabled) {
Icon(Icons.Default.KeyboardArrowLeft, "decrease value")
}
}
TextField(
value = formatter(value),
onValueChange = { stringValue: String ->
val number = stringValue.toDoubleOrNull()
number?.let { onValueChange(number) }
isError = number == null
},
isError = isError,
enabled = enabled,
textStyle = textStyle,
label = label,
supportingText = supportingText,
singleLine = true,
shape = shape,
colors = colors,
modifier = Modifier.weight(1f)
)
step.takeIf { it > 0.0 }?.let {
IconButton({ onValueChange(value.toDouble() + step) }, enabled = enabled) {
Icon(Icons.Default.KeyboardArrowRight, "increase value")
}
}
}
}

View File

@ -19,15 +19,12 @@ public class TimeAxisModel(
val currentRange = rangeProvider()
val rangeLength = currentRange.endInclusive - currentRange.start
val numTicks = floor(axisLength / minimumMajorTickSpacing).toInt()
val numMinorTicks = numTicks * 2
return object : TickValues<Instant> {
override val majorTickValues: List<Instant> = List(numTicks) {
currentRange.start + it.toDouble() / (numTicks - 1) * rangeLength
}
override val minorTickValues: List<Instant> = List(numMinorTicks) {
currentRange.start + it.toDouble() / (numMinorTicks - 1) * rangeLength
}
override val minorTickValues: List<Instant> = emptyList()
}
}

View File

@ -17,6 +17,8 @@ import space.kscience.controls.api.Device
import space.kscience.controls.api.PropertyChangedMessage
import space.kscience.controls.api.propertyMessageFlow
import space.kscience.controls.constructor.DeviceState
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.constructor.values
import space.kscience.controls.manager.clock
import space.kscience.controls.misc.ValueWithTime
import space.kscience.controls.spec.DevicePropertySpec
@ -139,7 +141,7 @@ public fun XYGraphScope<Instant, Double>.PlotDeviceProperty(
@Composable
public fun XYGraphScope<Instant, Double>.PlotNumberState(
context: Context,
state: DeviceState<out Number>,
state: DeviceState<Number>,
maxAge: Duration = defaultMaxAge,
maxPoints: Int = defaultMaxPoints,
minPoints: Int = defaultMinPoints,
@ -163,6 +165,19 @@ public fun XYGraphScope<Instant, Double>.PlotNumberState(
PlotTimeSeries(points, lineStyle)
}
@Composable
public fun XYGraphScope<Instant, Double>.PlotNumericState(
context: Context,
state: DeviceState<NumericalValue<*>>,
maxAge: Duration = defaultMaxAge,
maxPoints: Int = defaultMaxPoints,
minPoints: Int = defaultMinPoints,
sampling: Duration = defaultSampling,
lineStyle: LineStyle = defaultLineStyle,
): Unit {
PlotNumberState(context, state.values(), maxAge, maxPoints, minPoints, sampling, lineStyle)
}
private fun List<Instant>.averageTime(): Instant {
val min = min()

View File

@ -23,17 +23,27 @@ import kotlinx.coroutines.flow.onEach
import kotlinx.datetime.Instant
import org.jetbrains.compose.splitpane.ExperimentalSplitPaneApi
import org.jetbrains.compose.splitpane.HorizontalSplitPane
import space.kscience.controls.compose.PlotDeviceProperty
import space.kscience.controls.compose.PlotNumberState
import space.kscience.controls.compose.NumberTextField
import space.kscience.controls.compose.PlotNumericState
import space.kscience.controls.compose.TimeAxisModel
import space.kscience.controls.constructor.*
import space.kscience.controls.constructor.library.*
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.devices.Drive
import space.kscience.controls.constructor.devices.LimitSwitch
import space.kscience.controls.constructor.devices.LinearDrive
import space.kscience.controls.constructor.models.Inertia
import space.kscience.controls.constructor.models.MutableRangeState
import space.kscience.controls.constructor.models.PidParameters
import space.kscience.controls.constructor.models.ScrewDrive
import space.kscience.controls.constructor.timer
import space.kscience.controls.constructor.units.Kilograms
import space.kscience.controls.constructor.units.Meters
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.controls.manager.ClockManager
import space.kscience.controls.manager.DeviceManager
import space.kscience.controls.manager.clock
import space.kscience.controls.manager.install
import space.kscience.dataforge.context.Context
import space.kscience.dataforge.meta.Meta
import java.awt.Dimension
import kotlin.math.PI
import kotlin.math.sin
@ -43,67 +53,79 @@ import kotlin.time.Duration.Companion.seconds
import kotlin.time.DurationUnit
class LinearDrive(
drive: Drive,
start: LimitSwitch,
end: LimitSwitch,
pidParameters: PidParameters,
meta: Meta = Meta.EMPTY,
) : DeviceConstructor(drive.context, meta) {
val drive by device(drive)
val pid by device(
PidRegulator(
context = context,
position = drive.propertyAsState(Drive.position, 0.0),
pidParameters = pidParameters
)
)
private val binding = bind(pid.output, drive.stateOfForce())
val start by device(start)
val end by device(end)
}
/**
* A shortcut to create a virtual [LimitSwitch] from [DoubleInRangeState]
*/
fun LinearDrive(
context: Context,
positionState: MutableDoubleInRangeState,
mass: Double,
pidParameters: PidParameters,
meta: Meta = Meta.EMPTY,
): LinearDrive = LinearDrive(
drive = VirtualDrive(context, mass, positionState),
start = LimitSwitch(context, positionState.atStart),
end = LimitSwitch(context, positionState.atEnd),
pidParameters = pidParameters,
meta = meta
)
class Modulator(
context: Context,
target: MutableDeviceState<Double>,
target: MutableDeviceState<NumericalValue<Meters>>,
var freq: Double = 0.1,
var timeStep: Duration = 5.milliseconds,
) : DeviceConstructor(context) {
private val clockStart = clock.now()
val timer = timer(10.milliseconds)
private val modulation = timer.onNext {
private val modulation = timer(10.milliseconds).onNext {
val timeFromStart = clock.now() - clockStart
val t = timeFromStart.toDouble(DurationUnit.SECONDS)
target.value = 5 * sin(2.0 * PI * freq * t) +
target.value = NumericalValue(
5 * sin(2.0 * PI * freq * t) +
sin(2 * PI * 21 * freq * t + 0.02 * (timeFromStart / timeStep))
)
}
}
private val inertia = NumericalValue<Kilograms>(0.1)
private val leverage = NumericalValue<Meters>(0.05)
private val maxAge = 10.seconds
private val range = -6.0..6.0
/**
* The whole physical model is here
*/
private fun createLinearDriveModel(context: Context, pidParameters: PidParameters): LinearDrive {
//create a drive model with zero starting force
val drive = Drive(context)
//a screw drive to converse a rotational moment into a linear one
val screwDrive = ScrewDrive(context, leverage)
// Create a physical position coerced in a given range
val position = MutableRangeState<Meters>(0.0, range)
/**
* Create an inertia model.
* The inertia uses drive force as input. Position is used as both input and output
*
* Force is the input parameter, position is output parameter
*
*/
val inertia = Inertia.linear(
context = context,
force = screwDrive.transformForce(drive.force),
mass = inertia,
position = position
)
/**
* Create a limit switches from physical position
*/
val startLimitSwitch = LimitSwitch(context, position.atStart)
val endLimitSwitch = LimitSwitch(context, position.atEnd)
return context.install(
"linearDrive",
LinearDrive(drive, startLimitSwitch, endLimitSwitch, position, pidParameters)
)
}
private fun createModulator(linearDrive: LinearDrive): Modulator = linearDrive.context.install(
"modulator",
Modulator(linearDrive.context, linearDrive.pid.target)
)
@OptIn(ExperimentalSplitPaneApi::class, ExperimentalKoalaPlotApi::class)
fun main() = application {
val context = remember {
@ -113,27 +135,16 @@ fun main() = application {
}
}
val clock = remember { context.clock }
var pidParameters by remember {
mutableStateOf(PidParameters(kp = 2.5, ki = 0.0, kd = -0.1, timeStep = 0.005.seconds))
mutableStateOf(PidParameters(kp = 900.0, ki = 20.0, kd = -50.0, timeStep = 0.005.seconds))
}
val state = remember { MutableDoubleInRangeState(0.0, -6.0..6.0) }
val linearDrive = remember {
context.install(
"linearDrive",
LinearDrive(context, state, 0.05, pidParameters)
)
val linearDrive: LinearDrive = remember {
createLinearDriveModel(context, pidParameters)
}
val modulator = remember {
context.install(
"modulator",
Modulator(context, linearDrive.pid.target)
)
createModulator(linearDrive)
}
//bind pid parameters
@ -145,6 +156,8 @@ fun main() = application {
}.collect()
}
val clock = remember { context.clock }
Window(title = "Pid regulator simulator", onCloseRequest = ::exitApplication) {
window.minimumSize = Dimension(800, 400)
MaterialTheme {
@ -153,48 +166,51 @@ fun main() = application {
Column(modifier = Modifier.background(color = Color.LightGray).fillMaxHeight()) {
Row {
Text("kp:", Modifier.align(Alignment.CenterVertically).width(50.dp).padding(5.dp))
TextField(
String.format("%.2f", pidParameters.kp),
{ pidParameters = pidParameters.copy(kp = it.toDouble()) },
Modifier.width(100.dp),
enabled = false
NumberTextField(
value = pidParameters.kp,
onValueChange = { pidParameters = pidParameters.copy(kp = it.toDouble()) },
formatter = { String.format("%.2f", it.toDouble()) },
step = 1.0,
modifier = Modifier.width(200.dp),
)
Slider(
pidParameters.kp.toFloat(),
{ pidParameters = pidParameters.copy(kp = it.toDouble()) },
valueRange = 0f..20f,
valueRange = 0f..1000f,
steps = 100
)
}
Row {
Text("ki:", Modifier.align(Alignment.CenterVertically).width(50.dp).padding(5.dp))
TextField(
String.format("%.2f", pidParameters.ki),
{ pidParameters = pidParameters.copy(ki = it.toDouble()) },
Modifier.width(100.dp),
enabled = false
NumberTextField(
value = pidParameters.ki,
onValueChange = { pidParameters = pidParameters.copy(ki = it.toDouble()) },
formatter = { String.format("%.2f", it.toDouble()) },
step = 0.1,
modifier = Modifier.width(200.dp),
)
Slider(
pidParameters.ki.toFloat(),
{ pidParameters = pidParameters.copy(ki = it.toDouble()) },
valueRange = -10f..10f,
valueRange = -100f..100f,
steps = 100
)
}
Row {
Text("kd:", Modifier.align(Alignment.CenterVertically).width(50.dp).padding(5.dp))
TextField(
String.format("%.2f", pidParameters.kd),
{ pidParameters = pidParameters.copy(kd = it.toDouble()) },
Modifier.width(100.dp),
enabled = false
NumberTextField(
value = pidParameters.kd,
onValueChange = { pidParameters = pidParameters.copy(kd = it.toDouble()) },
formatter = { String.format("%.2f", it.toDouble()) },
step = 0.1,
modifier = Modifier.width(200.dp),
)
Slider(
pidParameters.kd.toFloat(),
{ pidParameters = pidParameters.copy(kd = it.toDouble()) },
valueRange = -10f..10f,
valueRange = -100f..100f,
steps = 100
)
}
@ -204,7 +220,7 @@ fun main() = application {
TextField(
pidParameters.timeStep.toString(DurationUnit.MILLISECONDS),
{ pidParameters = pidParameters.copy(timeStep = it.toDouble().milliseconds) },
Modifier.width(100.dp),
Modifier.width(200.dp),
enabled = false
)
@ -233,7 +249,7 @@ fun main() = application {
ChartLayout {
XYGraph<Instant, Double>(
xAxisModel = remember { TimeAxisModel.recent(maxAge, clock) },
yAxisModel = rememberDoubleLinearAxisModel(state.range),
yAxisModel = rememberDoubleLinearAxisModel((range.start - 1.0)..(range.endInclusive + 1.0)),
xAxisTitle = { Text("Time in seconds relative to current") },
xAxisLabels = { it: Instant ->
androidx.compose.material3.Text(
@ -244,20 +260,14 @@ fun main() = application {
},
yAxisLabels = { it: Double -> Text(it.toString(2)) }
) {
PlotNumberState(
PlotNumericState(
context = context,
state = state,
state = linearDrive.position,
maxAge = maxAge,
sampling = 50.milliseconds,
lineStyle = LineStyle(SolidColor(Color.Blue))
)
PlotDeviceProperty(
linearDrive.drive,
Drive.position,
maxAge = maxAge,
sampling = 50.milliseconds,
)
PlotNumberState(
PlotNumericState(
context = context,
state = linearDrive.pid.target,
maxAge = maxAge,
@ -273,10 +283,6 @@ fun main() = application {
}
1 -> {
Text("Regulator position", color = Color.Black)
}
2 -> {
Text("Regulator target", color = Color.Red)
}
}