Add simulation support to FalconEncoder and FalconMotor.

vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt

@@ -13,6 +13,7 @@ import com.ctre.phoenix.motorcontrol.DemandType
 import com.ctre.phoenix.motorcontrol.IMotorController
 import com.ctre.phoenix.motorcontrol.NeutralMode
 import com.ctre.phoenix.motorcontrol.StatusFrame
+import edu.wpi.first.wpilibj.RobotController
 import kotlin.math.roundToInt
 import kotlin.properties.Delegates
 import org.ghrobotics.lib.mathematics.units.SIKey
@@ -40,8 +41,10 @@ import org.ghrobotics.lib.motors.FalconMotor
  */
 abstract class FalconCTRE<K : SIKey>(
     val motorController: IMotorController,
-    private val model: NativeUnitModel<K>
-) : AbstractFalconMotor<K>() {
+    private val model: NativeUnitModel<K>,
+    units: K,
+    simName: String = "FalconCTRE[${motorController.deviceID}]"
+) : AbstractFalconMotor<K>(simName) {
 
     /**
      * The previous demand.
@@ -52,13 +55,14 @@ abstract class FalconCTRE<K : SIKey>(
     /**
      * The encoder (if any) that is connected to the motor controller.
      */
-    override val encoder = FalconCTREEncoder(motorController, 0, model)
+    override val encoder = FalconCTREEncoder(motorController, 0, model, units)
 
     /**
      * Returns the voltage across the motor windings.
      */
     override val voltageOutput: SIUnit<Volt>
-        get() = motorController.motorOutputVoltage.volts
+        get() = if (simVoltageOutput != null) simVoltageOutput.get().volts else
+            motorController.motorOutputVoltage.volts
 
     /**
      * Whether the output of the motor is inverted or not. Slaves do not
@@ -193,27 +197,39 @@ abstract class FalconCTRE<K : SIKey>(
      * @param voltage The voltage to set.
      * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output.
      */
-    override fun setVoltage(voltage: SIUnit<Volt>, arbitraryFeedForward: SIUnit<Volt>) =
+    override fun setVoltage(voltage: SIUnit<Volt>, arbitraryFeedForward: SIUnit<Volt>) {
+        if (simVoltageOutput != null) {
+            simVoltageOutput.set(voltage.value + arbitraryFeedForward.value)
+            return
+        }
+
         sendDemand(
-            Demand(
-                ControlMode.PercentOutput, (voltage / voltageCompSaturation).unitlessValue,
-                DemandType.ArbitraryFeedForward, (arbitraryFeedForward / voltageCompSaturation).unitlessValue
-            )
+                Demand(
+                    ControlMode.PercentOutput, (voltage / voltageCompSaturation).unitlessValue,
+                    DemandType.ArbitraryFeedForward, (arbitraryFeedForward / voltageCompSaturation).unitlessValue
+                )
         )
+    }
 
     /**
      * Commands a certain duty cycle to the motor.
      *
      * @param dutyCycle The duty cycle to command.
      * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output.
      */
-    override fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit<Volt>) =
+    override fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit<Volt>) {
+        if (simVoltageOutput != null) {
+            simVoltageOutput.set(dutyCycle * RobotController.getBatteryVoltage() + arbitraryFeedForward.value)
+            return
+        }
+
         sendDemand(
             Demand(
                 ControlMode.PercentOutput, dutyCycle,
                 DemandType.ArbitraryFeedForward, (arbitraryFeedForward / voltageCompSaturation).unitlessValue
             )
         )
+    }
 
     /**
      * Sets the velocity setpoint of the motor controller.

vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt

@@ -30,8 +30,9 @@ import org.ghrobotics.lib.motors.AbstractFalconEncoder
 class FalconCTREEncoder<K : SIKey>(
     private val motorController: IMotorController,
     private val pidIdx: Int = 0,
-    model: NativeUnitModel<K>
-) : AbstractFalconEncoder<K>(model) {
+    model: NativeUnitModel<K>,
+    units: K
+) : AbstractFalconEncoder<K>(model, units, "FalconCTREEncoder[${motorController.deviceID}]") {
     /**
      * Returns the raw velocity from the encoder.
      */

vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt

@@ -30,16 +30,17 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 @Suppress("Unused")
 class FalconFX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val talonFX: TalonFX,
-    model: NativeUnitModel<K>
-) : FalconCTRE<K>(talonFX, model) {
+    model: NativeUnitModel<K>,
+    units: K
+) : FalconCTRE<K>(talonFX, model, units, "FalconFX[${talonFX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
      *
      * @param id The ID of the motor controller.
      * @param model The native unit model.
      */
-    constructor(id: Int, model: NativeUnitModel<K>) : this(TalonFX(id), model)
+    constructor(id: Int, model: NativeUnitModel<K>, units: K) : this(TalonFX(id), model, units)
 
     /**
      * Configures the feedback device for the motor controller.
@@ -93,11 +94,13 @@ class FalconFX<K : SIKey>(
 fun <K : SIKey> falconFX(
     talonFX: TalonFX,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconFX<K>.() -> Unit
-) = FalconFX(talonFX, model).also(block)
+) = FalconFX(talonFX, model, units).also(block)
 
 fun <K : SIKey> falconFX(
     id: Int,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconFX<K>.() -> Unit
-) = FalconFX(id, model).also(block)
+) = FalconFX(id, model, units).also(block)

vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt

@@ -24,16 +24,17 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
  */
 class FalconSPX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val victorSPX: VictorSPX,
-    model: NativeUnitModel<K>
-) : FalconCTRE<K>(victorSPX, model) {
+    model: NativeUnitModel<K>,
+    units: K
+) : FalconCTRE<K>(victorSPX, model, units, "FalconSPX[${victorSPX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
      *
      * @param id The ID of the motor controller.
      * @param model The native unit model.
      */
-    constructor(id: Int, model: NativeUnitModel<K>) : this(VictorSPX(id), model)
+    constructor(id: Int, model: NativeUnitModel<K>, units: K) : this(VictorSPX(id), model, units)
 
     /**
      * Returns the current drawn by the motor.
@@ -48,11 +49,13 @@ class FalconSPX<K : SIKey>(
 fun <K : SIKey> falconSPX(
     victorSPX: VictorSPX,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconSPX<K>.() -> Unit
-) = FalconSPX(victorSPX, model).also(block)
+) = FalconSPX(victorSPX, model, units).also(block)
 
 fun <K : SIKey> falconSPX(
     id: Int,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconSPX<K>.() -> Unit
-) = FalconSPX(id, model).also(block)
+) = FalconSPX(id, model, units).also(block)

vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt

@@ -30,16 +30,17 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 @Suppress("Unused")
 class FalconSRX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val talonSRX: TalonSRX,
-    model: NativeUnitModel<K>
-) : FalconCTRE<K>(talonSRX, model) {
+    model: NativeUnitModel<K>,
+    units: K
+) : FalconCTRE<K>(talonSRX, model, units, "FalconCTRE[${talonSRX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
      *
      * @param id The ID of the motor controller.
      * @param model The native unit model.
      */
-    constructor(id: Int, model: NativeUnitModel<K>) : this(TalonSRX(id), model)
+    constructor(id: Int, model: NativeUnitModel<K>, units: K) : this(TalonSRX(id), model, units)
 
     /**
      * Returns the current drawn by the motor.
@@ -102,11 +103,13 @@ class FalconSRX<K : SIKey>(
 fun <K : SIKey> falconSRX(
     talonSRX: TalonSRX,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconSRX<K>.() -> Unit
-) = FalconSRX(talonSRX, model).also(block)
+) = FalconSRX(talonSRX, model, units).also(block)
 
 fun <K : SIKey> falconSRX(
     id: Int,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconSRX<K>.() -> Unit
-) = FalconSRX(id, model).also(block)
+) = FalconSRX(id, model, units).also(block)

vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt

@@ -10,6 +10,7 @@ package org.ghrobotics.lib.motors.pwf
 
 import com.playingwithfusion.CANVenom
 import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.RobotController
 import kotlin.properties.Delegates
 import org.ghrobotics.lib.mathematics.units.Ampere
 import org.ghrobotics.lib.mathematics.units.SIKey
@@ -23,6 +24,12 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 import org.ghrobotics.lib.motors.AbstractFalconMotor
 import org.ghrobotics.lib.motors.FalconMotor
 
+internal fun getVenomID(venom: CANVenom): Int {
+    val field = venom.javaClass.getDeclaredField("m_motorID")
+    field.isAccessible = true
+    return field.getInt(venom)
+}
+
 /**
  * Wrapper around the Venom motor controller.
  *
@@ -31,27 +38,29 @@ import org.ghrobotics.lib.motors.FalconMotor
  */
 class FalconVenom<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val venom: CANVenom,
-    private val model: NativeUnitModel<K>
-) : AbstractFalconMotor<K>() {
+    private val model: NativeUnitModel<K>,
+    units: K
+) : AbstractFalconMotor<K>("FalconVenom[${getVenomID(venom)}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
      *
      * @param id The ID of the motor controller.
      * @param model The native unit model.
      */
-    constructor(id: Int, model: NativeUnitModel<K>) : this(CANVenom(id), model)
+    constructor(id: Int, model: NativeUnitModel<K>, units: K) : this(CANVenom(id), model, units)
 
     /**
      * The encoder for the Spark MAX.
      */
-    override val encoder = FalconVenomEncoder(venom, model)
+    override val encoder = FalconVenomEncoder(venom, model, units)
 
     /**
      * Returns the voltage across the motor windings.
      */
     override val voltageOutput: SIUnit<Volt>
-        get() = venom.outputVoltage.volts
+        get() = if (simVoltageOutput != null) simVoltageOutput.get().volts else
+            venom.outputVoltage.volts
 
     /**
      * Returns the current drawn by the motor.
@@ -109,6 +118,11 @@ class FalconVenom<K : SIKey>(
      * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output.
      */
     override fun setVoltage(voltage: SIUnit<Volt>, arbitraryFeedForward: SIUnit<Volt>) {
+        if (simVoltageOutput != null) {
+            simVoltageOutput.set(voltage.value + arbitraryFeedForward.value)
+            return
+        }
+
         venom.setCommand(CANVenom.ControlMode.VoltageControl, voltage.value, 0.0, arbitraryFeedForward.value / 6.0)
     }
 
@@ -119,6 +133,10 @@ class FalconVenom<K : SIKey>(
      * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output.
      */
     override fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit<Volt>) {
+        if (simVoltageOutput != null) {
+            simVoltageOutput.set(dutyCycle * RobotController.getBatteryVoltage() + arbitraryFeedForward.value)
+            return
+        }
         venom.setCommand(CANVenom.ControlMode.Proportional, dutyCycle, 0.0, arbitraryFeedForward.value / 6.0)
     }
 
@@ -172,11 +190,13 @@ class FalconVenom<K : SIKey>(
 fun <K : SIKey> falconVenom(
     venom: CANVenom,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconVenom<K>.() -> Unit
-) = FalconVenom(venom, model).also(block)
+) = FalconVenom(venom, model, units).also(block)
 
 fun <K : SIKey> falconVenom(
     id: Int,
     model: NativeUnitModel<K>,
+    units: K,
     block: FalconVenom<K>.() -> Unit
-) = FalconVenom(id, model).also(block)
+) = FalconVenom(id, model, units).also(block)

vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt

@@ -21,8 +21,9 @@ import org.ghrobotics.lib.motors.AbstractFalconEncoder
  */
 class FalconVenomEncoder<K : SIKey>(
     private val venom: CANVenom,
-    model: NativeUnitModel<K>
-) : AbstractFalconEncoder<K>(model) {
+    model: NativeUnitModel<K>,
+    units: K
+) : AbstractFalconEncoder<K>(model, units, "FalconVenomEncoder[${getVenomID(venom)}]") {
     /**
      * Returns the raw velocity from the encoder.
      */