From d0e3faea8818d27cb80529f0a596d472bd702f42 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Sun, 2 Jun 2024 17:58:38 +0300 Subject: [PATCH] Complete rework of PID demo and underlying devices --- .../constructor/ConstructorElement.kt | 50 ++++- .../controls/constructor/DeviceState.kt | 16 +- .../controls/constructor/devices/Drive.kt | 18 ++ .../constructor/devices/EncoderDevice.kt | 20 ++ .../{library => devices}/LimitSwitch.kt | 3 +- .../constructor/devices/LinearDrive.kt | 38 ++++ .../controls/constructor/devices/StepDrive.kt | 61 ++++++ .../constructor/library/DoubleInRangeState.kt | 57 ----- .../controls/constructor/library/Drive.kt | 102 --------- .../constructor/library/PidRegulator.kt | 74 ------- .../controls/constructor/library/Regulator.kt | 17 -- .../controls/constructor/library/StepDrive.kt | 33 --- .../constructor/library/Transmission.kt | 33 --- .../controls/constructor/models/Inertia.kt | 104 +++++++++ .../constructor/models/PidRegulator.kt | 73 +++++++ .../controls/constructor/models/RangeState.kt | 68 ++++++ .../controls/constructor/models/Reducer.kt | 25 +++ .../controls/constructor/models/ScrewDrive.kt | 22 ++ .../constructor/units/NumericalValue.kt | 38 +++- .../constructor/units/UnitsOfMeasurement.kt | 27 ++- .../src/commonMain/kotlin/NumberTextField.kt | 59 +++++ .../src/commonMain/kotlin/TimeAxisModel.kt | 5 +- .../src/commonMain/kotlin/koalaPlots.kt | 17 +- .../constructor/src/jvmMain/kotlin/PidDemo.kt | 204 +++++++++--------- 24 files changed, 719 insertions(+), 445 deletions(-) create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/Drive.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/EncoderDevice.kt rename controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/{library => devices}/LimitSwitch.kt (92%) create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LinearDrive.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/StepDrive.kt delete mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/DoubleInRangeState.kt delete mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Drive.kt delete mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/PidRegulator.kt delete mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Regulator.kt delete mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/StepDrive.kt delete mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Transmission.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Inertia.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/PidRegulator.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/RangeState.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Reducer.kt create mode 100644 controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/ScrewDrive.kt create mode 100644 controls-visualisation-compose/src/commonMain/kotlin/NumberTextField.kt diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/ConstructorElement.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/ConstructorElement.kt index 0aa72a0..8073689 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/ConstructorElement.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/ConstructorElement.kt @@ -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 > StateContainer.state(state: D): D { +public fun > 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 StateContainer.stateOf(initialValue: T): MutableDeviceState = state( +public fun StateContainer.stateOf(initialValue: T): MutableDeviceState = registerState( MutableDeviceState(initialValue) ) @@ -97,23 +99,53 @@ public fun 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> = emptySet(), + reads: Collection> = 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> = emptySet(), + reads: Collection> = 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 StateContainer.mapState( origin: DeviceState, transformation: (T) -> R, -): DeviceStateWithDependencies = state(DeviceState.map(origin, transformation)) +): DeviceStateWithDependencies = registerState(DeviceState.map(origin, transformation)) public fun StateContainer.flowState( origin: DeviceState, initialValue: R, - transformation: suspend FlowCollector.(T) -> Unit + transformation: suspend FlowCollector.(T) -> Unit, ): DeviceStateWithDependencies { 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 StateContainer.combineState( first: DeviceState, second: DeviceState, transformation: (T1, T2) -> R, -): DeviceState = state(DeviceState.combine(first, second, transformation)) +): DeviceState = registerState(DeviceState.combine(first, second, transformation)) /** * Create and start binding between [sourceState] and [targetState]. Changes made to [sourceState] are automatically diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/DeviceState.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/DeviceState.kt index 5083347..c326565 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/DeviceState.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/DeviceState.kt @@ -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 { +public interface DeviceState { public val value: T public val valueFlow: Flow @@ -49,7 +51,7 @@ public interface DeviceStateWithDependencies : DeviceState { } public fun DeviceState.withDependencies( - dependencies: Collection> + dependencies: Collection>, ): DeviceStateWithDependencies = object : DeviceStateWithDependencies, DeviceState by this { override val dependencies: Collection> = dependencies } @@ -72,6 +74,16 @@ public fun DeviceState.Companion.map( public fun DeviceState.map(mapper: (T) -> R): DeviceStateWithDependencies = DeviceState.map(this, mapper) +public fun DeviceState>.values(): DeviceState = object : DeviceState { + override val value: Double + get() = this@values.value.value + + override val valueFlow: Flow + 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. */ diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/Drive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/Drive.kt new file mode 100644 index 0000000..3436856 --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/Drive.kt @@ -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> = MutableDeviceState(NumericalValue(0)), +) : DeviceConstructor(context) { + public val force: MutableDeviceState> by property(MetaConverter.numerical(), force) +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/EncoderDevice.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/EncoderDevice.kt new file mode 100644 index 0000000..cb73cff --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/EncoderDevice.kt @@ -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> +) : DeviceConstructor(context) { + public val position: DeviceState> by property(MetaConverter.numerical(), position) +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/LimitSwitch.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LimitSwitch.kt similarity index 92% rename from controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/LimitSwitch.kt rename to controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LimitSwitch.kt index 919d026..cba6b83 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/LimitSwitch.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LimitSwitch.kt @@ -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 diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LinearDrive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LinearDrive.kt new file mode 100644 index 0000000..51039d1 --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/LinearDrive.kt @@ -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>, + pidParameters: PidParameters, + context: Context = drive.context, + meta: Meta = Meta.EMPTY, +) : DeviceConstructor(context, meta) { + + public val position: DeviceState> by property(MetaConverter.numerical(), position) + + public val drive: Drive by device(drive) + public val pid: PidRegulator = 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) +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/StepDrive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/StepDrive.kt new file mode 100644 index 0000000..072789b --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/devices/StepDrive.kt @@ -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, + target: MutableDeviceState = MutableDeviceState(0), + private val writeTicks: suspend (ticks: Int, speed: Double) -> Unit, +) : DeviceConstructor(context) { + + public val target: MutableDeviceState by property(MetaConverter.int, target) + + public val speed: MutableDeviceState by property(MetaConverter.double, speed) + + private val positionState = stateOf(target.value) + + public val position: DeviceState 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, + step: NumericalValue, +): DeviceState> = position.map { zero + it * step } + diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/DoubleInRangeState.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/DoubleInRangeState.kt deleted file mode 100644 index 10773da..0000000 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/DoubleInRangeState.kt +++ /dev/null @@ -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, - public val range: ClosedFloatingPointRange, -) : DeviceState { - - override val valueFlow: Flow 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 = input.map { it <= range.start } - - /** - * A state showing that the range is on its higher boundary - */ - public val atEnd: DeviceState = input.map { it >= range.endInclusive } - - override fun toString(): String = "DoubleRangeState(value=${value},range=$range)" -} - -public class MutableDoubleInRangeState( - private val mutableInput: MutableDeviceState, - range: ClosedFloatingPointRange -) : DoubleInRangeState(mutableInput, range), MutableDeviceState { - override var value: Double - get() = super.value - set(value) { - mutableInput.value = value.coerceIn(range) - } -} - -public fun MutableDoubleInRangeState( - initialValue: Double, - range: ClosedFloatingPointRange -): MutableDoubleInRangeState = MutableDoubleInRangeState(MutableDeviceState(initialValue),range) - - -public fun DeviceState.coerceIn( - range: ClosedFloatingPointRange -): DoubleInRangeState = DoubleInRangeState(this, range) - -public fun MutableDeviceState.coerceIn( - range: ClosedFloatingPointRange -): MutableDoubleInRangeState = MutableDoubleInRangeState(this, range) \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Drive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Drive.kt deleted file mode 100644 index 77a16ca..0000000 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Drive.kt +++ /dev/null @@ -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() { - public val force: MutableDevicePropertySpec by Drive.mutableProperty( - MetaConverter.double, - Drive::force - ) - - public val position: DevicePropertySpec by doubleProperty { position } - } -} - -/** - * A virtual drive - */ -public class VirtualDrive( - context: Context, - private val mass: Double, - public val positionState: MutableDeviceState, -) : Drive, DeviceBySpec(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, - ): Factory = Factory { context, _ -> - VirtualDrive(context, mass, positionState) - } - } -} - -public fun Drive.stateOfForce(initialForce: Double = 0.0): MutableDeviceState = - propertyAsState(Drive.force, initialForce) diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/PidRegulator.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/PidRegulator.kt deleted file mode 100644 index 27c3f95..0000000 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/PidRegulator.kt +++ /dev/null @@ -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, - public var pidParameters: PidParameters, // TODO expose as property - output: MutableDeviceState = MutableDeviceState(0.0), -) : Regulator(context) { - - override val target: MutableDeviceState = stateOf(0.0) - override val output: MutableDeviceState = 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)) \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Regulator.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Regulator.kt deleted file mode 100644 index 628fb0e..0000000 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Regulator.kt +++ /dev/null @@ -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(context: Context) : DeviceConstructor(context) { - - public abstract val target: MutableDeviceState - - public abstract val output: DeviceState -} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/StepDrive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/StepDrive.kt deleted file mode 100644 index 47a9742..0000000 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/StepDrive.kt +++ /dev/null @@ -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, - public val zero: NumericalValue = NumericalValue(0.0), - direction: Direction = Direction.UP, - input: MutableDeviceState = MutableDeviceState(0), - hold: MutableDeviceState = MutableDeviceState(false) -) : Transmission>(context) { - - override val input: MutableDeviceState by property(MetaConverter.int, input) - - public val hold: MutableDeviceState by property(MetaConverter.boolean, hold) - - override val output: DeviceState> = combineState( - input, hold - ) { input, hold -> - //TODO use hold parameter - zero + input * direction.coef * step - } -} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Transmission.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Transmission.kt deleted file mode 100644 index 5974a2f..0000000 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/library/Transmission.kt +++ /dev/null @@ -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(context: Context) : DeviceConstructor(context) { - public abstract val input: MutableDeviceState - public abstract val output: DeviceState - - public companion object { - /** - * Create a device that is a hard connection between two physical quantities - */ - public suspend fun direct( - context: Context, - input: MutableDeviceState, - transform: suspend (T) -> R - ): Transmission { - val initialValue = transform(input.value) - return object : Transmission(context) { - override val input: MutableDeviceState = input - override val output: DeviceState = flowState(input, initialValue) { emit(transform(it)) } - } - } - } -} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Inertia.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Inertia.kt new file mode 100644 index 0000000..d6c3fc7 --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Inertia.kt @@ -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( + context: Context, + force: DeviceState, //TODO add system unit sets + inertia: Double, + public val position: MutableDeviceState>, + public val velocity: MutableDeviceState>, + 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>, + mass: NumericalValue, + position: MutableDeviceState>, + velocity: MutableDeviceState> = MutableDeviceState(NumericalValue(0.0)), + ): Inertia = Inertia( + context = context, + force = force.values(), + inertia = mass.value, + position = position, + velocity = velocity + ) +// +// +// public fun linear( +// context: Context, +// force: DeviceState>, +// mass: NumericalValue, +// initialPosition: NumericalValue, +// initialVelocity: NumericalValue = NumericalValue(0), +// ): Inertia = Inertia( +// context = context, +// force = force.values(), +// inertia = mass.value, +// position = MutableDeviceState(initialPosition), +// velocity = MutableDeviceState(initialVelocity) +// ) + + public fun circular( + context: Context, + force: DeviceState>, + momentOfInertia: NumericalValue, + position: MutableDeviceState>, + velocity: MutableDeviceState> = MutableDeviceState(NumericalValue(0.0)), + ): Inertia = Inertia( + context = context, + force = force.values(), + inertia = momentOfInertia.value, + position = position, + velocity = velocity + ) +// +// public fun circular( +// context: Context, +// force: DeviceState>, +// momentOfInertia: NumericalValue, +// initialPosition: NumericalValue, +// initialVelocity: NumericalValue = NumericalValue(0), +// ): Inertia = Inertia( +// context = context, +// force = force.values(), +// inertia = momentOfInertia.value, +// position = MutableDeviceState(initialPosition), +// velocity = MutableDeviceState(initialVelocity) +// ) + } +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/PidRegulator.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/PidRegulator.kt new file mode 100644 index 0000000..8c97594 --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/PidRegulator.kt @@ -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

