diff --git a/build.gradle b/build.gradle index c43304f..924bb07 100644 --- 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 index 2c905e9..1ece7df 100644 --- 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 ): 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 ): 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 index 7ac45fa..31ddb2e 100644 --- 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)) + } }