From 58e56872ad179c98883e3863d1e002e3af7342cc Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 14 Apr 2020 15:58:45 -0700
Subject: [PATCH 01/12] Add simulation support to falcon encoder and falcon
 motor

Run formatter

Fix NPE in falcon max encoder
---
 .../ghrobotics/lib/motors/ctre/FalconCTRE.kt  |  5 ++--
 .../lib/motors/ctre/FalconCTREEncoder.kt      |  2 +-
 .../ghrobotics/lib/motors/ctre/FalconFX.kt    |  2 +-
 .../ghrobotics/lib/motors/ctre/FalconSPX.kt   |  2 +-
 .../ghrobotics/lib/motors/ctre/FalconSRX.kt   |  2 +-
 .../ghrobotics/lib/motors/pwf/FalconVenom.kt  |  8 +++++-
 .../lib/motors/pwf/FalconVenomEncoder.kt      |  2 +-
 .../ghrobotics/lib/motors/rev/FalconMAX.kt    |  2 +-
 .../lib/motors/rev/FalconMAXEncoder.kt        |  8 +++++-
 .../lib/motors/AbstractFalconEncoder.kt       | 26 ++++++++++++++++---
 .../lib/motors/AbstractFalconMotor.kt         |  5 +++-
 .../ghrobotics/lib/motors/FalconEncoder.kt    | 17 ++++++++++--
 12 files changed, 65 insertions(+), 16 deletions(-)

diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
index 8a20354a..028e18fe 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
@@ -40,8 +40,9 @@ 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>,
+    simName: String = "FalconCTRE[${motorController.deviceID}]"
+) : AbstractFalconMotor<K>(simName) {
 
     /**
      * The previous demand.
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt
index 44c9977e..d001ae0f 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt
@@ -31,7 +31,7 @@ class FalconCTREEncoder<K : SIKey>(
     private val motorController: IMotorController,
     private val pidIdx: Int = 0,
     model: NativeUnitModel<K>
-) : AbstractFalconEncoder<K>(model) {
+) : AbstractFalconEncoder<K>(model, "FalconCTREEncoder[${motorController.deviceID}]") {
     /**
      * Returns the raw velocity from the encoder.
      */
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt
index ab1dfd8e..9382e720 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt
@@ -31,7 +31,7 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 class FalconFX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val talonFX: TalonFX,
     model: NativeUnitModel<K>
-) : FalconCTRE<K>(talonFX, model) {
+) : FalconCTRE<K>(talonFX, model, "FalconFX[${talonFX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt
index 796cec69..965ce006 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt
@@ -25,7 +25,7 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 class FalconSPX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val victorSPX: VictorSPX,
     model: NativeUnitModel<K>
-) : FalconCTRE<K>(victorSPX, model) {
+) : FalconCTRE<K>(victorSPX, model, "FalconSPX[${victorSPX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt
index 3b71fa21..2adfb3f4 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt
@@ -31,7 +31,7 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 class FalconSRX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val talonSRX: TalonSRX,
     model: NativeUnitModel<K>
-) : FalconCTRE<K>(talonSRX, model) {
+) : FalconCTRE<K>(talonSRX, model, "FalconCTRE[${talonSRX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
index 4b6eb6df..4c3b515b 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
+++ b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
@@ -23,6 +23,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.
  *
@@ -32,7 +38,7 @@ import org.ghrobotics.lib.motors.FalconMotor
 class FalconVenom<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val venom: CANVenom,
     private val model: NativeUnitModel<K>
-) : AbstractFalconMotor<K>() {
+) : AbstractFalconMotor<K>("FalconVenom[${getVenomID(venom)}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt
index 9593963b..d818fb06 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt
+++ b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt
@@ -22,7 +22,7 @@ import org.ghrobotics.lib.motors.AbstractFalconEncoder
 class FalconVenomEncoder<K : SIKey>(
     private val venom: CANVenom,
     model: NativeUnitModel<K>
-) : AbstractFalconEncoder<K>(model) {
+) : AbstractFalconEncoder<K>(model, "FalconVenomEncoder[${getVenomID(venom)}]") {
     /**
      * Returns the raw velocity from the encoder.
      */
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
index 8ac89df9..175d4a32 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
@@ -41,7 +41,7 @@ class FalconMAX<K : SIKey>(
     private val model: NativeUnitModel<K>,
     useAlternateEncoder: Boolean = false,
     alternateEncoderCPR: Int = 8192
-) : AbstractFalconMotor<K>() {
+) : AbstractFalconMotor<K>("FalconMAX[${canSparkMax.deviceId}]") {
 
     /**
      * Creates a Spark MAX motor controller. The alternate encoder CPR is defaulted
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
index ce5eafab..4b2d4372 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
@@ -16,6 +16,12 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity
 import org.ghrobotics.lib.motors.AbstractFalconEncoder
 
+fun getCanEncoderID(canEncoder: CANEncoder): Int {
+    val method = canEncoder.javaClass.getDeclaredMethod("getID")
+    method.isAccessible = true
+    return method.invoke(canEncoder) as Int
+}
+
 /**
  * Represents an encoder connected to a Spark MAX.
  *
@@ -25,7 +31,7 @@ import org.ghrobotics.lib.motors.AbstractFalconEncoder
 class FalconMAXEncoder<K : SIKey>(
     val canEncoder: CANEncoder,
     model: NativeUnitModel<K>
-) : AbstractFalconEncoder<K>(model) {
+) : AbstractFalconEncoder<K>(model, "FalconMAXEncoder[${getCanEncoderID(canEncoder)}]") {
     /**
      * Returns the raw velocity from the encoder.
      */
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
index d80d6a48..976644c0 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
@@ -8,16 +8,36 @@
 
 package org.ghrobotics.lib.motors
 
+import edu.wpi.first.hal.SimDevice
+import edu.wpi.first.hal.SimDouble
 import org.ghrobotics.lib.mathematics.units.SIKey
 import org.ghrobotics.lib.mathematics.units.SIUnit
 import org.ghrobotics.lib.mathematics.units.derived.Velocity
 import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 
 abstract class AbstractFalconEncoder<K : SIKey>(
-    val model: NativeUnitModel<K>
+    val model: NativeUnitModel<K>,
+    simName: String
 ) : FalconEncoder<K> {
-    override val position: SIUnit<K> get() = model.fromNativeUnitPosition(rawPosition)
-    override val velocity: SIUnit<Velocity<K>> get() = model.fromNativeUnitVelocity(rawVelocity)
+
+    private val simDevice: SimDevice? = SimDevice.create(simName)
+    private val simPositionHandle: SimDouble? = simDevice?.createDouble("Position", false, 0.0)
+    private val simVelocityHandle: SimDouble? = simDevice?.createDouble("Position", false, 0.0)
+
+    override fun setSimulatedPosition(position: SIUnit<K>) {
+        simPositionHandle?.set(position.value)
+    }
+
+    override fun setSimulatedVelocity(position: SIUnit<K>) {
+        simVelocityHandle?.set(position.value)
+    }
+
+    override val position: SIUnit<K>
+        get() = if (simPositionHandle != null) SIUnit(simPositionHandle.get())
+        else model.fromNativeUnitPosition(rawPosition)
+    override val velocity: SIUnit<Velocity<K>>
+        get() = if (simVelocityHandle != null) SIUnit(simVelocityHandle.get())
+        else model.fromNativeUnitVelocity(rawVelocity)
 
     override fun resetPosition(newPosition: SIUnit<K>) {
         resetPositionRaw(model.toNativeUnitPosition(newPosition))
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
index 8fe65595..a6f8d53c 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
@@ -8,9 +8,12 @@
 
 package org.ghrobotics.lib.motors
 
+import edu.wpi.first.hal.SimDevice
 import org.ghrobotics.lib.mathematics.units.SIKey
 
-abstract class AbstractFalconMotor<K : SIKey> : FalconMotor<K> {
+abstract class AbstractFalconMotor<K : SIKey>(simName: String) : FalconMotor<K> {
+
+    private val simDevice: SimDevice? = SimDevice.create(simName)
 
     override var useMotionProfileForPosition: Boolean = false
 
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
index b97168bc..f9c2a2a6 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
@@ -17,11 +17,21 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity
 interface FalconEncoder<K : SIKey> {
 
     /**
-     * The velocity of the encoder in [K]/s
+     * Sets the simulated [position] of this motor.
+     */
+    fun setSimulatedPosition(position: SIUnit<K>)
+
+    /**
+     * Sets the simulated [velocity] of this motor.
+     */
+    fun setSimulatedVelocity(position: SIUnit<K>)
+
+    /**
+     * The velocity of the encoder in [K]/s. When in a simulation, returns the simulated velocity.
      */
     val velocity: SIUnit<Velocity<K>>
     /**
-     * The position of the encoder in [K]
+     * The position of the encoder in [K]. When in a simulation, returns the simulated position.
      */
     val position: SIUnit<K>
 
@@ -34,6 +44,9 @@ interface FalconEncoder<K : SIKey> {
      */
     val rawPosition: SIUnit<NativeUnit>
 
+    /**
+     * Reset the position of the encoder. Does not work in a simulation.
+     */
     fun resetPosition(newPosition: SIUnit<K>)
 
     fun resetPositionRaw(newPosition: SIUnit<NativeUnit>)

From f55d000ca4e332b6967b5e64cc560f2c80aa48d6 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 14 Apr 2020 17:34:05 -0700
Subject: [PATCH 02/12] Implement setDutyCycle and setVoltage voltage
 simulation

---
 .../ghrobotics/lib/motors/ctre/FalconCTRE.kt  | 42 ++++++++++---------
 .../ghrobotics/lib/motors/pwf/FalconVenom.kt  | 10 +++++
 .../ghrobotics/lib/motors/rev/FalconMAX.kt    | 10 +++++
 .../lib/motors/AbstractFalconMotor.kt         |  3 ++
 4 files changed, 46 insertions(+), 19 deletions(-)

diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
index 028e18fe..8af8c16b 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
@@ -8,29 +8,21 @@
 
 package org.ghrobotics.lib.motors.ctre
 
-import com.ctre.phoenix.motorcontrol.ControlMode
-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 kotlin.math.roundToInt
-import kotlin.properties.Delegates
-import org.ghrobotics.lib.mathematics.units.SIKey
-import org.ghrobotics.lib.mathematics.units.SIUnit
-import org.ghrobotics.lib.mathematics.units.Second
+import com.ctre.phoenix.motorcontrol.*
+import edu.wpi.first.wpilibj.RobotController
+import org.ghrobotics.lib.mathematics.units.*
 import org.ghrobotics.lib.mathematics.units.derived.Acceleration
 import org.ghrobotics.lib.mathematics.units.derived.Velocity
 import org.ghrobotics.lib.mathematics.units.derived.Volt
 import org.ghrobotics.lib.mathematics.units.derived.volts
-import org.ghrobotics.lib.mathematics.units.inMilliseconds
 import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 import org.ghrobotics.lib.mathematics.units.nativeunit.inNativeUnitsPer100ms
 import org.ghrobotics.lib.mathematics.units.nativeunit.inNativeUnitsPer100msPerSecond
 import org.ghrobotics.lib.mathematics.units.operations.div
-import org.ghrobotics.lib.mathematics.units.seconds
-import org.ghrobotics.lib.mathematics.units.unitlessValue
 import org.ghrobotics.lib.motors.AbstractFalconMotor
 import org.ghrobotics.lib.motors.FalconMotor
+import kotlin.math.roundToInt
+import kotlin.properties.Delegates
 
 /**
  * Represents the abstract class for all CTRE motor controllers.
@@ -194,13 +186,19 @@ 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.
@@ -208,13 +206,19 @@ abstract class FalconCTRE<K : SIKey>(
      * @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.
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
index 4c3b515b..0c819e7c 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
+++ b/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
@@ -115,6 +116,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)
     }
 
@@ -125,6 +131,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)
     }
 
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
index 175d4a32..e5cc6a3e 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
@@ -13,6 +13,7 @@ import com.revrobotics.CANPIDController
 import com.revrobotics.CANSparkMax
 import com.revrobotics.CANSparkMaxLowLevel
 import com.revrobotics.ControlType
+import edu.wpi.first.wpilibj.RobotController
 import kotlin.properties.Delegates
 import org.ghrobotics.lib.mathematics.units.Ampere
 import org.ghrobotics.lib.mathematics.units.SIKey
@@ -169,6 +170,11 @@ class FalconMAX<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
+        }
+
         controller.setReference(voltage.value, ControlType.kVoltage, 0, arbitraryFeedForward.value)
     }
 
@@ -179,6 +185,10 @@ class FalconMAX<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
+        }
         controller.setReference(dutyCycle, ControlType.kDutyCycle, 0, arbitraryFeedForward.value)
     }
 
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
index a6f8d53c..a6161cfe 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
@@ -9,12 +9,15 @@
 package org.ghrobotics.lib.motors
 
 import edu.wpi.first.hal.SimDevice
+import edu.wpi.first.hal.SimDouble
 import org.ghrobotics.lib.mathematics.units.SIKey
 
 abstract class AbstractFalconMotor<K : SIKey>(simName: String) : FalconMotor<K> {
 
     private val simDevice: SimDevice? = SimDevice.create(simName)
 
+    val simVoltageOutput: SimDouble? = simDevice?.createDouble("Voltage output", true, 0.0);
+
     override var useMotionProfileForPosition: Boolean = false
 
     override fun follow(motor: FalconMotor<*>): Boolean {

From 405b8502aa76acb09193b8dd30c0ce9bdb0caa24 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 14 Apr 2020 19:14:37 -0700
Subject: [PATCH 03/12] Fix Falcon MAX reflection based device id

---
 .../ghrobotics/lib/motors/rev/FalconMAXEncoder.kt   | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
index 4b2d4372..3da908ea 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
@@ -9,6 +9,7 @@
 package org.ghrobotics.lib.motors.rev
 
 import com.revrobotics.CANEncoder
+import com.revrobotics.CANSparkMax
 import org.ghrobotics.lib.mathematics.units.SIKey
 import org.ghrobotics.lib.mathematics.units.SIUnit
 import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnit
@@ -17,9 +18,9 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity
 import org.ghrobotics.lib.motors.AbstractFalconEncoder
 
 fun getCanEncoderID(canEncoder: CANEncoder): Int {
-    val method = canEncoder.javaClass.getDeclaredMethod("getID")
-    method.isAccessible = true
-    return method.invoke(canEncoder) as Int
+    val field = Class.forName("com.revrobotics.CANSensor").getDeclaredField("m_device")
+    val canSparkMax = field.apply { isAccessible = true }.get(canEncoder) as CANSparkMax
+    return canSparkMax.deviceId
 }
 
 /**
@@ -32,9 +33,9 @@ class FalconMAXEncoder<K : SIKey>(
     val canEncoder: CANEncoder,
     model: NativeUnitModel<K>
 ) : AbstractFalconEncoder<K>(model, "FalconMAXEncoder[${getCanEncoderID(canEncoder)}]") {
-    /**
-     * Returns the raw velocity from the encoder.
-     */
+/**
+ * Returns the raw velocity from the encoder.
+ */
     override val rawVelocity: SIUnit<NativeUnitVelocity> get() = SIUnit(canEncoder.velocity / 60.0)
 
     /**

From 6c7c67b6080d0585d66d75945e82dc8ca137f997 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 14 Apr 2020 21:57:09 -0700
Subject: [PATCH 04/12] Use less jank reflection

---
 .../lib/motors/rev/FalconMAXEncoder.kt        |  3 +-
 vendorREV/vendordeps/REVRobotics.json         | 80 +++++++++----------
 2 files changed, 42 insertions(+), 41 deletions(-)

diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
index 3da908ea..bb057b20 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
@@ -9,6 +9,7 @@
 package org.ghrobotics.lib.motors.rev
 
 import com.revrobotics.CANEncoder
+import com.revrobotics.CANSensor
 import com.revrobotics.CANSparkMax
 import org.ghrobotics.lib.mathematics.units.SIKey
 import org.ghrobotics.lib.mathematics.units.SIUnit
@@ -18,7 +19,7 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity
 import org.ghrobotics.lib.motors.AbstractFalconEncoder
 
 fun getCanEncoderID(canEncoder: CANEncoder): Int {
-    val field = Class.forName("com.revrobotics.CANSensor").getDeclaredField("m_device")
+    val field = CANSensor::class.java.getDeclaredField("m_device")
     val canSparkMax = field.apply { isAccessible = true }.get(canEncoder) as CANSparkMax
     return canSparkMax.deviceId
 }
diff --git a/vendorREV/vendordeps/REVRobotics.json b/vendorREV/vendordeps/REVRobotics.json
index 48605e6e..b0bec85b 100644
--- a/vendorREV/vendordeps/REVRobotics.json
+++ b/vendorREV/vendordeps/REVRobotics.json
@@ -1,24 +1,45 @@
 {
-  "cppDependencies": [
+  "fileName": "REVRobotics.json",
+  "name": "REVRobotics",
+  "version": "1.5.2",
+  "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+  "mavenUrls": [
+    "http://www.revrobotics.com/content/sw/max/sdk/maven/"
+  ],
+  "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json",
+  "javaDependencies": [
     {
-      "artifactId": "SparkMax-cpp",
-      "binaryPlatforms": [
+      "groupId": "com.revrobotics.frc",
+      "artifactId": "SparkMax-java",
+      "version": "1.5.2"
+    }
+  ],
+  "jniDependencies": [
+    {
+      "groupId": "com.revrobotics.frc",
+      "artifactId": "SparkMax-driver",
+      "version": "1.5.2",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
         "windowsx86-64",
         "windowsx86",
         "linuxaarch64bionic",
         "linuxx86-64",
         "linuxathena",
         "linuxraspbian"
-      ],
+      ]
+    }
+  ],
+  "cppDependencies": [
+    {
       "groupId": "com.revrobotics.frc",
-      "headerClassifier": "headers",
+      "artifactId": "SparkMax-cpp",
+      "version": "1.5.2",
       "libName": "SparkMax",
+      "headerClassifier": "headers",
       "sharedLibrary": false,
       "skipInvalidPlatforms": true,
-      "version": "1.5.1"
-    },
-    {
-      "artifactId": "SparkMax-driver",
       "binaryPlatforms": [
         "windowsx86-64",
         "windowsx86",
@@ -26,45 +47,24 @@
         "linuxx86-64",
         "linuxathena",
         "linuxraspbian"
-      ],
-      "groupId": "com.revrobotics.frc",
-      "headerClassifier": "headers",
-      "libName": "SparkMaxDriver",
-      "sharedLibrary": false,
-      "skipInvalidPlatforms": true,
-      "version": "1.5.1"
-    }
-  ],
-  "fileName": "REVRobotics.json",
-  "javaDependencies": [
+      ]
+    },
     {
-      "artifactId": "SparkMax-java",
       "groupId": "com.revrobotics.frc",
-      "version": "1.5.1"
-    }
-  ],
-  "jniDependencies": [
-    {
       "artifactId": "SparkMax-driver",
-      "groupId": "com.revrobotics.frc",
-      "isJar": false,
+      "version": "1.5.2",
+      "libName": "SparkMaxDriver",
+      "headerClassifier": "headers",
+      "sharedLibrary": false,
       "skipInvalidPlatforms": true,
-      "validPlatforms": [
+      "binaryPlatforms": [
         "windowsx86-64",
         "windowsx86",
         "linuxaarch64bionic",
         "linuxx86-64",
         "linuxathena",
         "linuxraspbian"
-      ],
-      "version": "1.5.1"
+      ]
     }
-  ],
-  "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json",
-  "mavenUrls": [
-    "http://www.revrobotics.com/content/sw/max/sdk/maven/"
-  ],
-  "name": "REVRobotics",
-  "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
-  "version": "1.5.1"
-}
+  ]
+}
\ No newline at end of file

From 66caf3efb934558a57ee7bc0104babb58f1f536f Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 14 Apr 2020 22:06:03 -0700
Subject: [PATCH 05/12] Apply spotless fixes

---
 .../ghrobotics/lib/motors/ctre/FalconCTRE.kt    | 17 +++++++++++++----
 .../ghrobotics/lib/motors/pwf/FalconVenom.kt    |  2 +-
 .../lib/motors/AbstractFalconMotor.kt           |  2 +-
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
index 8af8c16b..2441f8b8 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
@@ -8,21 +8,30 @@
 
 package org.ghrobotics.lib.motors.ctre
 
-import com.ctre.phoenix.motorcontrol.*
+import com.ctre.phoenix.motorcontrol.ControlMode
+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 org.ghrobotics.lib.mathematics.units.*
+import kotlin.math.roundToInt
+import kotlin.properties.Delegates
+import org.ghrobotics.lib.mathematics.units.SIKey
+import org.ghrobotics.lib.mathematics.units.SIUnit
+import org.ghrobotics.lib.mathematics.units.Second
 import org.ghrobotics.lib.mathematics.units.derived.Acceleration
 import org.ghrobotics.lib.mathematics.units.derived.Velocity
 import org.ghrobotics.lib.mathematics.units.derived.Volt
 import org.ghrobotics.lib.mathematics.units.derived.volts
+import org.ghrobotics.lib.mathematics.units.inMilliseconds
 import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 import org.ghrobotics.lib.mathematics.units.nativeunit.inNativeUnitsPer100ms
 import org.ghrobotics.lib.mathematics.units.nativeunit.inNativeUnitsPer100msPerSecond
 import org.ghrobotics.lib.mathematics.units.operations.div
+import org.ghrobotics.lib.mathematics.units.seconds
+import org.ghrobotics.lib.mathematics.units.unitlessValue
 import org.ghrobotics.lib.motors.AbstractFalconMotor
 import org.ghrobotics.lib.motors.FalconMotor
-import kotlin.math.roundToInt
-import kotlin.properties.Delegates
 
 /**
  * Represents the abstract class for all CTRE motor controllers.
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
index 0c819e7c..652c66f8 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
+++ b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
@@ -120,7 +120,7 @@ class FalconVenom<K : SIKey>(
             simVoltageOutput.set(voltage.value + arbitraryFeedForward.value)
             return
         }
-        
+
         venom.setCommand(CANVenom.ControlMode.VoltageControl, voltage.value, 0.0, arbitraryFeedForward.value / 6.0)
     }
 
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
index a6161cfe..30e08bed 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconMotor.kt
@@ -16,7 +16,7 @@ abstract class AbstractFalconMotor<K : SIKey>(simName: String) : FalconMotor<K>
 
     private val simDevice: SimDevice? = SimDevice.create(simName)
 
-    val simVoltageOutput: SimDouble? = simDevice?.createDouble("Voltage output", true, 0.0);
+    val simVoltageOutput: SimDouble? = simDevice?.createDouble("Voltage output", true, 0.0)
 
     override var useMotionProfileForPosition: Boolean = false
 

From ab03b391a3a796cd08b1d72e46fba6a9b895656b Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Wed, 15 Apr 2020 12:22:24 -0700
Subject: [PATCH 06/12] Add unit name support (BREAKING CHANGE)

---
 .../org/ghrobotics/lib/motors/ctre/FalconCTRE.kt    |  3 ++-
 .../ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt |  5 +++--
 .../org/ghrobotics/lib/motors/ctre/FalconFX.kt      | 13 ++++++++-----
 .../org/ghrobotics/lib/motors/ctre/FalconSPX.kt     | 13 ++++++++-----
 .../org/ghrobotics/lib/motors/ctre/FalconSRX.kt     | 13 ++++++++-----
 .../org/ghrobotics/lib/motors/pwf/FalconVenom.kt    | 13 ++++++++-----
 .../ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt |  5 +++--
 .../org/ghrobotics/lib/motors/rev/FalconMAX.kt      | 12 ++++++++----
 .../ghrobotics/lib/motors/rev/FalconMAXEncoder.kt   |  5 +++--
 .../ghrobotics/lib/motors/AbstractFalconEncoder.kt  |  5 +++--
 10 files changed, 54 insertions(+), 33 deletions(-)

diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
index 2441f8b8..f682ef80 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
@@ -42,6 +42,7 @@ import org.ghrobotics.lib.motors.FalconMotor
 abstract class FalconCTRE<K : SIKey>(
     val motorController: IMotorController,
     private val model: NativeUnitModel<K>,
+    units: K,
     simName: String = "FalconCTRE[${motorController.deviceID}]"
 ) : AbstractFalconMotor<K>(simName) {
 
@@ -54,7 +55,7 @@ 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.
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt
index d001ae0f..97afd41e 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTREEncoder.kt
+++ b/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, "FalconCTREEncoder[${motorController.deviceID}]") {
+    model: NativeUnitModel<K>,
+    units: K
+) : AbstractFalconEncoder<K>(model, units, "FalconCTREEncoder[${motorController.deviceID}]") {
     /**
      * Returns the raw velocity from the encoder.
      */
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt
index 9382e720..841479f4 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconFX.kt
@@ -30,8 +30,9 @@ 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, "FalconFX[${talonFX.deviceID}]") {
+    model: NativeUnitModel<K>,
+    units: K
+) : FalconCTRE<K>(talonFX, model, units, "FalconFX[${talonFX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
@@ -39,7 +40,7 @@ class FalconFX<K : SIKey>(
      * @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)
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt
index 965ce006..09fcd0ca 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSPX.kt
@@ -24,8 +24,9 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
  */
 class FalconSPX<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val victorSPX: VictorSPX,
-    model: NativeUnitModel<K>
-) : FalconCTRE<K>(victorSPX, model, "FalconSPX[${victorSPX.deviceID}]") {
+    model: NativeUnitModel<K>,
+    units: K
+) : FalconCTRE<K>(victorSPX, model, units, "FalconSPX[${victorSPX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
@@ -33,7 +34,7 @@ class FalconSPX<K : SIKey>(
      * @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)
diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt
index 2adfb3f4..8ec08c87 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconSRX.kt
@@ -30,8 +30,9 @@ 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, "FalconCTRE[${talonSRX.deviceID}]") {
+    model: NativeUnitModel<K>,
+    units: K
+) : FalconCTRE<K>(talonSRX, model, units, "FalconCTRE[${talonSRX.deviceID}]") {
 
     /**
      * Alternate constructor where users can supply ID and native unit model.
@@ -39,7 +40,7 @@ class FalconSRX<K : SIKey>(
      * @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)
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
index 652c66f8..c5f8264a 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
+++ b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
@@ -38,7 +38,8 @@ internal fun getVenomID(venom: CANVenom): Int {
  */
 class FalconVenom<K : SIKey>(
     @Suppress("MemberVisibilityCanBePrivate") val venom: CANVenom,
-    private val model: NativeUnitModel<K>
+    private val model: NativeUnitModel<K>,
+    units: K
 ) : AbstractFalconMotor<K>("FalconVenom[${getVenomID(venom)}]") {
 
     /**
@@ -47,12 +48,12 @@ class FalconVenom<K : SIKey>(
      * @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.
@@ -188,11 +189,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)
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt
index d818fb06..bc9535e7 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenomEncoder.kt
+++ b/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, "FalconVenomEncoder[${getVenomID(venom)}]") {
+    model: NativeUnitModel<K>,
+    units: K
+) : AbstractFalconEncoder<K>(model, units, "FalconVenomEncoder[${getVenomID(venom)}]") {
     /**
      * Returns the raw velocity from the encoder.
      */
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
index e5cc6a3e..ff0ee5e1 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
@@ -40,6 +40,7 @@ import org.ghrobotics.lib.motors.FalconMotor
 class FalconMAX<K : SIKey>(
     val canSparkMax: CANSparkMax,
     private val model: NativeUnitModel<K>,
+    units: K,
     useAlternateEncoder: Boolean = false,
     alternateEncoderCPR: Int = 8192
 ) : AbstractFalconMotor<K>("FalconMAX[${canSparkMax.deviceId}]") {
@@ -57,10 +58,11 @@ class FalconMAX<K : SIKey>(
         id: Int,
         type: CANSparkMaxLowLevel.MotorType,
         model: NativeUnitModel<K>,
+        units: K,
         useAlternateEncoder: Boolean = false,
         alternateEncoderCPR: Int = 8196
     ) : this(
-        CANSparkMax(id, type), model, useAlternateEncoder, alternateEncoderCPR
+        CANSparkMax(id, type), model, units, useAlternateEncoder, alternateEncoderCPR
     )
 
     /**
@@ -76,7 +78,7 @@ class FalconMAX<K : SIKey>(
         if (useAlternateEncoder) canSparkMax.getAlternateEncoder(
             AlternateEncoderType.kQuadrature,
             alternateEncoderCPR
-        ) else canSparkMax.encoder, model
+        ) else canSparkMax.encoder, model, units
     )
 
     /**
@@ -242,16 +244,18 @@ class FalconMAX<K : SIKey>(
 fun <K : SIKey> falconMAX(
     canSparkMax: CANSparkMax,
     model: NativeUnitModel<K>,
+    units: K,
     useAlternateEncoder: Boolean = false,
     alternateEncoderCPR: Int = 8192,
     block: FalconMAX<K>.() -> Unit
-) = FalconMAX(canSparkMax, model, useAlternateEncoder, alternateEncoderCPR).also(block)
+) = FalconMAX(canSparkMax, model, units, useAlternateEncoder, alternateEncoderCPR).also(block)
 
 fun <K : SIKey> falconMAX(
     id: Int,
     type: CANSparkMaxLowLevel.MotorType,
     model: NativeUnitModel<K>,
+    units: K,
     useAlternateEncoder: Boolean = false,
     alternateEncoderCPR: Int = 8192,
     block: FalconMAX<K>.() -> Unit
-) = FalconMAX(id, type, model, useAlternateEncoder, alternateEncoderCPR).also(block)
+) = FalconMAX(id, type, model, units, useAlternateEncoder, alternateEncoderCPR).also(block)
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
index bb057b20..58048897 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAXEncoder.kt
@@ -32,8 +32,9 @@ fun getCanEncoderID(canEncoder: CANEncoder): Int {
  */
 class FalconMAXEncoder<K : SIKey>(
     val canEncoder: CANEncoder,
-    model: NativeUnitModel<K>
-) : AbstractFalconEncoder<K>(model, "FalconMAXEncoder[${getCanEncoderID(canEncoder)}]") {
+    model: NativeUnitModel<K>,
+    units: K
+) : AbstractFalconEncoder<K>(model, units, "FalconMAXEncoder[${getCanEncoderID(canEncoder)}]") {
 /**
  * Returns the raw velocity from the encoder.
  */
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
index 976644c0..b2ea9f57 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
@@ -17,12 +17,13 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitModel
 
 abstract class AbstractFalconEncoder<K : SIKey>(
     val model: NativeUnitModel<K>,
+    units: K,
     simName: String
 ) : FalconEncoder<K> {
 
     private val simDevice: SimDevice? = SimDevice.create(simName)
-    private val simPositionHandle: SimDouble? = simDevice?.createDouble("Position", false, 0.0)
-    private val simVelocityHandle: SimDouble? = simDevice?.createDouble("Position", false, 0.0)
+    private val simPositionHandle: SimDouble? = simDevice?.createDouble("Position, ${units::class.java.simpleName}", false, 0.0)
+    private val simVelocityHandle: SimDouble? = simDevice?.createDouble("Velocity, ${units::class.java.simpleName}/s", false, 0.0)
 
     override fun setSimulatedPosition(position: SIUnit<K>) {
         simPositionHandle?.set(position.value)

From 336a2093487d8de6b05797b9f213c5006b6b9197 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Wed, 15 Apr 2020 13:59:23 -0700
Subject: [PATCH 07/12] Make sim entries plural

---
 .../kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
index b2ea9f57..653de2cd 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
@@ -22,8 +22,8 @@ abstract class AbstractFalconEncoder<K : SIKey>(
 ) : FalconEncoder<K> {
 
     private val simDevice: SimDevice? = SimDevice.create(simName)
-    private val simPositionHandle: SimDouble? = simDevice?.createDouble("Position, ${units::class.java.simpleName}", false, 0.0)
-    private val simVelocityHandle: SimDouble? = simDevice?.createDouble("Velocity, ${units::class.java.simpleName}/s", false, 0.0)
+    private val simPositionHandle: SimDouble? = simDevice?.createDouble("Position, ${units::class.java.simpleName}s", false, 0.0)
+    private val simVelocityHandle: SimDouble? = simDevice?.createDouble("Velocity, ${units::class.java.simpleName}s/s", false, 0.0)
 
     override fun setSimulatedPosition(position: SIUnit<K>) {
         simPositionHandle?.set(position.value)

From 4a374a5557b41ce48b2c6abdf6455fcef0abfaed Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 20 Apr 2020 12:21:43 -0700
Subject: [PATCH 08/12] Update FalconTimedRobot to include simulation methods

---
 .../ghrobotics/lib/wrappers/FalconTimedRobot.kt | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt
index aef4c29e..1dbbeb4c 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt
@@ -76,6 +76,10 @@ abstract class FalconTimedRobot {
             this@FalconTimedRobot.testInit()
         }
 
+        override fun simulationInit() {
+            this@FalconTimedRobot.simulationInit()
+        }
+
         override fun robotPeriodic() {
             this@FalconTimedRobot.robotPeriodic()
             CommandScheduler.getInstance().run()
@@ -89,21 +93,32 @@ abstract class FalconTimedRobot {
             this@FalconTimedRobot.teleopPeriodic()
         }
 
+        override fun testPeriodic() {
+            this@FalconTimedRobot.testPeriodic()
+        }
+
         override fun disabledPeriodic() {
             this@FalconTimedRobot.disabledPeriodic()
         }
+
+        override fun simulationPeriodic() {
+            this@FalconTimedRobot.simulationPeriodic()
+        }
     }
 
     protected open fun robotInit() {}
     protected open fun autonomousInit() {}
     protected open fun teleopInit() {}
     protected open fun disabledInit() {}
-    private fun testInit() {}
+    protected open fun testInit() {}
+    protected open fun simulationInit() {}
 
     protected open fun robotPeriodic() {}
     protected open fun autonomousPeriodic() {}
     protected open fun teleopPeriodic() {}
     protected open fun disabledPeriodic() {}
+    protected open fun testPeriodic() {}
+    protected open fun simulationPeriodic() {}
 
     protected fun getSubsystemChecks(): Command {
         return FalconSubsystemHandler.testCommand

From 0602569138dea219f59e9f3d3e1bd343c183b947 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 20 Apr 2020 12:26:40 -0700
Subject: [PATCH 09/12] Add sim support to voltageOutput method

---
 .../main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt   | 3 ++-
 .../main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt   | 3 ++-
 .../src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt | 3 ++-
 .../kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt     | 3 +++
 4 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
index f682ef80..af93c718 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
@@ -61,7 +61,8 @@ abstract class FalconCTRE<K : SIKey>(
      * 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
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
index c5f8264a..5592263d 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
+++ b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
@@ -59,7 +59,8 @@ class FalconVenom<K : SIKey>(
      * 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.
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
index ff0ee5e1..881f7e81 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
@@ -94,7 +94,8 @@ class FalconMAX<K : SIKey>(
      * Returns the voltage across the motor windings.
      */
     override val voltageOutput: SIUnit<Volt>
-        get() = (canSparkMax.appliedOutput * canSparkMax.busVoltage).volts
+        get() = if(simVoltageOutput != null) simVoltageOutput.get().volts else
+            (canSparkMax.appliedOutput * canSparkMax.busVoltage).volts
 
     /**
      * Returns the current drawn by the motor.
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt
index 1dbbeb4c..6ec7f331 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/wrappers/FalconTimedRobot.kt
@@ -38,6 +38,9 @@ abstract class FalconTimedRobot {
 
     protected val wrappedValue = WpiTimedRobot()
 
+    val isEnabled
+        get() = wrappedValue.isEnabled
+
     protected inner class WpiTimedRobot : TimedRobot() {
 
         private val kLanguage_Kotlin = 6

From bab8af46749fd0ed0c15e10a425c7a59e6a0287b Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 20 Apr 2020 12:56:28 -0700
Subject: [PATCH 10/12] Fix units on sim velocity

---
 .../kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt   | 2 +-
 wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
index 653de2cd..68bb73d3 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
@@ -29,7 +29,7 @@ abstract class AbstractFalconEncoder<K : SIKey>(
         simPositionHandle?.set(position.value)
     }
 
-    override fun setSimulatedVelocity(position: SIUnit<K>) {
+    override fun setSimulatedVelocity(velocity: SIUnit<Velocity<K>>) {
         simVelocityHandle?.set(position.value)
     }
 
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
index f9c2a2a6..3e4ac8c1 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
@@ -24,7 +24,7 @@ interface FalconEncoder<K : SIKey> {
     /**
      * Sets the simulated [velocity] of this motor.
      */
-    fun setSimulatedVelocity(position: SIUnit<K>)
+    fun setSimulatedVelocity(velocity: SIUnit<Velocity<K>>)
 
     /**
      * The velocity of the encoder in [K]/s. When in a simulation, returns the simulated velocity.

From 13fc50f55d27fedbb9a3987ce9b8cd982a7bb9a2 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Mon, 20 Apr 2020 13:48:42 -0700
Subject: [PATCH 11/12] Fix pos/vel mixup in sim encoder

---
 .../org/ghrobotics/lib/motors/AbstractFalconEncoder.kt    | 8 ++++----
 .../kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt     | 8 ++++----
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
index 68bb73d3..bfb2865a 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/AbstractFalconEncoder.kt
@@ -25,12 +25,12 @@ abstract class AbstractFalconEncoder<K : SIKey>(
     private val simPositionHandle: SimDouble? = simDevice?.createDouble("Position, ${units::class.java.simpleName}s", false, 0.0)
     private val simVelocityHandle: SimDouble? = simDevice?.createDouble("Velocity, ${units::class.java.simpleName}s/s", false, 0.0)
 
-    override fun setSimulatedPosition(position: SIUnit<K>) {
-        simPositionHandle?.set(position.value)
+    override fun setSimulatedPosition(simPosition: SIUnit<K>) {
+        simPositionHandle?.set(simPosition.value)
     }
 
-    override fun setSimulatedVelocity(velocity: SIUnit<Velocity<K>>) {
-        simVelocityHandle?.set(position.value)
+    override fun setSimulatedVelocity(simVelocity: SIUnit<Velocity<K>>) {
+        simVelocityHandle?.set(simVelocity.value)
     }
 
     override val position: SIUnit<K>
diff --git a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
index 3e4ac8c1..484fd193 100644
--- a/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
+++ b/wpi/src/main/kotlin/org/ghrobotics/lib/motors/FalconEncoder.kt
@@ -17,14 +17,14 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity
 interface FalconEncoder<K : SIKey> {
 
     /**
-     * Sets the simulated [position] of this motor.
+     * Sets the simulated [simPosition] of this motor.
      */
-    fun setSimulatedPosition(position: SIUnit<K>)
+    fun setSimulatedPosition(simPosition: SIUnit<K>)
 
     /**
-     * Sets the simulated [velocity] of this motor.
+     * Sets the simulated [simVelocity] of this motor.
      */
-    fun setSimulatedVelocity(velocity: SIUnit<Velocity<K>>)
+    fun setSimulatedVelocity(simVelocity: SIUnit<Velocity<K>>)
 
     /**
      * The velocity of the encoder in [K]/s. When in a simulation, returns the simulated velocity.

From 17aad54cdfad63b3e8d4d7086546a9d506626fb6 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Thu, 23 Jul 2020 10:34:36 -0700
Subject: [PATCH 12/12] Run spotless

---
 .../main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt    | 2 +-
 .../main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt    | 2 +-
 .../src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt  | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
index af93c718..25f3088e 100644
--- a/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
+++ b/vendorCTRE/src/main/kotlin/org/ghrobotics/lib/motors/ctre/FalconCTRE.kt
@@ -61,7 +61,7 @@ abstract class FalconCTRE<K : SIKey>(
      * Returns the voltage across the motor windings.
      */
     override val voltageOutput: SIUnit<Volt>
-        get() = if(simVoltageOutput != null) simVoltageOutput.get().volts else
+        get() = if (simVoltageOutput != null) simVoltageOutput.get().volts else
             motorController.motorOutputVoltage.volts
 
     /**
diff --git a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
index 5592263d..66858787 100644
--- a/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
+++ b/vendorPWF/src/main/kotlin/org/ghrobotics/lib/motors/pwf/FalconVenom.kt
@@ -59,7 +59,7 @@ class FalconVenom<K : SIKey>(
      * Returns the voltage across the motor windings.
      */
     override val voltageOutput: SIUnit<Volt>
-        get() = if(simVoltageOutput != null) simVoltageOutput.get().volts else
+        get() = if (simVoltageOutput != null) simVoltageOutput.get().volts else
             venom.outputVoltage.volts
 
     /**
diff --git a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
index 881f7e81..351c715d 100644
--- a/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
+++ b/vendorREV/src/main/kotlin/org/ghrobotics/lib/motors/rev/FalconMAX.kt
@@ -94,7 +94,7 @@ class FalconMAX<K : SIKey>(
      * Returns the voltage across the motor windings.
      */
     override val voltageOutput: SIUnit<Volt>
-        get() = if(simVoltageOutput != null) simVoltageOutput.get().volts else
+        get() = if (simVoltageOutput != null) simVoltageOutput.get().volts else
             (canSparkMax.appliedOutput * canSparkMax.busVoltage).volts
 
     /**