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..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 @@ -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( val motorController: IMotorController, - private val model: NativeUnitModel -) : AbstractFalconMotor() { + private val model: NativeUnitModel, + units: K, + simName: String = "FalconCTRE[${motorController.deviceID}]" +) : AbstractFalconMotor(simName) { /** * The previous demand. @@ -52,13 +55,14 @@ abstract class FalconCTRE( /** * 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 - 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,13 +197,19 @@ abstract class FalconCTRE( * @param voltage The voltage to set. * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output. */ - override fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit) = + override fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit) { + 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. @@ -207,13 +217,19 @@ abstract class FalconCTRE( * @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) = + override fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit) { + 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/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..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( private val motorController: IMotorController, private val pidIdx: Int = 0, - model: NativeUnitModel -) : AbstractFalconEncoder(model) { + model: NativeUnitModel, + units: K +) : AbstractFalconEncoder(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 ab1dfd8e..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( @Suppress("MemberVisibilityCanBePrivate") val talonFX: TalonFX, - model: NativeUnitModel -) : FalconCTRE(talonFX, model) { + model: NativeUnitModel, + units: K +) : FalconCTRE(talonFX, model, units, "FalconFX[${talonFX.deviceID}]") { /** * Alternate constructor where users can supply ID and native unit model. @@ -39,7 +40,7 @@ class FalconFX( * @param id The ID of the motor controller. * @param model The native unit model. */ - constructor(id: Int, model: NativeUnitModel) : this(TalonFX(id), model) + constructor(id: Int, model: NativeUnitModel, units: K) : this(TalonFX(id), model, units) /** * Configures the feedback device for the motor controller. @@ -93,11 +94,13 @@ class FalconFX( fun falconFX( talonFX: TalonFX, model: NativeUnitModel, + units: K, block: FalconFX.() -> Unit -) = FalconFX(talonFX, model).also(block) +) = FalconFX(talonFX, model, units).also(block) fun falconFX( id: Int, model: NativeUnitModel, + units: K, block: FalconFX.() -> 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 796cec69..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( @Suppress("MemberVisibilityCanBePrivate") val victorSPX: VictorSPX, - model: NativeUnitModel -) : FalconCTRE(victorSPX, model) { + model: NativeUnitModel, + units: K +) : FalconCTRE(victorSPX, model, units, "FalconSPX[${victorSPX.deviceID}]") { /** * Alternate constructor where users can supply ID and native unit model. @@ -33,7 +34,7 @@ class FalconSPX( * @param id The ID of the motor controller. * @param model The native unit model. */ - constructor(id: Int, model: NativeUnitModel) : this(VictorSPX(id), model) + constructor(id: Int, model: NativeUnitModel, units: K) : this(VictorSPX(id), model, units) /** * Returns the current drawn by the motor. @@ -48,11 +49,13 @@ class FalconSPX( fun falconSPX( victorSPX: VictorSPX, model: NativeUnitModel, + units: K, block: FalconSPX.() -> Unit -) = FalconSPX(victorSPX, model).also(block) +) = FalconSPX(victorSPX, model, units).also(block) fun falconSPX( id: Int, model: NativeUnitModel, + units: K, block: FalconSPX.() -> 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 3b71fa21..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( @Suppress("MemberVisibilityCanBePrivate") val talonSRX: TalonSRX, - model: NativeUnitModel -) : FalconCTRE(talonSRX, model) { + model: NativeUnitModel, + units: K +) : FalconCTRE(talonSRX, model, units, "FalconCTRE[${talonSRX.deviceID}]") { /** * Alternate constructor where users can supply ID and native unit model. @@ -39,7 +40,7 @@ class FalconSRX( * @param id The ID of the motor controller. * @param model The native unit model. */ - constructor(id: Int, model: NativeUnitModel) : this(TalonSRX(id), model) + constructor(id: Int, model: NativeUnitModel, units: K) : this(TalonSRX(id), model, units) /** * Returns the current drawn by the motor. @@ -102,11 +103,13 @@ class FalconSRX( fun falconSRX( talonSRX: TalonSRX, model: NativeUnitModel, + units: K, block: FalconSRX.() -> Unit -) = FalconSRX(talonSRX, model).also(block) +) = FalconSRX(talonSRX, model, units).also(block) fun falconSRX( id: Int, model: NativeUnitModel, + units: K, block: FalconSRX.() -> 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 4b6eb6df..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 @@ -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,8 +38,9 @@ import org.ghrobotics.lib.motors.FalconMotor */ class FalconVenom( @Suppress("MemberVisibilityCanBePrivate") val venom: CANVenom, - private val model: NativeUnitModel -) : AbstractFalconMotor() { + private val model: NativeUnitModel, + units: K +) : AbstractFalconMotor("FalconVenom[${getVenomID(venom)}]") { /** * Alternate constructor where users can supply ID and native unit model. @@ -40,18 +48,19 @@ class FalconVenom( * @param id The ID of the motor controller. * @param model The native unit model. */ - constructor(id: Int, model: NativeUnitModel) : this(CANVenom(id), model) + constructor(id: Int, model: NativeUnitModel, 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 - 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( * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output. */ override fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit) { + 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( * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output. */ override fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit) { + 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( fun falconVenom( venom: CANVenom, model: NativeUnitModel, + units: K, block: FalconVenom.() -> Unit -) = FalconVenom(venom, model).also(block) +) = FalconVenom(venom, model, units).also(block) fun falconVenom( id: Int, model: NativeUnitModel, + units: K, block: FalconVenom.() -> 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 9593963b..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( private val venom: CANVenom, - model: NativeUnitModel -) : AbstractFalconEncoder(model) { + model: NativeUnitModel, + units: K +) : AbstractFalconEncoder(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 8ac89df9..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 @@ -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 @@ -39,9 +40,10 @@ import org.ghrobotics.lib.motors.FalconMotor class FalconMAX( val canSparkMax: CANSparkMax, private val model: NativeUnitModel, + units: K, useAlternateEncoder: Boolean = false, alternateEncoderCPR: Int = 8192 -) : AbstractFalconMotor() { +) : AbstractFalconMotor("FalconMAX[${canSparkMax.deviceId}]") { /** * Creates a Spark MAX motor controller. The alternate encoder CPR is defaulted @@ -56,10 +58,11 @@ class FalconMAX( id: Int, type: CANSparkMaxLowLevel.MotorType, model: NativeUnitModel, + units: K, useAlternateEncoder: Boolean = false, alternateEncoderCPR: Int = 8196 ) : this( - CANSparkMax(id, type), model, useAlternateEncoder, alternateEncoderCPR + CANSparkMax(id, type), model, units, useAlternateEncoder, alternateEncoderCPR ) /** @@ -75,7 +78,7 @@ class FalconMAX( if (useAlternateEncoder) canSparkMax.getAlternateEncoder( AlternateEncoderType.kQuadrature, alternateEncoderCPR - ) else canSparkMax.encoder, model + ) else canSparkMax.encoder, model, units ) /** @@ -91,7 +94,8 @@ class FalconMAX( * Returns the voltage across the motor windings. */ override val voltageOutput: SIUnit - 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. @@ -169,6 +173,11 @@ class FalconMAX( * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output. */ override fun setVoltage(voltage: SIUnit, arbitraryFeedForward: SIUnit) { + if (simVoltageOutput != null) { + simVoltageOutput.set(voltage.value + arbitraryFeedForward.value) + return + } + controller.setReference(voltage.value, ControlType.kVoltage, 0, arbitraryFeedForward.value) } @@ -179,6 +188,10 @@ class FalconMAX( * @param arbitraryFeedForward The arbitrary feedforward to add to the motor output. */ override fun setDutyCycle(dutyCycle: Double, arbitraryFeedForward: SIUnit) { + if (simVoltageOutput != null) { + simVoltageOutput.set(dutyCycle * RobotController.getBatteryVoltage() + arbitraryFeedForward.value) + return + } controller.setReference(dutyCycle, ControlType.kDutyCycle, 0, arbitraryFeedForward.value) } @@ -232,16 +245,18 @@ class FalconMAX( fun falconMAX( canSparkMax: CANSparkMax, model: NativeUnitModel, + units: K, useAlternateEncoder: Boolean = false, alternateEncoderCPR: Int = 8192, block: FalconMAX.() -> Unit -) = FalconMAX(canSparkMax, model, useAlternateEncoder, alternateEncoderCPR).also(block) +) = FalconMAX(canSparkMax, model, units, useAlternateEncoder, alternateEncoderCPR).also(block) fun falconMAX( id: Int, type: CANSparkMaxLowLevel.MotorType, model: NativeUnitModel, + units: K, useAlternateEncoder: Boolean = false, alternateEncoderCPR: Int = 8192, block: FalconMAX.() -> 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 ce5eafab..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 @@ -9,6 +9,8 @@ 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 import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnit @@ -16,6 +18,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 field = CANSensor::class.java.getDeclaredField("m_device") + val canSparkMax = field.apply { isAccessible = true }.get(canEncoder) as CANSparkMax + return canSparkMax.deviceId +} + /** * Represents an encoder connected to a Spark MAX. * @@ -24,11 +32,12 @@ import org.ghrobotics.lib.motors.AbstractFalconEncoder */ class FalconMAXEncoder( val canEncoder: CANEncoder, - model: NativeUnitModel -) : AbstractFalconEncoder(model) { - /** - * Returns the raw velocity from the encoder. - */ + model: NativeUnitModel, + units: K +) : AbstractFalconEncoder(model, units, "FalconMAXEncoder[${getCanEncoderID(canEncoder)}]") { +/** + * Returns the raw velocity from the encoder. + */ override val rawVelocity: SIUnit get() = SIUnit(canEncoder.velocity / 60.0) /** 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 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..bfb2865a 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,37 @@ 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( - val model: NativeUnitModel + val model: NativeUnitModel, + units: K, + simName: String ) : FalconEncoder { - override val position: SIUnit get() = model.fromNativeUnitPosition(rawPosition) - override val velocity: SIUnit> get() = model.fromNativeUnitVelocity(rawVelocity) + + private val simDevice: SimDevice? = SimDevice.create(simName) + 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(simPosition: SIUnit) { + simPositionHandle?.set(simPosition.value) + } + + override fun setSimulatedVelocity(simVelocity: SIUnit>) { + simVelocityHandle?.set(simVelocity.value) + } + + override val position: SIUnit + get() = if (simPositionHandle != null) SIUnit(simPositionHandle.get()) + else model.fromNativeUnitPosition(rawPosition) + override val velocity: SIUnit> + get() = if (simVelocityHandle != null) SIUnit(simVelocityHandle.get()) + else model.fromNativeUnitVelocity(rawVelocity) override fun resetPosition(newPosition: SIUnit) { 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..30e08bed 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,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 : FalconMotor { +abstract class AbstractFalconMotor(simName: String) : FalconMotor { + + private val simDevice: SimDevice? = SimDevice.create(simName) + + val simVoltageOutput: SimDouble? = simDevice?.createDouble("Voltage output", true, 0.0) 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..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,11 +17,21 @@ import org.ghrobotics.lib.mathematics.units.nativeunit.NativeUnitVelocity interface FalconEncoder { /** - * The velocity of the encoder in [K]/s + * Sets the simulated [simPosition] of this motor. + */ + fun setSimulatedPosition(simPosition: SIUnit) + + /** + * Sets the simulated [simVelocity] of this motor. + */ + fun setSimulatedVelocity(simVelocity: SIUnit>) + + /** + * The velocity of the encoder in [K]/s. When in a simulation, returns the simulated velocity. */ val velocity: SIUnit> /** - * 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 @@ -34,6 +44,9 @@ interface FalconEncoder { */ val rawPosition: SIUnit + /** + * Reset the position of the encoder. Does not work in a simulation. + */ fun resetPosition(newPosition: SIUnit) fun resetPositionRaw(newPosition: SIUnit) 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..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 @@ -76,6 +79,10 @@ abstract class FalconTimedRobot { this@FalconTimedRobot.testInit() } + override fun simulationInit() { + this@FalconTimedRobot.simulationInit() + } + override fun robotPeriodic() { this@FalconTimedRobot.robotPeriodic() CommandScheduler.getInstance().run() @@ -89,21 +96,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