units fixes

team4099 · Mar 5, 2023 · 28c8678 · 28c8678
1 parent c2b7d3e
commit 28c8678
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 30 deletions.
diff --git a/build.gradle b/build.gradle
@@ -39,7 +39,7 @@ publishing {
 		release(MavenPublication) {
 			groupId = 'org.team4099'
 			artifactId = 'falconutils'
-			version = '1.1.15'
+			version = '1.1.16'
 
 			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
@@ -36,9 +36,13 @@ import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.rotations
 import kotlin.math.PI
 
-enum class Timescale(val velocity: Time, val acceleration: Time, val pidTimeConversion: (Time) -> Double) {
-  REV_NEO(1.minutes, 1.seconds, {it.inMilliseconds}),
-  CTRE(100.milli.seconds, 1.seconds, {it.inSeconds}),
+enum class Timescale(
+  val velocity: Time,
+  val acceleration: Time,
+  val pidTimeConversion: (Time) -> Double
+) {
+  REV_NEO(1.minutes, 1.seconds, { it.inMilliseconds }),
+  CTRE(100.milli.seconds, 1.seconds, { it.inSeconds }),
 }
 
 interface MechanismSensor<U : UnitKey> {
@@ -125,9 +129,10 @@ class LinearMechanismSensor(
     derivativeGain: DerivativeGain<Meter, Volt>,
   ): Double {
     return (
-      derivativeGain.inVoltsPerMeterPerSecond * timescale.pidTimeConversion(timescale.velocity) /
-        positionToRawUnits(1.meters)
-      ) / compensationVoltage.inVolts * fullPowerThrottle
+      derivativeGain.inVoltsPerMeterPerSecond *
+        timescale.pidTimeConversion(timescale.velocity) / positionToRawUnits(1.meters)
+      ) /
+      compensationVoltage.inVolts * fullPowerThrottle
   }
 
   override inline fun proportionalVelocityGainToRawUnits(
@@ -142,18 +147,22 @@ class LinearMechanismSensor(
   ): Double {
     return (
       integralGain.inVoltsPerMeters /
-        (velocityToRawUnits(1.meters.perSecond) * timescale.pidTimeConversion(timescale.velocity))
-      ) /
-      compensationVoltage.inVolts * fullPowerThrottle
+        (
+          velocityToRawUnits(1.meters.perSecond) *
+            timescale.pidTimeConversion(timescale.velocity)
+          )
+      ) / compensationVoltage.inVolts *
+      fullPowerThrottle
   }
 
   override inline fun derivativeVelocityGainToRawUnits(
     derivativeGain: DerivativeGain<Velocity<Meter>, Volt>,
   ): Double {
     return (
-      derivativeGain.inVoltsPerMetersPerSecondPerSecond * timescale.pidTimeConversion(timescale.velocity) /
-        velocityToRawUnits(1.meters.perSecond)
-      ) / compensationVoltage.inVolts * fullPowerThrottle
+      derivativeGain.inVoltsPerMetersPerSecondPerSecond *
+        timescale.pidTimeConversion(timescale.velocity) / velocityToRawUnits(1.meters.perSecond)
+      ) /
+      compensationVoltage.inVolts * fullPowerThrottle
   }
 
   override inline fun velocityFeedforwardToRawUnits(
@@ -212,9 +221,10 @@ class AngularMechanismSensor(
     derivativeGain: DerivativeGain<Radian, Volt>
   ): Double {
     return (
-      derivativeGain.inVoltsPerRadianPerSecond * timescale.pidTimeConversion(timescale.velocity) /
-        positionToRawUnits(1.radians)
-      ) / compensationVoltage.inVolts * fullPowerThrottle
+      derivativeGain.inVoltsPerRadianPerSecond *
+        timescale.pidTimeConversion(timescale.velocity) / positionToRawUnits(1.radians)
+      ) /
+      compensationVoltage.inVolts * fullPowerThrottle
   }
 
   override inline fun proportionalVelocityGainToRawUnits(
@@ -231,18 +241,22 @@ class AngularMechanismSensor(
   ): Double {
     return (
       integralGain.inVoltsPerRadians /
-        (velocityToRawUnits(1.radians.perSecond) * timescale.pidTimeConversion(timescale.velocity))
-      ) /
-      compensationVoltage.inVolts * fullPowerThrottle
+        (
+          velocityToRawUnits(1.radians.perSecond) *
+            timescale.pidTimeConversion(timescale.velocity)
+          )
+      ) / compensationVoltage.inVolts *
+      fullPowerThrottle
   }
 
   override inline fun derivativeVelocityGainToRawUnits(
     derivativeGain: DerivativeGain<Velocity<Radian>, Volt>
   ): Double {
     return (
-      derivativeGain.inVoltsPerRadiansPerSecondPerSecond * timescale.pidTimeConversion(timescale.velocity) /
-        velocityToRawUnits(1.radians.perSecond)
-      ) / compensationVoltage.inVolts * fullPowerThrottle
+      derivativeGain.inVoltsPerRadiansPerSecondPerSecond *
+        timescale.pidTimeConversion(timescale.velocity) / velocityToRawUnits(1.radians.perSecond)
+      ) /
+      compensationVoltage.inVolts * fullPowerThrottle
   }
 
   override inline fun velocityFeedforwardToRawUnits(

diff --git a/src/main/kotlin/org/team4099/lib/units/derived/Controller.kt b/src/main/kotlin/org/team4099/lib/units/derived/Controller.kt
@@ -65,19 +65,19 @@ inline val <K : UnitKey> Value<K>.perMeter
   get() = Value<Fraction<K, Meter>>(value)
 
 inline val <K : UnitKey> Value<K>.perInch
-  get() = perMeter / METERS_PER_INCH
+  get() = perMeter * METERS_PER_INCH
 
 inline val <K : UnitKey> Value<K>.perFoot
-  get() = perMeter / METERS_PER_FOOT
+  get() = perMeter * METERS_PER_FOOT
 
 inline val <K : UnitKey> Value<K>.perRadian
   get() = Value<Fraction<K, Radian>>(value)
 
 inline val <K : UnitKey> Value<K>.perDegree
-  get() = perRadian / RADIANS_PER_DEGREES
+  get() = perRadian * RADIANS_PER_DEGREES
 
 inline val <K : UnitKey> Value<K>.perRotation
-  get() = perRadian / RADIANS_PER_ROTATION
+  get() = perRadian * RADIANS_PER_ROTATION
 
 inline val <K : UnitKey> Value<K>.perMeterSeconds
   get() = Value<Fraction<K, Product<Meter, Second>>>(value)

diff --git a/src/test/kotlin/team4099/controller/PIDControllerTest.kt b/src/test/kotlin/team4099/controller/PIDControllerTest.kt
@@ -15,12 +15,12 @@ import org.team4099.lib.units.base.meters
 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.inVoltsPerDegree
 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.perDegree
 import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.perMinute
@@ -75,9 +75,9 @@ class PIDControllerTest {
 
   @Test
   fun testConversion() {
-    val kp = (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond
+    val kp = 0.4.volts.perDegree
 
-    println(kp.inDegreesPerSecondPerDegreesPerSecond)
+    println(kp.inVoltsPerDegree)
   }
 
   @Test