Skip to content

Commit

Permalink
gear ratio units + bug fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
sswadkar committed Jan 28, 2023
1 parent fbbfc92 commit ac77d8d
Show file tree
Hide file tree
Showing 5 changed files with 95 additions and 7 deletions.
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ publishing {
release(MavenPublication) {
groupId = 'org.team4099'
artifactId = 'falconutils'
version = '1.1.5'
version = '1.1.6'

from(components["kotlin"])
}
Expand Down
27 changes: 21 additions & 6 deletions src/main/kotlin/org/team4099/lib/units/derived/Controller.kt
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ inline val ProportionalGain<Meter, Volt>.inVoltsPerFoot: Double
inline val ProportionalGain<Radian, Volt>.inVoltsPerRadian: Double
get() = value

inline val ProportionalGain<Radian, Volt>.inVoltsPerDegrees: Double
inline val ProportionalGain<Radian, Volt>.inVoltsPerDegree: Double
get() = inVoltsPerRadian * RADIANS_PER_DEGREES

inline val ProportionalGain<Radian, Volt>.inVoltsPerRotation: Double
Expand All @@ -133,6 +133,9 @@ inline val ProportionalGain<Velocity<Meter>, Volt>.inVoltsPerMetersPerSecond: Do
inline val ProportionalGain<Velocity<Radian>, Volt>.inVoltsPerRadiansPerSecond: Double
get() = value

inline val ProportionalGain<Velocity<Radian>, Volt>.inVoltsPerRotationPerMinute: Double
get() = inVoltsPerRadiansPerSecond * RADIANS_PER_ROTATION / SECONDS_PER_MINUTE

inline val IntegralGain<Meter, Volt>.inVoltsPerMeterSeconds: Double
get() = value

Expand All @@ -151,12 +154,18 @@ inline val IntegralGain<Radian, Volt>.inVoltsPerRadianSeconds: Double
inline val IntegralGain<Radian, Volt>.inVoltsPerDegreeSeconds: Double
get() = inVoltsPerRadianSeconds * RADIANS_PER_DEGREES

inline val IntegralGain<Radian, Volt>.inVoltsPerRotationsMinutes: Double
get() = inVoltsPerRadianSeconds * SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
inline val IntegralGain<Radian, Volt>.inVoltsPerRotationSeconds: Double
get() = inVoltsPerRadianSeconds / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val IntegralGain<Velocity<Radian>, Volt>.inVoltsPerRadians: Double
get() = value

inline val IntegralGain<Velocity<Radian>, Volt>.inVoltsPerDegrees: Double
get() = inVoltsPerRadians * RADIANS_PER_DEGREES

inline val IntegralGain<Velocity<Radian>, Volt>.inVoltsPerRotations: Double
get() = inVoltsPerRadians / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val DerivativeGain<Meter, Volt>.inVoltsPerMeterPerSecond: Double
get() = value

Expand All @@ -176,11 +185,17 @@ inline val DerivativeGain<Radian, Volt>.inVoltsPerDegreePerSecond: Double
get() = inVoltsPerRadianPerSecond * RADIANS_PER_DEGREES

inline val DerivativeGain<Radian, Volt>.inVoltsPerRotationsPerMinute: Double
get() = inVoltsPerRadianPerSecond * SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
get() = inVoltsPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val DerivativeGain<Velocity<Radian>, Volt>.inVoltsPerRadiansPerSecondPerSecond: Double
get() = value

inline val DerivativeGain<Velocity<Radian>, Volt>.inVoltsPerDegreesPerSecondPerSecond: Double
get() = inVoltsPerRadiansPerSecondPerSecond * RADIANS_PER_DEGREES

inline val DerivativeGain<Velocity<Radian>, Volt>.inVoltsPerRotationsPerMinutePerSecond: Double
get() = inVoltsPerRadiansPerSecondPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val ProportionalGain<Meter, Ampere>.inAmpsPerMeter: Double
get() = value

Expand Down Expand Up @@ -215,7 +230,7 @@ inline val IntegralGain<Radian, Ampere>.inAmpsPerDegreeSeconds: Double
get() = inAmpsPerRadianSeconds * RADIANS_PER_DEGREES

inline val IntegralGain<Radian, Ampere>.inAmpsPerRotationsMinutes: Double
get() = inAmpsPerRadianSeconds * SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
get() = inAmpsPerRadianSeconds / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION

inline val DerivativeGain<Meter, Ampere>.inAmpsPerMeterPerSecond: Double
get() = value
Expand All @@ -233,4 +248,4 @@ inline val DerivativeGain<Radian, Ampere>.inAmpsPerDegreePerSecond: Double
get() = inAmpsPerRadianPerSecond * RADIANS_PER_DEGREES

inline val DerivativeGain<Radian, Ampere>.inAmpsPerRotationsPerMinute: Double
get() = inAmpsPerRadianPerSecond * SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
get() = inAmpsPerRadianPerSecond / SECONDS_PER_MINUTE * RADIANS_PER_ROTATION
36 changes: 36 additions & 0 deletions src/main/kotlin/org/team4099/lib/units/derived/GearRatio.kt
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
package org.team4099.lib.units.derived

import org.team4099.lib.units.Fraction
import org.team4099.lib.units.Unitless
import org.team4099.lib.units.Value

typealias GearRatio = Value<Fraction<Driving, Driven>>

typealias Driving = Unitless

typealias Driven = Unitless

// Reductions are < 1
inline val Double.reduction
get() = GearRatio(1 / this)

inline val Double.overdrive
get() = GearRatio(this)

inline val Number.reduction
get() = this.toDouble().reduction

inline val Number.overdrive
get() = this.toDouble().overdrive

inline val Double.driving
get() = Value<Driving>(this)

inline val Double.driven
get() = Value<Driven>(this)

inline val GearRatio.asDrivingOverDriven
get() = 1 / value

inline val GearRatio.asDrivenOverDriving
get() = value
16 changes: 16 additions & 0 deletions src/test/kotlin/team4099/controller/PIDControllerTest.kt
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,14 @@ import org.team4099.lib.units.base.seconds
import org.team4099.lib.units.derived.ProportionalGain
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreesPerSecond
import org.team4099.lib.units.derived.inVoltsPerRotation
import org.team4099.lib.units.derived.inVoltsPerRotationPerMinute
import org.team4099.lib.units.derived.inVoltsPerRotationsPerMinutePerSecond
import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
import org.team4099.lib.units.derived.radiansPerSecondPerRadiansPerSecond
import org.team4099.lib.units.derived.rotations
import org.team4099.lib.units.derived.volts
import org.team4099.lib.units.perMinute
import org.team4099.lib.units.perSecond

class PIDControllerTest {
Expand Down Expand Up @@ -74,4 +79,15 @@ class PIDControllerTest {

println(kp.inDegreesPerSecondPerDegreesPerSecond)
}

@Test
fun testRPM() {
val kp = 2.0.volts / 1.0.rotations.perMinute
val kI = 2.0.volts / 1.0.rotations
val kD = 2.0.volts / 1.0.rotations.perMinute.perSecond

println(kp.inVoltsPerRotationPerMinute)
println(kI.inVoltsPerRotation)
println(kD.inVoltsPerRotationsPerMinutePerSecond)
}
}
21 changes: 21 additions & 0 deletions src/test/kotlin/team4099/units/GearRatioTest.kt
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
package team4099.units

import org.junit.jupiter.api.Test
import org.team4099.lib.units.derived.asDrivenOverDriving
import org.team4099.lib.units.derived.driven
import org.team4099.lib.units.derived.driving
import org.team4099.lib.units.derived.overdrive
import org.team4099.lib.units.derived.reduction

class GearRatioTest {

private val kEpsilon = 1E-9

@Test
fun testGearRatio() {
val reductionRatio = (18.0.driving / 72.0.driven).reduction
val overdriveRatio = (18.0.driving / 72.0.driven).overdrive
println(reductionRatio.asDrivenOverDriving) // 4.0
println(overdriveRatio.asDrivenOverDriving) // 0.25
}
}

0 comments on commit ac77d8d

Please sign in to comment.