From fe60d7043f59d64f6697686f840a14d9ebab3405 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sat, 21 Jan 2023 19:37:51 -0500 Subject: [PATCH] inline value and mechanism sensors --- build.gradle | 2 +- .../org/team4099/lib/units/MechanismUnits.kt | 70 ++++++++++--------- .../kotlin/org/team4099/lib/units/Value.kt | 30 ++++---- 3 files changed, 54 insertions(+), 48 deletions(-) diff --git a/build.gradle b/build.gradle index 4134168..14a5951 100644 --- a/build.gradle +++ b/build.gradle @@ -51,7 +51,7 @@ publishing { release(MavenPublication) { groupId = 'org.team4099' artifactId = 'falconutils' - version = '1.1.0' + version = '1.1.1' from(components["kotlin"]) } diff --git a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt index 595d465..5396545 100644 --- a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt +++ b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt @@ -72,44 +72,45 @@ interface MechanismSensor { } class LinearMechanismSensor( - private val diameter: Length, - private val ratio: Double, - private val timescale: Timescale, - private val fullPowerThrottle: Double, - private val compensationVoltage: ElectricalPotential, + val diameter: Length, + val ratio: Double, + val timescale: Timescale, + val fullPowerThrottle: Double, + val compensationVoltage: ElectricalPotential, val getRawVelocity: () -> Double, val getRawPosition: () -> Double ) : MechanismSensor { - override val position: Length + override inline val position: Length get() = (diameter * PI) * getRawPosition() * ratio - override val velocity: Value> + override inline val velocity: Value> get() { val linearUnscaledVelocity = diameter.inMeters * getRawVelocity() * ratio * PI return (linearUnscaledVelocity / timescale.velocity.inSeconds).meters.perSecond } - override fun positionToRawUnits(position: Value): Double = position / diameter / ratio / PI + override inline fun positionToRawUnits(position: Value): Double = + position / diameter / ratio / PI - override fun velocityToRawUnits(velocity: Value>): Double { + override inline fun velocityToRawUnits(velocity: Value>): Double { val linearUnscaledVelocity = velocity.inMetersPerSecond * timescale.velocity.inSeconds return linearUnscaledVelocity / diameter.inMeters / ratio / PI } - override fun accelerationToRawUnits(acceleration: Value>): Double { + override inline fun accelerationToRawUnits(acceleration: Value>): Double { val linearUnscaledVelocity = velocity.inMetersPerSecond * timescale.velocity.inSeconds * timescale.acceleration.inSeconds return linearUnscaledVelocity / diameter.inMeters / ratio / PI } - override fun proportionalPositionGainToRawUnits( + override inline fun proportionalPositionGainToRawUnits( proportionalGain: ProportionalGain ): Double { return (proportionalGain.inVoltsPerMeter / (positionToRawUnits(1.meters))) / compensationVoltage.inVolts * fullPowerThrottle } - override fun integralPositionGainToRawUnits( + override inline fun integralPositionGainToRawUnits( integralGain: IntegralGain, ): Double { return ( @@ -119,7 +120,7 @@ class LinearMechanismSensor( compensationVoltage.inVolts * fullPowerThrottle } - override fun derivativePositionGainToRawUnits( + override inline fun derivativePositionGainToRawUnits( derivativeGain: DerivativeGain, ): Double { return ( @@ -128,14 +129,14 @@ class LinearMechanismSensor( ) / compensationVoltage.inVolts * fullPowerThrottle } - override fun proportionalVelocityGainToRawUnits( + override inline fun proportionalVelocityGainToRawUnits( proportionalGain: ProportionalGain, Volt> ): Double { return (proportionalGain.inVoltsPerMetersPerSecond / (velocityToRawUnits(1.meters.perSecond))) / compensationVoltage.inVolts * fullPowerThrottle } - override fun integralVelocityGainToRawUnits( + override inline fun integralVelocityGainToRawUnits( integralGain: IntegralGain, Volt>, ): Double { return ( @@ -145,7 +146,7 @@ class LinearMechanismSensor( compensationVoltage.inVolts * fullPowerThrottle } - override fun derivativeVelocityGainToRawUnits( + override inline fun derivativeVelocityGainToRawUnits( derivativeGain: DerivativeGain, Volt>, ): Double { return ( @@ -154,7 +155,7 @@ class LinearMechanismSensor( ) / compensationVoltage.inVolts * fullPowerThrottle } - override fun velocityFeedforwardToRawUnits( + override inline fun velocityFeedforwardToRawUnits( velocityFeedforward: VelocityFeedforward ): Double { return velocityFeedforward.value * velocityToRawUnits(1.0.meters.perSecond) / @@ -163,39 +164,42 @@ class LinearMechanismSensor( } class AngularMechanismSensor( - private val ratio: Double, - private val timescale: Timescale, - private val fullPowerThrottle: Double, - private val compensationVoltage: ElectricalPotential, + val ratio: Double, + val timescale: Timescale, + val fullPowerThrottle: Double, + val compensationVoltage: ElectricalPotential, val getRawVelocity: () -> Double, val getRawPosition: () -> Double ) : MechanismSensor { - override val position: Value + override inline val position: Value get() = (getRawPosition() * ratio).rotations - override val velocity: Value> + override inline val velocity: Value> get() = (getRawVelocity() * ratio / timescale.velocity.inSeconds).rotations.perSecond - override fun positionToRawUnits(position: Value): Double = position.inRotations / ratio + override inline fun positionToRawUnits(position: Value): Double = + position.inRotations / ratio - override fun velocityToRawUnits(velocity: Value>): Double = + override inline fun velocityToRawUnits(velocity: Value>): Double = (velocity.inRotationsPerSecond * timescale.velocity.inSeconds) / ratio - override fun accelerationToRawUnits(acceleration: Value>): Double = + override inline fun accelerationToRawUnits(acceleration: Value>): Double = ( acceleration.inRotationsPerSecondPerSecond * timescale.velocity.inSeconds * timescale.acceleration.inSeconds ) / ratio - override fun proportionalPositionGainToRawUnits( + override inline fun proportionalPositionGainToRawUnits( proportionalGain: ProportionalGain ): Double { return (proportionalGain.inVoltsPerRadian / (positionToRawUnits(1.radians))) / compensationVoltage.inVolts * fullPowerThrottle } - override fun integralPositionGainToRawUnits(integralGain: IntegralGain): Double { + override inline fun integralPositionGainToRawUnits( + integralGain: IntegralGain + ): Double { return ( integralGain.inVoltsPerRadianSeconds / (positionToRawUnits(1.radians) * timescale.velocity.inSeconds) @@ -203,7 +207,7 @@ class AngularMechanismSensor( compensationVoltage.inVolts * fullPowerThrottle } - override fun derivativePositionGainToRawUnits( + override inline fun derivativePositionGainToRawUnits( derivativeGain: DerivativeGain ): Double { return ( @@ -212,7 +216,7 @@ class AngularMechanismSensor( ) / compensationVoltage.inVolts * fullPowerThrottle } - override fun proportionalVelocityGainToRawUnits( + override inline fun proportionalVelocityGainToRawUnits( proportionalGain: ProportionalGain, Volt> ): Double { return ( @@ -221,7 +225,7 @@ class AngularMechanismSensor( ) / compensationVoltage.inVolts * fullPowerThrottle } - override fun integralVelocityGainToRawUnits( + override inline fun integralVelocityGainToRawUnits( integralGain: IntegralGain, Volt> ): Double { return ( @@ -231,7 +235,7 @@ class AngularMechanismSensor( compensationVoltage.inVolts * fullPowerThrottle } - override fun derivativeVelocityGainToRawUnits( + override inline fun derivativeVelocityGainToRawUnits( derivativeGain: DerivativeGain, Volt> ): Double { return ( @@ -240,7 +244,7 @@ class AngularMechanismSensor( ) / compensationVoltage.inVolts * fullPowerThrottle } - override fun velocityFeedforwardToRawUnits( + override inline fun velocityFeedforwardToRawUnits( velocityFeedforward: VelocityFeedforward ): Double { return velocityFeedforward.value * velocityToRawUnits(1.0.radians.perSecond) / diff --git a/src/main/kotlin/org/team4099/lib/units/Value.kt b/src/main/kotlin/org/team4099/lib/units/Value.kt index c8f7626..21d3627 100644 --- a/src/main/kotlin/org/team4099/lib/units/Value.kt +++ b/src/main/kotlin/org/team4099/lib/units/Value.kt @@ -5,31 +5,33 @@ import kotlin.math.sign @JvmInline value class Value(val value: Double) : Comparable> { - val absoluteValue: Value + inline val absoluteValue: Value get() = Value(value.absoluteValue) - val sign: Double + inline val sign: Double get() = sign(value) - operator fun plus(o: Value): Value = Value(value + o.value) - operator fun minus(o: Value): Value = Value(value - o.value) + inline operator fun plus(o: Value): Value = Value(value + o.value) + inline operator fun minus(o: Value): Value = Value(value - o.value) - operator fun times(k: Double): Value = Value(value * k) - operator fun times(k: Number): Value = this * k.toDouble() - operator fun times(o: Value>): Value = Value(value * o.value) + inline operator fun times(k: Double): Value = Value(value * k) + inline operator fun times(k: Number): Value = this * k.toDouble() + inline operator fun times(o: Value>): Value = + Value(value * o.value) @Suppress("INAPPLICABLE_JVM_NAME") @JvmName("times1") - operator fun times(o: Value): Value> = Value(value * o.value) + inline operator fun times(o: Value): Value> = + Value(value * o.value) - operator fun unaryMinus(): Value = Value(-value) + inline operator fun unaryMinus(): Value = Value(-value) - operator fun div(k: Double): Value = Value(value / k) - operator fun div(k: Number): Value = this / k.toDouble() - operator fun div(o: Value): Double = value / o.value - operator fun div(o: Value): Value> = Value(value / o.value) + inline operator fun div(k: Double): Value = Value(value / k) + inline operator fun div(k: Number): Value = this / k.toDouble() + inline operator fun div(o: Value): Double = value / o.value + inline operator fun div(o: Value): Value> = Value(value / o.value) - override operator fun compareTo(other: Value): Int = value.compareTo(other.value) + override inline operator fun compareTo(other: Value): Int = value.compareTo(other.value) } infix fun ClosedRange>.step(step: Value): Iterable> {