( + context: Context, + private val position: DeviceState>, + public var pidParameters: PidParameters, // TODO expose as property + output: MutableDeviceState> = MutableDeviceState(NumericalValue(0.0)), + private val convertOutput: (NumericalValue

) -> NumericalValue = { NumericalValue(it.value) }, +) : ModelConstructor(context) { + + public val target: MutableDeviceState> = stateOf(NumericalValue(0.0)) + public val output: MutableDeviceState> = registerState(output) + + private val updateJob = launch { + var lastPosition: NumericalValue

= target.value + + var integral: NumericalValue

= 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

= 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) + } + } + } +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/RangeState.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/RangeState.kt new file mode 100644 index 0000000..c760699 --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/RangeState.kt @@ -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>( + private val input: DeviceState, + public val range: ClosedRange, +) : DeviceState { + + override val valueFlow: Flow 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 = input.map { it <= range.start } + + /** + * A state showing that the range is on its higher boundary + */ + public val atEnd: DeviceState = input.map { it >= range.endInclusive } + + override fun toString(): String = "DoubleRangeState(value=${value},range=$range)" +} + +public class MutableRangeState>( + private val mutableInput: MutableDeviceState, + range: ClosedRange, +) : RangeState(mutableInput, range), MutableDeviceState { + override var value: T + get() = super.value + set(value) { + mutableInput.value = value.coerceIn(range) + } +} + +public fun > MutableRangeState( + initialValue: T, + range: ClosedRange, +): MutableRangeState = MutableRangeState(MutableDeviceState(initialValue), range) + +public fun MutableRangeState( + initialValue: Double, + range: ClosedRange, +): MutableRangeState> = MutableRangeState( + initialValue = NumericalValue(initialValue), + range = NumericalValue(range.start)..NumericalValue(range.endInclusive) +) + + +public fun > DeviceState.coerceIn( + range: ClosedFloatingPointRange, +): RangeState = RangeState(this, range) + + +public fun > MutableDeviceState.coerceIn( + range: ClosedFloatingPointRange, +): MutableRangeState = MutableRangeState(this, range) diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Reducer.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Reducer.kt new file mode 100644 index 0000000..caba5ff --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/Reducer.kt @@ -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>, + public val output: MutableDeviceState>, +) : ModelConstructor(context) { + init { + registerState(input) + registerState(output) + transformTo(input, output) { + it * ratio + } + } +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/ScrewDrive.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/ScrewDrive.kt new file mode 100644 index 0000000..6e2f86c --- /dev/null +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/models/ScrewDrive.kt @@ -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, +) : ModelConstructor(context) { + public fun transformForce( + stateOfForce: DeviceState>, + ): DeviceState> = DeviceState.map(stateOfForce) { + NumericalValue(it.value * leverage.value) + } + +} \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/NumericalValue.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/NumericalValue.kt index 98fe418..60de195 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/NumericalValue.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/NumericalValue.kt @@ -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(public val value: Double) +public value class NumericalValue(public val value: Double): Comparable> { + override fun compareTo(other: NumericalValue): Int = value.compareTo(other.value) -public operator fun NumericalValue.compareTo(other: NumericalValue): Int = - value.compareTo(other.value) +} + +public fun NumericalValue( + number: Number, +): NumericalValue = NumericalValue(number.toDouble()) public operator fun NumericalValue.plus( - other: NumericalValue + other: NumericalValue, ): NumericalValue = NumericalValue(this.value + other.value) public operator fun NumericalValue.minus( - other: NumericalValue + other: NumericalValue, ): NumericalValue = NumericalValue(this.value - other.value) public operator fun NumericalValue.times( - c: Number + c: Number, ): NumericalValue = NumericalValue(this.value * c.toDouble()) public operator fun Number.times( - numericalValue: NumericalValue + numericalValue: NumericalValue, ): NumericalValue = NumericalValue(numericalValue.value * toDouble()) public operator fun NumericalValue.times( - c: Double + c: Double, ): NumericalValue = NumericalValue(this.value * c) public operator fun NumericalValue.div( - c: Number -): NumericalValue = NumericalValue(this.value / c.toDouble()) \ No newline at end of file + c: Number, +): NumericalValue = NumericalValue(this.value / c.toDouble()) + + +private object NumericalValueMetaConverter : MetaConverter> { + override fun convert(obj: NumericalValue<*>): Meta = Meta(obj.value) + + override fun readOrNull(source: Meta): NumericalValue<*>? = source.double?.let { NumericalValue(it) } +} + +@Suppress("UNCHECKED_CAST") +public fun MetaConverter.Companion.numerical(): MetaConverter> = + NumericalValueMetaConverter as MetaConverter> \ No newline at end of file diff --git a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/UnitsOfMeasurement.kt b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/UnitsOfMeasurement.kt index ed295ef..1264423 100644 --- a/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/UnitsOfMeasurement.kt +++ b/controls-constructor/src/commonMain/kotlin/space/kscience/controls/constructor/units/UnitsOfMeasurement.kt @@ -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 \ No newline at end of file +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 \ No newline at end of file diff --git a/controls-visualisation-compose/src/commonMain/kotlin/NumberTextField.kt b/controls-visualisation-compose/src/commonMain/kotlin/NumberTextField.kt new file mode 100644 index 0000000..68e61ca --- /dev/null +++ b/controls-visualisation-compose/src/commonMain/kotlin/NumberTextField.kt @@ -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") + } + } + } +} \ No newline at end of file diff --git a/controls-visualisation-compose/src/commonMain/kotlin/TimeAxisModel.kt b/controls-visualisation-compose/src/commonMain/kotlin/TimeAxisModel.kt index 2c82802..65ce8d9 100644 --- a/controls-visualisation-compose/src/commonMain/kotlin/TimeAxisModel.kt +++ b/controls-visualisation-compose/src/commonMain/kotlin/TimeAxisModel.kt @@ -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 { override val majorTickValues: List = List(numTicks) { currentRange.start + it.toDouble() / (numTicks - 1) * rangeLength } - override val minorTickValues: List = List(numMinorTicks) { - currentRange.start + it.toDouble() / (numMinorTicks - 1) * rangeLength - } + override val minorTickValues: List = emptyList() } } diff --git a/controls-visualisation-compose/src/commonMain/kotlin/koalaPlots.kt b/controls-visualisation-compose/src/commonMain/kotlin/koalaPlots.kt index d721f0a..855de10 100644 --- a/controls-visualisation-compose/src/commonMain/kotlin/koalaPlots.kt +++ b/controls-visualisation-compose/src/commonMain/kotlin/koalaPlots.kt @@ -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.PlotDeviceProperty( @Composable public fun XYGraphScope.PlotNumberState( context: Context, - state: DeviceState, + state: DeviceState, maxAge: Duration = defaultMaxAge, maxPoints: Int = defaultMaxPoints, minPoints: Int = defaultMinPoints, @@ -163,6 +165,19 @@ public fun XYGraphScope.PlotNumberState( PlotTimeSeries(points, lineStyle) } +@Composable +public fun XYGraphScope.PlotNumericState( + context: Context, + state: DeviceState>, + 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.averageTime(): Instant { val min = min() diff --git a/demo/constructor/src/jvmMain/kotlin/PidDemo.kt b/demo/constructor/src/jvmMain/kotlin/PidDemo.kt index fb0ab64..9129a1c 100644 --- a/demo/constructor/src/jvmMain/kotlin/PidDemo.kt +++ b/demo/constructor/src/jvmMain/kotlin/PidDemo.kt @@ -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, + target: MutableDeviceState>, 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) + - sin(2 * PI * 21 * freq * t + 0.02 * (timeFromStart / timeStep)) + target.value = NumericalValue( + 5 * sin(2.0 * PI * freq * t) + + sin(2 * PI * 21 * freq * t + 0.02 * (timeFromStart / timeStep)) + ) } } +private val inertia = NumericalValue(0.1) + +private val leverage = NumericalValue(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(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( 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) } }