diff --git a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt index 8e21841..ee33257 100644 --- a/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt +++ b/src/main/kotlin/org/team4099/lib/units/MechanismUnits.kt @@ -11,6 +11,7 @@ import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.minutes import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.AccelerationFeedforward import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain @@ -75,6 +76,8 @@ interface MechanismSensor { ): Double fun velocityFeedforwardToRawUnits(velocityFeedforward: VelocityFeedforward): Double + + fun accelerationFeedforwardToRawUnits(accelerationFeedforward: AccelerationFeedforward): Double } class LinearMechanismSensor( @@ -173,6 +176,13 @@ class LinearMechanismSensor( velocityToRawUnits(1.0.meters.perSecond) / compensationVoltage.inVolts * fullPowerThrottle } + + override inline fun accelerationFeedforwardToRawUnits( + accelerationFeedforward: AccelerationFeedforward): Double { + return accelerationFeedforward.value / + accelerationToRawUnits(1.0.meters.perSecond.perSecond) / + compensationVoltage.inVolts * fullPowerThrottle + } } class AngularMechanismSensor( @@ -268,6 +278,13 @@ class AngularMechanismSensor( velocityToRawUnits(1.0.radians.perSecond) / compensationVoltage.inVolts * fullPowerThrottle } + + override inline fun accelerationFeedforwardToRawUnits( + accelerationFeedforward: AccelerationFeedforward): Double { + return accelerationFeedforward.value / + accelerationToRawUnits(1.0.radians.perSecond.perSecond) / + compensationVoltage.inVolts * fullPowerThrottle + } } fun ctreAngularMechanismSensor(