Fix step drive demo

This commit is contained in:
Alexander Nozik 2024-06-03 13:44:20 +03:00
parent d0e3faea88
commit a2b7d1ecb0
13 changed files with 298 additions and 61 deletions

View File

@ -32,12 +32,12 @@ public abstract class DeviceConstructor(
_constructorElements.remove(constructorElement)
}
override fun <T, S: DeviceState<T>> registerAsProperty(
override fun <T, S: DeviceState<T>> registerProperty(
converter: MetaConverter<T>,
descriptor: PropertyDescriptor,
state: S,
): S {
val res = super.registerAsProperty(converter, descriptor, state)
val res = super.registerProperty(converter, descriptor, state)
registerElement(PropertyConstructorElement(this, descriptor.name, state))
return res
}
@ -84,7 +84,7 @@ public fun <T, S : DeviceState<T>> DeviceConstructor.property(
PropertyDelegateProvider { _: DeviceConstructor, property ->
val name = nameOverride ?: property.name
val descriptor = PropertyDescriptor(name).apply(descriptorBuilder)
registerAsProperty(converter, descriptor, state)
registerProperty(converter, descriptor, state)
ReadOnlyProperty { _: DeviceConstructor, _ ->
state
}
@ -145,6 +145,6 @@ public fun <T, S : DeviceState<T>> DeviceConstructor.registerAsProperty(
spec: DevicePropertySpec<*, T>,
state: S,
): S {
registerAsProperty(spec.converter, spec.descriptor, state)
registerProperty(spec.converter, spec.descriptor, state)
return state
}

View File

@ -42,10 +42,15 @@ public open class DeviceGroup(
}
}
private class Action(
val invoke: suspend (Meta?) -> Meta?,
private class Action<T, R>(
val inputConverter: MetaConverter<T>,
val outputConverter: MetaConverter<R>,
val descriptor: ActionDescriptor,
)
val action: suspend (T) -> R,
) {
suspend operator fun invoke(argument: Meta?): Meta? = argument?.let { inputConverter.readOrNull(it) }
?.let { action(it)?.let { outputConverter.convert(it) } }
}
private val sharedMessageFlow = MutableSharedFlow<DeviceMessage>()
@ -93,7 +98,7 @@ public open class DeviceGroup(
/**
* Register a new property based on [DeviceState]. Properties could be modified dynamically
*/
public open fun <T, S : DeviceState<T>> registerAsProperty(
public open fun <T, S : DeviceState<T>> registerProperty(
converter: MetaConverter<T>,
descriptor: PropertyDescriptor,
state: S,
@ -112,7 +117,26 @@ public open class DeviceGroup(
return state
}
private val actions: MutableMap<Name, Action> = hashMapOf()
private val actions: MutableMap<Name, Action<*, *>> = hashMapOf()
public fun <T, R> registerAction(
inputConverter: MetaConverter<T>,
outputConverter: MetaConverter<R>,
descriptor: ActionDescriptor,
action: suspend (T) -> R,
): suspend (T) -> R {
val name = descriptor.name.parseAsName()
require(actions[name] == null) { "Can't add action with name $name. It already exists." }
actions[name] = Action(
inputConverter = inputConverter,
outputConverter = outputConverter,
descriptor = descriptor,
action = action
)
return {
action(it)
}
}
override val propertyDescriptors: Collection<PropertyDescriptor>
get() = properties.values.map { it.descriptor }
@ -137,8 +161,8 @@ public open class DeviceGroup(
override suspend fun execute(actionName: String, argument: Meta?): Meta? {
val action = actions[actionName] ?: error("Action with name $actionName not found")
return action.invoke(argument)
val action: Action<*, *> = actions[actionName] ?: error("Action with name $actionName not found")
return action(argument)
}
final override var lifecycleState: DeviceLifecycleState = DeviceLifecycleState.STOPPED
@ -176,7 +200,7 @@ public open class DeviceGroup(
}
public fun <T> DeviceGroup.registerAsProperty(propertySpec: DevicePropertySpec<*, T>, state: DeviceState<T>) {
registerAsProperty(propertySpec.converter, propertySpec.descriptor, state)
registerProperty(propertySpec.converter, propertySpec.descriptor, state)
}
public fun DeviceManager.registerDeviceGroup(
@ -253,7 +277,7 @@ public fun <T : Any> DeviceGroup.registerAsProperty(
state: DeviceState<T>,
descriptorBuilder: PropertyDescriptor.() -> Unit = {},
) {
registerAsProperty(
registerProperty(
converter,
PropertyDescriptor(name).apply(descriptorBuilder),
state
@ -269,7 +293,7 @@ public fun <T : Any> DeviceGroup.registerMutableProperty(
state: MutableDeviceState<T>,
descriptorBuilder: PropertyDescriptor.() -> Unit = {},
) {
registerAsProperty(
registerProperty(
converter,
PropertyDescriptor(name).apply(descriptorBuilder),
state

View File

@ -74,7 +74,7 @@ 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> {
public fun DeviceState<NumericalValue<out UnitsOfMeasurement>>.values(): DeviceState<Double> = object : DeviceState<Double> {
override val value: Double
get() = this@values.value.value

View File

@ -1,6 +1,5 @@
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
@ -16,46 +15,46 @@ import kotlin.time.DurationUnit
/**
* A step drive
*
* @param speed ticks per second
* @param ticksPerSecond ticks per second
* @param target target ticks state
* @param writeTicks a hardware callback
*/
public class StepDrive(
context: Context,
speed: MutableDeviceState<Double>,
ticksPerSecond: MutableDeviceState<Double>,
target: MutableDeviceState<Int> = MutableDeviceState(0),
private val writeTicks: suspend (ticks: Int, speed: Double) -> Unit,
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)
public val speed: MutableDeviceState<Double> by property(MetaConverter.double, ticksPerSecond)
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
private val ticker = onTimer(reads = setOf(target, position), writes = setOf(position)) { prev, next ->
val tickSpeed = ticksPerSecond.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())
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 }
zero: NumericalValue<Degrees> = NumericalValue(0),
): DeviceState<NumericalValue<Degrees>> = position.map {
zero + it.toDouble() * step
}

View File

@ -10,7 +10,7 @@ private class StateFlowAsState<T>(
override var value: T by flow::value
override val valueFlow: Flow<T> get() = flow
override fun toString(): String = "FlowAsState()"
override fun toString(): String = "FlowAsState($value)"
}
/**

View File

@ -23,7 +23,7 @@ private class VirtualDeviceState<T>(
callback(value)
}
override fun toString(): String = "VirtualDeviceState()"
override fun toString(): String = "VirtualDeviceState($value)"
}

View File

@ -16,7 +16,9 @@ public open class RangeState<T : Comparable<T>>(
public val range: ClosedRange<T>,
) : DeviceState<T> {
override val valueFlow: Flow<T> get() = input.valueFlow.map { it.coerceIn(range) }
override val valueFlow: Flow<T> get() = input.valueFlow.map {
it.coerceIn(range)
}
override val value: T get() = input.value.coerceIn(range)
@ -59,10 +61,10 @@ public fun <U : UnitsOfMeasurement> MutableRangeState(
public fun <T : Comparable<T>> DeviceState<T>.coerceIn(
range: ClosedFloatingPointRange<T>,
range: ClosedRange<T>,
): RangeState<T> = RangeState(this, range)
public fun <T : Comparable<T>> MutableDeviceState<T>.coerceIn(
range: ClosedFloatingPointRange<T>,
range: ClosedRange<T>,
): MutableRangeState<T> = MutableRangeState(this, range)

View File

@ -3,20 +3,26 @@ 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.controls.constructor.units.*
import space.kscience.dataforge.context.Context
import kotlin.math.PI
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)
NumericalValue(it.value * leverage.value/2/ PI)
}
public fun transformOffset(
stateOfAngle: DeviceState<NumericalValue<Degrees>>,
offset: NumericalValue<Meters> = NumericalValue(0),
): DeviceState<NumericalValue<Meters>> = DeviceState.map(stateOfAngle) {
offset + NumericalValue(it.value * leverage.value/2/ PI)
}
}

View File

@ -0,0 +1,17 @@
package space.kscience.controls.constructor.models
import space.kscience.controls.constructor.ModelConstructor
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.stateOf
import space.kscience.controls.constructor.units.Meters
import space.kscience.controls.constructor.units.NumericalValue
import space.kscience.dataforge.context.Context
public class XYPosition(
context: Context,
initialX: NumericalValue<Meters> = NumericalValue(0.0),
initialY: NumericalValue<Meters> = NumericalValue(0.0),
) : ModelConstructor(context) {
public val x: MutableDeviceState<NumericalValue<Meters>> = stateOf(initialX)
public val y: MutableDeviceState<NumericalValue<Meters>> = stateOf(initialY)
}

View File

@ -10,7 +10,7 @@ import kotlin.jvm.JvmInline
* A value without identity coupled to units of measurements.
*/
@JvmInline
public value class NumericalValue<U : UnitsOfMeasurement>(public val value: Double): Comparable<NumericalValue<U>> {
public value class NumericalValue<U : UnitsOfMeasurement>(public val value: Double) : Comparable<NumericalValue<U>> {
override fun compareTo(other: NumericalValue<U>): Int = value.compareTo(other.value)
}
@ -43,6 +43,9 @@ public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.div(
c: Number,
): NumericalValue<U> = NumericalValue(this.value / c.toDouble())
public operator fun <U : UnitsOfMeasurement> NumericalValue<U>.div(other: NumericalValue<U>): Double =
value / other.value
private object NumericalValueMetaConverter : MetaConverter<NumericalValue<*>> {
override fun convert(obj: NumericalValue<*>): Meta = Meta(obj.value)

View File

@ -23,19 +23,19 @@ import kotlin.time.Duration.Companion.milliseconds
import kotlin.time.DurationUnit
@Serializable
data class XY(val x: Double, val y: Double) {
private data class XY(val x: Double, val y: Double) {
companion object {
val ZERO = XY(0.0, 0.0)
}
}
val XY.length: Double get() = sqrt(x.pow(2) + y.pow(2))
private val XY.length: Double get() = sqrt(x.pow(2) + y.pow(2))
operator fun XY.plus(other: XY): XY = XY(x + other.x, y + other.y)
operator fun XY.times(c: Double): XY = XY(x * c, y * c)
operator fun XY.div(c: Double): XY = XY(x / c, y / c)
private operator fun XY.plus(other: XY): XY = XY(x + other.x, y + other.y)
private operator fun XY.times(c: Double): XY = XY(x * c, y * c)
private operator fun XY.div(c: Double): XY = XY(x / c, y / c)
class Spring(
private class Spring(
context: Context,
val k: Double,
val l0: Double,
@ -70,7 +70,7 @@ class Spring(
}
}
class MaterialPoint(
private class MaterialPoint(
context: Context,
val mass: Double,
val force: DeviceState<XY>,
@ -94,7 +94,7 @@ class MaterialPoint(
}
class BodyOnSprings(
private class BodyOnSprings(
context: Context,
mass: Double,
k: Double,

View File

@ -72,7 +72,7 @@ class Modulator(
}
private val inertia = NumericalValue<Kilograms>(0.1)
private val mass = NumericalValue<Kilograms>(0.1)
private val leverage = NumericalValue<Meters>(0.05)
@ -83,7 +83,13 @@ private val range = -6.0..6.0
/**
* The whole physical model is here
*/
private fun createLinearDriveModel(context: Context, pidParameters: PidParameters): LinearDrive {
internal fun createLinearDriveModel(
context: Context,
pidParameters: PidParameters,
mass: NumericalValue<Kilograms>,
leverage: NumericalValue<Meters>,
position: MutableRangeState<NumericalValue<Meters>>,
): LinearDrive {
//create a drive model with zero starting force
val drive = Drive(context)
@ -91,8 +97,6 @@ private fun createLinearDriveModel(context: Context, pidParameters: PidParameter
//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.
@ -101,10 +105,10 @@ private fun createLinearDriveModel(context: Context, pidParameters: PidParameter
* Force is the input parameter, position is output parameter
*
*/
val inertia = Inertia.linear(
val inertiaModel = Inertia.linear(
context = context,
force = screwDrive.transformForce(drive.force),
mass = inertia,
mass = mass,
position = position
)
@ -114,12 +118,12 @@ private fun createLinearDriveModel(context: Context, pidParameters: PidParameter
val startLimitSwitch = LimitSwitch(context, position.atStart)
val endLimitSwitch = LimitSwitch(context, position.atEnd)
return context.install(
"linearDrive",
LinearDrive(drive, startLimitSwitch, endLimitSwitch, position, pidParameters)
)
}
/**
* Install the resulting device
*/
return LinearDrive(drive, startLimitSwitch, endLimitSwitch, position, pidParameters)
}
private fun createModulator(linearDrive: LinearDrive): Modulator = linearDrive.context.install(
"modulator",
@ -140,11 +144,21 @@ fun main() = application {
}
val linearDrive: LinearDrive = remember {
createLinearDriveModel(context, pidParameters)
context.install(
"linearDrive",
createLinearDriveModel(
context = context,
pidParameters = pidParameters,
mass = mass,
leverage = leverage,
// Create a physical position coerced in a given range
position = MutableRangeState<Meters>(0.0, range)
)
)
}
val modulator = remember {
createModulator(linearDrive)
context.install("modulator", createModulator(linearDrive))
}
//bind pid parameters

View File

@ -1,2 +1,174 @@
package space.kscience.controls.demo.constructor
import androidx.compose.foundation.Canvas
import androidx.compose.foundation.layout.fillMaxSize
import androidx.compose.material.MaterialTheme
import androidx.compose.runtime.*
import androidx.compose.ui.Modifier
import androidx.compose.ui.geometry.Offset
import androidx.compose.ui.geometry.Size
import androidx.compose.ui.graphics.Color
import androidx.compose.ui.window.Window
import androidx.compose.ui.window.application
import kotlinx.coroutines.delay
import kotlinx.coroutines.isActive
import kotlinx.coroutines.launch
import space.kscience.controls.constructor.DeviceConstructor
import space.kscience.controls.constructor.MutableDeviceState
import space.kscience.controls.constructor.device
import space.kscience.controls.constructor.devices.StepDrive
import space.kscience.controls.constructor.devices.angle
import space.kscience.controls.constructor.models.RangeState
import space.kscience.controls.constructor.models.ScrewDrive
import space.kscience.controls.constructor.models.coerceIn
import space.kscience.controls.constructor.units.*
import space.kscience.controls.manager.ClockManager
import space.kscience.controls.manager.DeviceManager
import space.kscience.dataforge.context.Context
import java.awt.Dimension
import kotlin.random.Random
class Plotter(
context: Context,
xDrive: StepDrive,
yDrive: StepDrive,
val paint: suspend (Color) -> Unit,
) : DeviceConstructor(context) {
val xDrive by device(xDrive)
val yDrive by device(yDrive)
public fun moveToXY(x: Int, y: Int) {
xDrive.target.value = x
yDrive.target.value = y
}
//TODO add calibration
// TODO add draw as action
}
suspend fun Plotter.modernArt(xRange: IntRange, yRange: IntRange) {
while (isActive){
val randomX = Random.nextInt(xRange.first, xRange.last)
val randomY = Random.nextInt(xRange.first, xRange.last)
moveToXY(randomX, randomY)
delay(500)
paint(Color(Random.nextInt()))
}
}
suspend fun Plotter.square(xRange: IntRange, yRange: IntRange) {
while (isActive) {
moveToXY(xRange.first, yRange.first)
delay(1000)
paint(Color.Red)
moveToXY(xRange.first, yRange.last)
delay(1000)
paint(Color.Red)
moveToXY(xRange.last, yRange.last)
delay(1000)
paint(Color.Red)
moveToXY(xRange.last, yRange.first)
delay(1000)
paint(Color.Red)
}
}
private val xRange = NumericalValue<Meters>(-0.5)..NumericalValue<Meters>(0.5)
private val yRange = NumericalValue<Meters>(-0.5)..NumericalValue<Meters>(0.5)
private val ticksPerSecond = MutableDeviceState(250.0)
private val step = NumericalValue<Degrees>(1.2)
private data class PlotterPoint(
val x: NumericalValue<Meters>,
val y: NumericalValue<Meters>,
val color: Color = Color.Black,
)
suspend fun main() = application {
Window(title = "Pid regulator simulator", onCloseRequest = ::exitApplication) {
window.minimumSize = Dimension(400, 400)
val points = remember { mutableStateListOf<PlotterPoint>() }
var position by remember { mutableStateOf(PlotterPoint(NumericalValue(0), NumericalValue(0))) }
LaunchedEffect(Unit) {
val context = Context {
plugin(DeviceManager)
plugin(ClockManager)
}
/* Here goes the device definition block */
val xScrewDrive = ScrewDrive(context, NumericalValue(0.01))
val xDrive = StepDrive(context, ticksPerSecond)
val x: RangeState<NumericalValue<Meters>> = xScrewDrive.transformOffset(xDrive.angle(step)).coerceIn(xRange)
val yScrewDrive = ScrewDrive(context, NumericalValue(0.01))
val yDrive = StepDrive(context, ticksPerSecond)
val y: RangeState<NumericalValue<Meters>> = yScrewDrive.transformOffset(yDrive.angle(step)).coerceIn(yRange)
val plotter = Plotter(context, xDrive, yDrive) { color ->
println("Point X: ${x.value.value}, Y: ${y.value.value}, color: $color")
points.add(PlotterPoint(x.value, y.value, color))
}
/* Start visualization program */
launch {
x.valueFlow.collect {
position = position.copy(x = it)
}
}
launch {
y.valueFlow.collect {
position = position.copy(y = it)
}
}
launch {
val range = -100..100
plotter.modernArt(range, range)
//plotter.square(range, range)
}
}
/* Here goes the visualization block */
MaterialTheme {
Canvas(modifier = Modifier.fillMaxSize()) {
fun toOffset(x: NumericalValue<Meters>, y: NumericalValue<Meters>): Offset {
val canvasX = (x - xRange.start) / (xRange.endInclusive - xRange.start) * size.width
val canvasY = (y - yRange.start) / (yRange.endInclusive - yRange.start) * size.height
return Offset(canvasX.toFloat(), canvasY.toFloat())
}
val center = toOffset(position.x, position.y)
drawRect(
Color.LightGray,
topLeft = Offset(0f, center.y - 5f),
size = Size(size.width, 10f)
)
drawCircle(Color.Black, radius = 10f, center = center)
points.forEach {
drawCircle(it.color, radius = 2f, center = toOffset(it.x, it.y))
}
}
}
}
}