grring because we can't do math

team4099 · Mar 14, 2023 · f2b17bd · f2b17bd
1 parent 522d51b
commit f2b17bd
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 3 deletions.
diff --git a/build.gradle b/build.gradle
@@ -39,7 +39,7 @@ publishing {
 		release(MavenPublication) {
 			groupId = 'org.team4099'
 			artifactId = 'falconutils'
-			version = '1.1.17'
+			version = '1.1.18'
 
 			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
@@ -168,7 +168,8 @@ class LinearMechanismSensor(
   override inline fun velocityFeedforwardToRawUnits(
     velocityFeedforward: VelocityFeedforward<Meter, Volt>
   ): Double {
-    return velocityFeedforward.value * velocityToRawUnits(1.0.meters.perSecond) /
+    return velocityFeedforward.value /
+      velocityToRawUnits(1.0.meters.perSecond) /
       compensationVoltage.inVolts * fullPowerThrottle
   }
 }
@@ -262,7 +263,8 @@ class AngularMechanismSensor(
   override inline fun velocityFeedforwardToRawUnits(
     velocityFeedforward: VelocityFeedforward<Radian, Volt>
   ): Double {
-    return velocityFeedforward.value * velocityToRawUnits(1.0.radians.perSecond) /
+    return velocityFeedforward.value /
+      velocityToRawUnits(1.0.radians.perSecond) /
       compensationVoltage.inVolts * fullPowerThrottle
   }
 }

diff --git a/src/test/kotlin/team4099/controller/PIDControllerTest.kt b/src/test/kotlin/team4099/controller/PIDControllerTest.kt
@@ -7,10 +7,13 @@ import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.controller.TrapezoidProfile
 import org.team4099.lib.units.Acceleration
 import org.team4099.lib.units.Fraction
+import org.team4099.lib.units.LinearMechanismSensor
+import org.team4099.lib.units.Timescale
 import org.team4099.lib.units.Value
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.Second
 import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.ProportionalGain
@@ -90,4 +93,22 @@ class PIDControllerTest {
     println(kI.inVoltsPerRotation)
     println(kD.inVoltsPerRotationsPerMinutePerSecond)
   }
+
+  @Test
+  fun testFFConversion() {
+    val kFF = 11.3.volts / 4.56.meters.perSecond
+
+    val testSensor =
+      LinearMechanismSensor(
+        3.827.inches,
+        (14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0) / 2048,
+        Timescale.CTRE,
+        1023.0,
+        12.0.volts,
+        { 0.0 },
+        { 0.0 }
+      )
+
+    println(testSensor.velocityFeedforwardToRawUnits(kFF))
+  }
 }