From 3f4009d06c9a4f5b2b3d3d2727133ea5082d0ded Mon Sep 17 00:00:00 2001 From: DanielRocksUrMom <108989218+DanielRocksUrMom@users.noreply.github.com> Date: Wed, 3 Jan 2024 09:22:10 -0500 Subject: [PATCH] Nuked more stuff and other changes --- .../controls/feedforward/FFConstants.kt | 1 - .../motorcontrol/ctre/ChargerTalonFX.kt | 127 ++++++----- .../motorcontrol/ctre/ChargerTalonSRX.kt | 56 ++--- .../motorcontrol/rev/ChargerCANSparkMax.kt | 198 +++++++++--------- .../chargers/hardware/sensors/WithOffset.kt | 13 +- .../sensors/encoders/ResettableEncoder.kt | 23 +- .../sensors/encoders/TimestampedEncoder.kt | 23 -- .../sensors/encoders/ZeroableEncoder.kt | 8 - .../encoders/absolute/ChargerCANcoder.kt | 87 ++++---- .../relative/SparkMaxEncoderAdapter.kt | 29 --- .../relative/TalonFXEncoderAdapter.kt | 45 ---- .../relative/TalonSRXEncoderAdapter.kt | 37 ---- .../hardware/sensors/imu/ChargerPigeon2.kt | 2 +- .../sensors/vision/DriverCameraView.kt | 3 + .../BasicDifferentialDrivetrain.kt | 46 +--- .../EncoderDifferentialDrivetrain.kt | 53 +++-- .../subsystems/swervedrive/TestSubystem.kt | 6 - .../utils/math/equations/ejml/Multiline.kt | 21 -- .../math/equations/ejml/MultipleSequence.kt | 29 --- .../controls/pid/SuperPIDControllerTest.kt | 6 +- 20 files changed, 294 insertions(+), 519 deletions(-) delete mode 100644 src/main/kotlin/frc/chargers/hardware/sensors/encoders/TimestampedEncoder.kt delete mode 100644 src/main/kotlin/frc/chargers/hardware/sensors/encoders/ZeroableEncoder.kt delete mode 100644 src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/SparkMaxEncoderAdapter.kt delete mode 100644 src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonFXEncoderAdapter.kt delete mode 100644 src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonSRXEncoderAdapter.kt delete mode 100644 src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/TestSubystem.kt delete mode 100644 src/main/kotlin/frc/chargers/utils/math/equations/ejml/Multiline.kt delete mode 100644 src/main/kotlin/frc/chargers/utils/math/equations/ejml/MultipleSequence.kt diff --git a/src/main/kotlin/frc/chargers/controls/feedforward/FFConstants.kt b/src/main/kotlin/frc/chargers/controls/feedforward/FFConstants.kt index 215a2c0b..587cc586 100644 --- a/src/main/kotlin/frc/chargers/controls/feedforward/FFConstants.kt +++ b/src/main/kotlin/frc/chargers/controls/feedforward/FFConstants.kt @@ -4,7 +4,6 @@ import com.batterystaple.kmeasure.quantities.* import frc.chargers.utils.math.units.* - /** * Represents constants for Feedforward control * of an angular velocity targeting mechanism with no gravity. diff --git a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonFX.kt b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonFX.kt index c1bcb7f2..5ff7b55e 100644 --- a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonFX.kt +++ b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonFX.kt @@ -15,51 +15,58 @@ import frc.chargers.hardware.configuration.HardwareConfiguration import frc.chargers.hardware.configuration.safeConfigure import frc.chargers.hardware.motorcontrol.* import frc.chargers.hardware.sensors.encoders.PositionEncoder -import frc.chargers.hardware.sensors.encoders.relative.TalonFXEncoderAdapter +import frc.chargers.hardware.sensors.encoders.ResettableEncoder import com.ctre.phoenix6.configs.TalonFXConfiguration as CTRETalonFXConfiguration - /** * Creates an instance of a falcon motor, controlled by a TalonFX motor controller. + * This function supports inline configuration using the configure lambda function, + * which has the context of a [TalonFXConfiguration]. * - * You do not need to manually factory default this motor, as it is factory defaulted on startup. - * This setting can be changed by setting factoryDefault = false. + * You do not need to manually factory default this motor, as it is factory defaulted on startup, + * before configuration. This setting can be changed by setting factoryDefault = false. */ public fun falcon( canId: Int, - canBus: String? = null, - factoryDefault: Boolean = true -): ChargerTalonFX = when { - canBus != null -> ChargerTalonFX(canId, canBus) - else -> ChargerTalonFX(canId) -}.apply { + canBus: String = "rio", + factoryDefault: Boolean = true, + // This function block has the context of a TalonFXConfiguration, + // which means that it is basically treated as if it was called with the class itself. + // Example Usage: falcon(6){ beepOnBoot = true; neutralMode = NeutralModeValue.Brake } + configure: TalonFXConfiguration.() -> Unit = {} +): ChargerTalonFX = ChargerTalonFX(canId, canBus).apply { // factory defaults configs on startup is factoryDefault = true if (factoryDefault) { configurator.apply(CTRETalonFXConfiguration(), 0.02) } + val config = TalonFXConfiguration().apply(configure) + if (config != TalonFXConfiguration()){ + configure(config) + } } -/** - * Creates an instance of a falcon motor, controlled by a TalonFX motor controller. - * This function supports inline configuration using the configure lambda function, - * which has the context of a [TalonFXConfiguration]. - * - * You do not need to manually factory default this motor, as it is factory defaulted on startup, - * before configuration. This setting can be changed by setting factoryDefault = false. - */ -public inline fun falcon( - canId: Int, - canBus: String? = null, - factoryDefault: Boolean = true, - configure: TalonFXConfiguration.() -> Unit -): ChargerTalonFX = - falcon(canId,canBus,factoryDefault).apply { - // configures the motor by "applying" a context function(configure) - // onto a configuration object, then configuring using that config object. - configure(TalonFXConfiguration().apply(configure)) + +public class TalonFXEncoderAdapter( + private val motorController: TalonFX +): ResettableEncoder { + private val positionSignal = motorController.position + private val velocitySignal = motorController.velocity + + override fun setZero(newZero: Angle) { + val errorCode = motorController.setPosition(newZero.inUnit(rotations)) + if (errorCode != StatusCode.OK){ + error("When attempting to zero a talon fx, an error occurred: $errorCode") + } } + override val angularPosition: Angle + get() = positionSignal.refresh(true).value.ofUnit(rotations) + + override val angularVelocity: AngularVelocity + get() = velocitySignal.refresh(true).value.ofUnit(rotations/seconds) +} + /** * Represents a TalonFX motor controller. @@ -73,68 +80,72 @@ public inline fun falcon( public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"): TalonFX(deviceNumber, canBus), SmartEncoderMotorController, HardwareConfigurable { + @Suppress("LeakingThis") // Known to be safe; CTREMotorControllerEncoderAdapter ONLY uses final functions + // and does not pass around the reference to this class. + /** + * The encoder of the TalonFX. + */ + final override val encoder: TalonFXEncoderAdapter = + TalonFXEncoderAdapter(this) + + private val voltageSignal = supplyVoltage private val currentSignal = statorCurrent private val tempSignal = deviceTemp - final override val appliedVoltage: Voltage + override val appliedVoltage: Voltage get() = voltageSignal.refresh(true).value.ofUnit(volts) - final override val appliedCurrent: Current + override val appliedCurrent: Current get() = currentSignal.refresh(true).value.ofUnit(amps) - final override val tempCelsius: Double + override val tempCelsius: Double get() = tempSignal.refresh(true).value - @Suppress("LeakingThis") // Known to be safe; CTREMotorControllerEncoderAdapter ONLY uses final functions - // and does not pass around the reference to this class. - final override val encoder: TalonFXEncoderAdapter = - TalonFXEncoderAdapter(this) - private val currentSlotConfigs = Slot0Configs() private val velocityRequest = VelocityVoltage(0.0).also{ it.Slot = 0 } private val positionRequest = PositionVoltage(0.0).also{it.Slot = 0 } - private var currentPIDConstants = PIDConstants.None - private var currentFFConstants = AngularMotorFFConstants.None + private fun currentPIDConstants() = PIDConstants(currentSlotConfigs.kP, currentSlotConfigs.kI, currentSlotConfigs.kD) + + private fun setPIDConstants(newConstants: PIDConstants){ + currentSlotConfigs.kP = newConstants.kP + currentSlotConfigs.kI = newConstants.kI + currentSlotConfigs.kD = newConstants.kD + } - final override fun setAngularVelocity( + + override fun setAngularVelocity( target: AngularVelocity, pidConstants: PIDConstants, feedforwardConstants: AngularMotorFFConstants ) { - if (currentPIDConstants != pidConstants){ - currentSlotConfigs.kP = pidConstants.kP - currentSlotConfigs.kI = pidConstants.kI - currentSlotConfigs.kD = pidConstants.kD - currentPIDConstants = pidConstants + var configHasChanged = false + if (currentPIDConstants() != pidConstants){ + setPIDConstants(pidConstants) + configHasChanged = true } - if (currentFFConstants != feedforwardConstants){ + if (currentSlotConfigs.kS != feedforwardConstants.kS.inUnit(volts) || currentSlotConfigs.kV != feedforwardConstants.kV.inUnit(volts * seconds / rotations)){ currentSlotConfigs.kS = feedforwardConstants.kS.inUnit(volts) currentSlotConfigs.kV = feedforwardConstants.kV.inUnit(volts * seconds / rotations) - currentFFConstants = feedforwardConstants + configHasChanged = true + } + if (configHasChanged){ + configurator.apply(currentSlotConfigs) } velocityRequest.Velocity = target.inUnit(rotations/seconds) setControl(velocityRequest) } - private var Slot0Configs.pidConstants: PIDConstants - get() = PIDConstants(kP,kI,kD) - set(newConstants){ - kP = newConstants.kP - kI = newConstants.kI - kD = newConstants.kD - } - - final override fun setAngularPosition( + override fun setAngularPosition( target: Angle, pidConstants: PIDConstants, absoluteEncoder: PositionEncoder?, extraVoltage: Voltage ) { - if (currentSlotConfigs.pidConstants != pidConstants){ - currentSlotConfigs.pidConstants = pidConstants + if (currentPIDConstants() != pidConstants){ + setPIDConstants(pidConstants) configurator.apply(currentSlotConfigs) } if (absoluteEncoder == null){ @@ -146,6 +157,8 @@ public open class ChargerTalonFX(deviceNumber: Int, canBus: String = "rio"): setControl(positionRequest) } + + private val allConfigErrors: LinkedHashSet = linkedSetOf() private var configAppliedProperly = true private fun StatusCode.updateConfigStatus(): StatusCode { diff --git a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonSRX.kt b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonSRX.kt index e7575643..e1c3693d 100644 --- a/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonSRX.kt +++ b/src/main/kotlin/frc/chargers/hardware/motorcontrol/ctre/ChargerTalonSRX.kt @@ -2,15 +2,14 @@ package frc.chargers.hardware.motorcontrol.ctre import com.batterystaple.kmeasure.quantities.* import com.batterystaple.kmeasure.units.* +import com.ctre.phoenix.ErrorCode import com.ctre.phoenix.motorcontrol.* -import com.ctre.phoenix.motorcontrol.can.BaseTalon import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX -import com.ctre.phoenix.sensors.CANCoder import frc.chargers.hardware.configuration.HardwareConfigurable import frc.chargers.hardware.configuration.HardwareConfiguration import frc.chargers.hardware.motorcontrol.* import frc.chargers.hardware.sensors.encoders.Encoder -import frc.chargers.hardware.sensors.encoders.relative.TalonSRXEncoderAdapter +import frc.chargers.hardware.sensors.encoders.ResettableEncoder import kotlin.math.roundToInt private const val TALON_SRX_ENCODER_UNITS_PER_ROTATION = 2048 // From https://docs.ctre-phoenix.com/en/latest/ch14_MCSensor.html#sensor-resolution @@ -58,6 +57,36 @@ public inline fun redlineSRX( it.configure(TalonSRXConfiguration().apply(configure)) } +public class TalonSRXEncoderAdapter( + private val ctreMotorController: IMotorController, + private val pidIndex: Int, + private val anglePerPulse: Angle +) : ResettableEncoder, IMotorController by ctreMotorController { + public constructor( + ctreMotorController: IMotorController, + pidIndex: Int, + pulsesPerRotation: Int /* Can't use Double here or both constructors will have the same JVM signature */ + ) : this(ctreMotorController, pidIndex, (1/pulsesPerRotation.toDouble()).ofUnit(rotations)) + + override fun setZero(newZero: Angle) { + val errorCode = ctreMotorController.setSelectedSensorPosition((newZero/anglePerPulse).siValue, pidIndex, DEFAULT_TIMEOUT_MS) + if (errorCode != ErrorCode.OK){ + error("When setting the zero of a talon srx's encoder, an error ocurred: $errorCode") + } + } + + override val angularPosition: Angle + get() = ctreMotorController.getSelectedSensorPosition(pidIndex) * anglePerPulse + + override val angularVelocity: AngularVelocity + get() = ctreMotorController.getSelectedSensorVelocity(pidIndex) * anglePerPulse / timeBetweenPulses + + public companion object { + private const val DEFAULT_TIMEOUT_MS = 500 + private val timeBetweenPulses = 100.milli.seconds + } +} + /** * Represents a TalonSRX motor controller. * Includes everything in the CTRE TalonSRX class, @@ -107,19 +136,6 @@ public open class ChargerTalonSRX( configuration.selectedFeedbackCoefficients.forEach { (pidIndex, coefficient) -> configSelectedFeedbackCoefficient(coefficient, pidIndex, TIMEOUT_MILLIS) } - configuration.remoteFeedbackFilter?.let { - when(it) { - is BaseTalonRemoteFeedbackFilterDevice -> configRemoteFeedbackFilter(it.talon, it.remoteOrdinal, - TIMEOUT_MILLIS - ) - is CANCoderRemoteFeedbackFilterDevice -> configRemoteFeedbackFilter(it.canCoder, it.remoteOrdinal, - TIMEOUT_MILLIS - ) - is OtherRemoteFeedbackFilterDevice -> configRemoteFeedbackFilter(it.deviceId, it.remoteSensorSource, it.remoteOrdinal, - TIMEOUT_MILLIS - ) - } - } configuration.sensorTermFeedbackDevices.forEach { (sensorTerm, feedbackDevice) -> configSensorTerm(sensorTerm, feedbackDevice, TIMEOUT_MILLIS) } @@ -177,7 +193,6 @@ public data class TalonSRXConfiguration( var voltageCompensationEnabled: Boolean? = null, val selectedFeedbackSensors: MutableMap = mutableMapOf(), val selectedFeedbackCoefficients: MutableMap = mutableMapOf(), - var remoteFeedbackFilter: RemoteFeedbackFilterDevice? = null, val sensorTermFeedbackDevices: MutableMap = mutableMapOf(), val controlFramePeriods: MutableMap = mutableMapOf(), val statusFramePeriods: MutableMap = mutableMapOf(), @@ -192,12 +207,5 @@ public data class TalonSRXConfiguration( val customParameters: MutableMap = mutableMapOf() ): HardwareConfiguration -public sealed interface RemoteFeedbackFilterDevice -public data class CANCoderRemoteFeedbackFilterDevice(val canCoder: CANCoder, val remoteOrdinal: Int) : - RemoteFeedbackFilterDevice -public data class BaseTalonRemoteFeedbackFilterDevice(val talon: BaseTalon, val remoteOrdinal: Int) : - RemoteFeedbackFilterDevice -public data class OtherRemoteFeedbackFilterDevice(val deviceId: Int, val remoteSensorSource: RemoteSensorSource, val remoteOrdinal: Int) : - RemoteFeedbackFilterDevice public data class LimitSwitchConfig(val type: RemoteLimitSwitchSource, val normalOpenOrClose: LimitSwitchNormal, val deviceId: Int) diff --git a/src/main/kotlin/frc/chargers/hardware/motorcontrol/rev/ChargerCANSparkMax.kt b/src/main/kotlin/frc/chargers/hardware/motorcontrol/rev/ChargerCANSparkMax.kt index 2dd81f77..b708c384 100644 --- a/src/main/kotlin/frc/chargers/hardware/motorcontrol/rev/ChargerCANSparkMax.kt +++ b/src/main/kotlin/frc/chargers/hardware/motorcontrol/rev/ChargerCANSparkMax.kt @@ -23,50 +23,28 @@ import frc.chargers.wpilibextensions.delay import kotlin.math.roundToInt -/** - * A convenience function to create a [ChargerCANSparkMax] - * specifically to drive a Neo motor. - * - * You do not need to manually factory default this motor, as it is factory defaulted on startup. - * This setting can be changed by setting factoryDefault = false. - */ -public fun neoSparkMax( - canBusId: Int, - alternateEncoderConfiguration: AlternateEncoderConfiguration? = null, - factoryDefault: Boolean = true, -): ChargerCANSparkMax = - ChargerCANSparkMax(canBusId, CANSparkMaxLowLevel.MotorType.kBrushless, alternateEncoderConfiguration) - .also { - if (factoryDefault) { - it.restoreFactoryDefaults() - delay(200.milli.seconds) - println("SparkMax has been factory defaulted.") - } - } - - -/** - * A convenience function to create a [ChargerCANSparkMax] - * specifically to drive a brushed motor, such as a CIM. - * - * You do not need to manually factory default this motor, as it is factory defaulted on startup. - * This setting can be changed by setting factoryDefault = false. - */ -public fun brushedSparkMax( - canBusId: Int, - alternateEncoderConfiguration: AlternateEncoderConfiguration? = null, - factoryDefault: Boolean = true, -): ChargerCANSparkMax = - ChargerCANSparkMax(canBusId, CANSparkMaxLowLevel.MotorType.kBrushed, alternateEncoderConfiguration) - .also { - if (factoryDefault) { - it.restoreFactoryDefaults() - delay(200.milli.seconds) - println("SparkMax has been factory defaulted.") - } - } - +public sealed class SparkMaxEncoderType{ + /** + * Represents a regular spark max encoder. + */ + public data object Regular: SparkMaxEncoderType() + /** + * Represents a Spark Max Alternate encoder. + */ + public data class Alternate( + val countsPerRev: Int, + val encoderMeasurementPeriod: Time? = null, + val type: SparkMaxAlternateEncoder.Type = SparkMaxAlternateEncoder.Type.kQuadrature + ): SparkMaxEncoderType() + + /** + * Represents an absolute encoder connected to a spark max. + */ + public data class Absolute( + val type: SparkMaxAbsoluteEncoder.Type = SparkMaxAbsoluteEncoder.Type.kDutyCycle + ): SparkMaxEncoderType() +} /** * A convenience function to create a [ChargerCANSparkMax] @@ -80,15 +58,21 @@ public fun brushedSparkMax( */ public inline fun neoSparkMax( canBusId: Int, - alternateEncoderConfiguration: AlternateEncoderConfiguration? = null, factoryDefault: Boolean = true, - configure: SparkMaxConfiguration.() -> Unit + encoderType: SparkMaxEncoderType = SparkMaxEncoderType.Regular, + configure: SparkMaxConfiguration.() -> Unit = {} ): ChargerCANSparkMax = - neoSparkMax(canBusId, alternateEncoderConfiguration, factoryDefault) + ChargerCANSparkMax(canBusId, CANSparkMaxLowLevel.MotorType.kBrushless, encoderType) .also { + if (factoryDefault) { + it.restoreFactoryDefaults() + delay(200.milli.seconds) + println("SparkMax has been factory defaulted.") + } it.configure(SparkMaxConfiguration().apply(configure)) } + /** * A convenience function to create a [ChargerCANSparkMax] * specifically to drive a brushed motor, such as a CIM. @@ -101,29 +85,73 @@ public inline fun neoSparkMax( */ public inline fun brushedSparkMax( canBusId: Int, - alternateEncoderConfiguration: AlternateEncoderConfiguration? = null, factoryDefault: Boolean = true, - configure: SparkMaxConfiguration.() -> Unit + encoderType: SparkMaxEncoderType = SparkMaxEncoderType.Regular, + configure: SparkMaxConfiguration.() -> Unit = {} ): ChargerCANSparkMax = - brushedSparkMax(canBusId, alternateEncoderConfiguration, factoryDefault) + ChargerCANSparkMax(canBusId, CANSparkMaxLowLevel.MotorType.kBrushed, encoderType) .also { + if (factoryDefault) { + it.restoreFactoryDefaults() + delay(200.milli.seconds) + println("SparkMax has been factory defaulted.") + } it.configure(SparkMaxConfiguration().apply(configure)) } -public class SparkMaxEncoderAdapter(private val revEncoder: RelativeEncoder) : ResettableEncoder, RelativeEncoder by revEncoder { - private var previousPosition = revEncoder.position.ofUnit(rotations) + + +public class SparkMaxEncoderAdaptor( + private val revEncoder: MotorFeedbackSensor +) : ResettableEncoder{ + + init{ + require ( + revEncoder is AbsoluteEncoder || revEncoder is RelativeEncoder + ){"Encoder type of spark max is invalid: internal error."} + } + + private fun getPosition(): Angle = when (revEncoder){ + is AbsoluteEncoder -> revEncoder.position.ofUnit(rotations) + is RelativeEncoder -> revEncoder.position.ofUnit(rotations) + else -> error("Invalid encoder type for spark max") + } + + private fun getVelocity(): AngularVelocity = when (revEncoder){ + is AbsoluteEncoder -> revEncoder.velocity.ofUnit(rotations / seconds) + is RelativeEncoder -> revEncoder.velocity.ofUnit(rotations / minutes) + else -> error("Invalid encoder type for spark max") + } + + internal fun setInverted(value: Boolean): REVLibError = when (revEncoder){ + is AbsoluteEncoder -> revEncoder.setInverted(value) + is RelativeEncoder -> revEncoder.setInverted(value) + else -> error("Invalid encoder type for spark max") + } + + internal fun setAverageDepth(depth: Int): REVLibError = when(revEncoder){ + is AbsoluteEncoder -> revEncoder.setAverageDepth(depth) + is RelativeEncoder -> revEncoder.setAverageDepth(depth) + else -> error("Invalid encoder type for spark max") + } + + + private var previousPosition = getPosition() private var previousVelocity = AngularVelocity(0.0) override fun setZero(newZero: Angle){ - position = newZero.inUnit(rotations) + when (revEncoder){ + is AbsoluteEncoder -> revEncoder.setZeroOffset(newZero.inUnit(rotations)) + is RelativeEncoder -> revEncoder.setPosition(newZero.inUnit(rotations)) + } } override val angularPosition: Angle - get() = revEncoder.position.ofUnit(rotations) + get() = getPosition() .revertIfInvalid(previousPosition) .also{ previousPosition = it } override val angularVelocity: AngularVelocity - get() = revEncoder.velocity.ofUnit(rotations / minutes) + get() = getVelocity() .revertIfInvalid(previousVelocity) .also{ previousVelocity = it } } @@ -140,47 +168,32 @@ public class SparkMaxEncoderAdapter(private val revEncoder: RelativeEncoder) : R public open class ChargerCANSparkMax( deviceId: Int, type: MotorType, - alternateEncoderConfiguration: AlternateEncoderConfiguration? = null + encoderType: SparkMaxEncoderType = SparkMaxEncoderType.Regular ) : CANSparkMax(deviceId, type), SmartEncoderMotorController, HardwareConfigurable{ + final override val encoder: SparkMaxEncoderAdaptor = when (encoderType){ + SparkMaxEncoderType.Regular -> SparkMaxEncoderAdaptor( + super.getEncoder() + ) + + is SparkMaxEncoderType.Alternate -> SparkMaxEncoderAdaptor( + super.getAlternateEncoder(encoderType.type, encoderType.countsPerRev).also{ + if (encoderType.encoderMeasurementPeriod != null){ + it.measurementPeriod = encoderType.encoderMeasurementPeriod.inUnit(milli.seconds).toInt() + } + } + ) + is SparkMaxEncoderType.Absolute -> SparkMaxEncoderAdaptor( + super.getAbsoluteEncoder(encoderType.type) + ) + } - private inner class EncoderConfiguration( - var averageDepth: Int? = null, - var inverted: Boolean? = null, - var measurementPeriod: Time? = null, - var positionConversionFactor: Double? = null, - var velocityConversionFactor: Double? = null, - ) - private var encoderConfig = EncoderConfiguration() private var previousCurrent = Current(0.0) private var previousTemp = 0.0 private var previousVoltage = Voltage(0.0) - - /* - CANSparkMax throws some exception if this is initialized on motor creation, - so by lazy here delays the initialization until the encoder is accessed. - */ - override val encoder: SparkMaxEncoderAdapter by lazy { - (alternateEncoderConfiguration?.let { (countsPerRev, encoderType) -> - if (encoderType == null) { - SparkMaxEncoderAdapter(super.getAlternateEncoder(countsPerRev)) - } else { - SparkMaxEncoderAdapter(super.getAlternateEncoder(encoderType, countsPerRev)) - } - } - ?: SparkMaxEncoderAdapter(super.getEncoder()) - ).apply{ - encoderConfig.averageDepth?.let{averageDepth = it} - encoderConfig.inverted?.let{inverted = it} - encoderConfig.measurementPeriod?.let{measurementPeriod = (it.inUnit(seconds) * 1000).toInt()} - encoderConfig.positionConversionFactor?.let{positionConversionFactor = it} - encoderConfig.velocityConversionFactor?.let{velocityConversionFactor = it} - } - } - override val appliedCurrent: Current get() = outputCurrent.ofUnit(amps) .revertIfInvalid(previousCurrent) @@ -307,6 +320,9 @@ public open class ChargerCANSparkMax( for ((limitDirection, limit) in configuration.softLimits) { setSoftLimit(limitDirection, limit.inUnit(rotations).toFloat())?.updateConfigStatus() } + configuration.encoderAverageDepth?.let{ encoder.setAverageDepth(it) }?.updateConfigStatus() + configuration.encoderInverted?.let{ encoder.setInverted(it) }?.updateConfigStatus() + return@safeConfigure configAppliedProperly } @@ -339,12 +355,8 @@ public data class SparkMaxConfiguration( var secondaryCurrentLimit: SecondaryCurrentLimit? = null, val softLimits: MutableMap = mutableMapOf(), - // encoder configs var encoderAverageDepth: Int? = null, var encoderInverted: Boolean? = null, - var encoderMeasurementPeriod: Time? = null, - var encoderPositionConversionFactor: Double? = null, - var encoderVelocityConversionFactor: Double? = null, // sparkMaxPIDController configs; not used by ChargerLib var feedbackDFilter: Double? = null, @@ -354,11 +366,5 @@ public data class SparkMaxConfiguration( var positionPIDWrappingInputRange: ClosedRange? = null, ) : HardwareConfiguration -public data class AlternateEncoderConfiguration(val countsPerRev: Int, val encoderType: SparkMaxAlternateEncoder.Type? = null) { - public companion object { - public val SRXMagnetic1X: AlternateEncoderConfiguration = AlternateEncoderConfiguration(1024) - public val SRXMagnetic4X: AlternateEncoderConfiguration = AlternateEncoderConfiguration(4096) - } -} public data class SmartCurrentLimit(val stallLimit: Current, val freeLimit: Current? = null, val limitSpeed: AngularVelocity? = null) public data class SecondaryCurrentLimit(val limit: Current, val chopCycles: Int? = null) \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/WithOffset.kt b/src/main/kotlin/frc/chargers/hardware/sensors/WithOffset.kt index 604ac4a5..55027173 100644 --- a/src/main/kotlin/frc/chargers/hardware/sensors/WithOffset.kt +++ b/src/main/kotlin/frc/chargers/hardware/sensors/WithOffset.kt @@ -5,18 +5,29 @@ import frc.chargers.hardware.sensors.encoders.Encoder import frc.chargers.hardware.sensors.encoders.PositionEncoder import frc.chargers.hardware.sensors.imu.gyroscopes.HeadingProvider +/** + * Creates a [PositionEncoder] with a certain zero offset. + */ public fun PositionEncoder.withOffset(zeroOffset: Angle): PositionEncoder = object: PositionEncoder { override val angularPosition: Angle get() = this@withOffset.angularPosition - zeroOffset - } +/** + * Creates a [Encoder] with a certain zero offset. + */ public fun Encoder.withOffset(zeroOffset: Angle): Encoder = object: Encoder by this{ override val angularPosition: Angle get() = this@withOffset.angularPosition - zeroOffset } +/** + * Creates a [HeadingProvider] with a certain zero offset. + */ public fun HeadingProvider.withOffset(zeroOffset: Angle): HeadingProvider = HeadingProvider{heading - zeroOffset} +/** + * Creates a [HeadingProvider] that zeroes itself according to it's existing position. + */ public fun HeadingProvider.withZero(): HeadingProvider = withOffset(heading) diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ResettableEncoder.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ResettableEncoder.kt index 9004fe3c..c70ce513 100644 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ResettableEncoder.kt +++ b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ResettableEncoder.kt @@ -1,7 +1,6 @@ package frc.chargers.hardware.sensors.encoders import com.batterystaple.kmeasure.quantities.Angle -import com.batterystaple.kmeasure.units.degrees /** * An encoder that allows resetting zero values. @@ -10,30 +9,12 @@ import com.batterystaple.kmeasure.units.degrees * Depending on the underlying implementation, new zeroes * may or may not persist between restarts or runs of the program. */ -public interface ResettableEncoder : ZeroableEncoder { +public interface ResettableEncoder : Encoder { /** * Sets the specified angular position to be the new zero. */ public fun setZero(newZero: Angle) - override fun setZero() { setZero(angularPosition) } + public fun setZero() { setZero(angularPosition) } } -/** - * an encoder which can both be reset and which provides timestamps. - * - * @see TimestampedEncoder - * @see ResettableEncoder - */ -public interface ResettableTimestampedEncoder: ResettableEncoder, TimestampedEncoder - -public class ZeroableEncoderOffsetManager(private val zeroableEncoder: ZeroableEncoder): ResettableEncoder, ZeroableEncoder by zeroableEncoder { - public var offset: Angle = 0.degrees - public override fun setZero() { - offset = 0.degrees - } - - public override fun setZero(newZero: Angle) { - offset -= newZero // Seems kinda weird, but I did the math and I'm pretty sure this is the correct formula - } -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/TimestampedEncoder.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/TimestampedEncoder.kt deleted file mode 100644 index 4dddaabc..00000000 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/TimestampedEncoder.kt +++ /dev/null @@ -1,23 +0,0 @@ -package frc.chargers.hardware.sensors.encoders - -import com.batterystaple.kmeasure.dimensions.AngleDimension -import com.batterystaple.kmeasure.dimensions.AngularVelocityDimension -import com.batterystaple.kmeasure.quantities.Angle -import com.batterystaple.kmeasure.quantities.AngularVelocity -import frc.chargers.utils.QuantityMeasurement - -/** - * Represents an encoder that has timestamps - * corresponding to its angular position and angular velocity measurements. - */ -public interface TimestampedEncoder: Encoder { - public val timestampedAngularPosition: QuantityMeasurement - - public val timestampedAngularVelocity: QuantityMeasurement - - override val angularPosition: Angle - get() = timestampedAngularPosition.value - - override val angularVelocity: AngularVelocity - get() = timestampedAngularVelocity.value -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ZeroableEncoder.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ZeroableEncoder.kt deleted file mode 100644 index 1ce3309e..00000000 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/ZeroableEncoder.kt +++ /dev/null @@ -1,8 +0,0 @@ -package frc.chargers.hardware.sensors.encoders - -public interface ZeroableEncoder : Encoder { - /** - * Sets the current angular position to be the new zero. - */ - public fun setZero() -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/absolute/ChargerCANcoder.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/absolute/ChargerCANcoder.kt index a027f2d3..56574fb3 100644 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/absolute/ChargerCANcoder.kt +++ b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/absolute/ChargerCANcoder.kt @@ -1,7 +1,5 @@ package frc.chargers.hardware.sensors.encoders.absolute -import com.batterystaple.kmeasure.dimensions.AngleDimension -import com.batterystaple.kmeasure.dimensions.AngularVelocityDimension import com.batterystaple.kmeasure.quantities.* import com.batterystaple.kmeasure.units.hertz import com.batterystaple.kmeasure.units.rotations @@ -14,9 +12,7 @@ import com.ctre.phoenix6.signals.SensorDirectionValue import frc.chargers.hardware.configuration.HardwareConfigurable import frc.chargers.hardware.configuration.HardwareConfiguration import frc.chargers.hardware.configuration.safeConfigure -import frc.chargers.hardware.sensors.encoders.ResettableTimestampedEncoder -import frc.chargers.hardware.sensors.encoders.TimestampedEncoder -import frc.chargers.utils.QuantityMeasurement +import frc.chargers.hardware.sensors.encoders.ResettableEncoder /** @@ -41,7 +37,7 @@ public class ChargerCANcoder( deviceId: Int, canBus: String = "", factoryDefault: Boolean = true -): CTRECANcoder(deviceId, canBus), ResettableTimestampedEncoder, HardwareConfigurable { +): CTRECANcoder(deviceId, canBus), ResettableEncoder, HardwareConfigurable { init{ if (factoryDefault){ @@ -50,7 +46,40 @@ public class ChargerCANcoder( } } - public val absolute: TimestampedEncoder = AbsoluteEncoderAdaptor() + /** + * Represents the absolute encoder of the CANcoder. + */ + public val absolute: ResettableEncoder = AbsoluteEncoderAdaptor() + + private inner class AbsoluteEncoderAdaptor: ResettableEncoder by this, HardwareConfigurable by this{ + private val absolutePosSignal = absolutePosition + + override val angularPosition: Angle + get() = absolutePosSignal.refresh(true).value.ofUnit(rotations) + } + + + private var filterVelocity: Boolean = true + private val posSignal = position + private val velSignal = if (filterVelocity) velocity else unfilteredVelocity + + override fun setZero(newZero: Angle){ + setPosition(newZero.inUnit(rotations)) + } + + /** + * Obtains the relative position from the CANcoder. + */ + override val angularPosition: Angle + get() = posSignal.refresh(true).value.ofUnit(rotations) + + /** + * Obtains the velocity of the CANcoder. + */ + override val angularVelocity: AngularVelocity + get() = velSignal.refresh(true).value.ofUnit(rotations/seconds) + + private val allConfigErrors: LinkedHashSet = linkedSetOf() private var configAppliedProperly = true @@ -89,50 +118,6 @@ public class ChargerCANcoder( return@safeConfigure configAppliedProperly } } - - private var filterVelocity: Boolean = true - private val posSignal = position - private val velSignal = if (filterVelocity) velocity else unfilteredVelocity - - override fun setZero(newZero: Angle){ - setPosition(newZero.inUnit(rotations)) - } - - override val timestampedAngularPosition: QuantityMeasurement - get(){ - val statusSignal = posSignal.refresh() - return QuantityMeasurement( - value = statusSignal.value.ofUnit(rotations), - timestamp = statusSignal.timestamp.time.ofUnit(seconds) - ) - } - - override val timestampedAngularVelocity: QuantityMeasurement - get(){ - val statusSignal = velSignal.refresh(true) - return QuantityMeasurement( - value = statusSignal.value.ofUnit(rotations / seconds), - timestamp = statusSignal.timestamp.time.ofUnit(seconds), - ) - } - - - private inner class AbsoluteEncoderAdaptor: TimestampedEncoder by this, HardwareConfigurable by this{ - - private val absolutePosSignal = absolutePosition - - override val timestampedAngularPosition: QuantityMeasurement - get(){ - val statusSignal = absolutePosSignal.refresh(true) - return QuantityMeasurement( - value = statusSignal.value.ofUnit(rotations), - timestamp = statusSignal.timestamp.time.ofUnit(seconds) - ) - } - - override val angularPosition: Angle - get() = absolutePosSignal.refresh(true).value.ofUnit(rotations) - } } diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/SparkMaxEncoderAdapter.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/SparkMaxEncoderAdapter.kt deleted file mode 100644 index e3f1817f..00000000 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/SparkMaxEncoderAdapter.kt +++ /dev/null @@ -1,29 +0,0 @@ -package frc.chargers.hardware.sensors.encoders.relative - -import com.batterystaple.kmeasure.quantities.* -import com.batterystaple.kmeasure.units.minutes -import com.batterystaple.kmeasure.units.rotations -import com.revrobotics.RelativeEncoder -import frc.chargers.hardware.sensors.encoders.ResettableEncoder -import frc.chargers.utils.revertIfInvalid - -/** - * An adapter from the REV RelativeEncoder class to the ChargerLib Encoder interface. - */ -public class SparkMaxEncoderAdapter(private val revEncoder: RelativeEncoder) : ResettableEncoder, RelativeEncoder by revEncoder { - private var previousPosition = revEncoder.position.ofUnit(rotations) - private var previousVelocity = AngularVelocity(0.0) - - override fun setZero(newZero: Angle){ - position = newZero.inUnit(rotations) - } - override val angularPosition: Angle - get() = revEncoder.position.ofUnit(rotations) - .revertIfInvalid(previousPosition) - .also{ previousPosition = it } - - override val angularVelocity: AngularVelocity - get() = revEncoder.velocity.ofUnit(rotations / minutes) - .revertIfInvalid(previousVelocity) - .also{ previousVelocity = it } -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonFXEncoderAdapter.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonFXEncoderAdapter.kt deleted file mode 100644 index 3209b63f..00000000 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonFXEncoderAdapter.kt +++ /dev/null @@ -1,45 +0,0 @@ -package frc.chargers.hardware.sensors.encoders.relative - -import com.batterystaple.kmeasure.dimensions.AngleDimension -import com.batterystaple.kmeasure.dimensions.AngularVelocityDimension -import com.batterystaple.kmeasure.quantities.* -import com.batterystaple.kmeasure.units.rotations -import com.batterystaple.kmeasure.units.seconds -import com.ctre.phoenix6.hardware.TalonFX -import frc.chargers.hardware.sensors.encoders.ResettableTimestampedEncoder -import frc.chargers.utils.QuantityMeasurement - -/** - * Adapts the TalonFX motor class's builtin encoder functionality to the charger encoder interface. - * Also used for fusedCANCoder applications - */ -public class TalonFXEncoderAdapter( - private val motorController: TalonFX -): ResettableTimestampedEncoder { - - override fun setZero(newZero: Angle) { - motorController.setPosition(newZero.inUnit(rotations)) - } - - private val positionSignal = motorController.position - private val velocitySignal = motorController.velocity - - override val timestampedAngularPosition: QuantityMeasurement - get(){ - val statusSignal = positionSignal.refresh(true) - return QuantityMeasurement( - value = statusSignal.value.ofUnit(rotations), - timestamp = statusSignal.timestamp.time.ofUnit(seconds) - ) - } - - override val timestampedAngularVelocity: QuantityMeasurement - get(){ - val statusSignal = velocitySignal.refresh() - return QuantityMeasurement( - value = statusSignal.value.ofUnit(rotations / seconds), - timestamp = statusSignal.timestamp.time.ofUnit(seconds) - ) - } - -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonSRXEncoderAdapter.kt b/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonSRXEncoderAdapter.kt deleted file mode 100644 index ab17c1b1..00000000 --- a/src/main/kotlin/frc/chargers/hardware/sensors/encoders/relative/TalonSRXEncoderAdapter.kt +++ /dev/null @@ -1,37 +0,0 @@ -package frc.chargers.hardware.sensors.encoders.relative - -import com.batterystaple.kmeasure.quantities.* -import com.batterystaple.kmeasure.units.milli -import com.batterystaple.kmeasure.units.rotations -import com.batterystaple.kmeasure.units.seconds -import com.ctre.phoenix.motorcontrol.IMotorController -import frc.chargers.hardware.sensors.encoders.Encoder - -/** - * An adapter from the phoenix v5 encoder class to the ChargerLib Encoder interface. - */ -public class TalonSRXEncoderAdapter( - private val ctreMotorController: IMotorController, - private val pidIndex: Int, - private val anglePerPulse: Angle -) : Encoder, IMotorController by ctreMotorController { - public constructor( - ctreMotorController: IMotorController, - pidIndex: Int, - pulsesPerRotation: Int /* Can't use Double here or both constructors will have the same JVM signature */ - ) : this(ctreMotorController, pidIndex, (1/pulsesPerRotation.toDouble()).ofUnit(rotations)) - - override var angularPosition: Angle - get() = ctreMotorController.getSelectedSensorPosition(pidIndex) * anglePerPulse - set(value) { - ctreMotorController.setSelectedSensorPosition((value/anglePerPulse).value, pidIndex, DEFAULT_TIMEOUT_MS) // TODO: Should throw if error code? - } - - override val angularVelocity: AngularVelocity - get() = ctreMotorController.getSelectedSensorVelocity(pidIndex) * anglePerPulse / timeBetweenPulses - - public companion object { - private const val DEFAULT_TIMEOUT_MS = 500 - private val timeBetweenPulses = 100.milli.seconds - } -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/imu/ChargerPigeon2.kt b/src/main/kotlin/frc/chargers/hardware/sensors/imu/ChargerPigeon2.kt index ba398d78..c4a708e1 100644 --- a/src/main/kotlin/frc/chargers/hardware/sensors/imu/ChargerPigeon2.kt +++ b/src/main/kotlin/frc/chargers/hardware/sensors/imu/ChargerPigeon2.kt @@ -37,7 +37,7 @@ public fun ChargerPigeon2( public class ChargerPigeon2( canId: Int, canBus: String = "rio" -): Pigeon2(canId, canBus), ZeroableHeadingProvider, HardwareConfigurable{ +): Pigeon2(canId, canBus), ZeroableHeadingProvider, HardwareConfigurable { /** * The gyroscope of the Pigeon; contains yaw, pitch, and roll data diff --git a/src/main/kotlin/frc/chargers/hardware/sensors/vision/DriverCameraView.kt b/src/main/kotlin/frc/chargers/hardware/sensors/vision/DriverCameraView.kt index 59b91191..602d443e 100644 --- a/src/main/kotlin/frc/chargers/hardware/sensors/vision/DriverCameraView.kt +++ b/src/main/kotlin/frc/chargers/hardware/sensors/vision/DriverCameraView.kt @@ -4,6 +4,9 @@ import edu.wpi.first.networktables.NetworkTableEntry import edu.wpi.first.networktables.NetworkTableInstance import edu.wpi.first.cameraserver.CameraServer +/** + * Wraps WPILib's [CameraServer]; being used to handle driver cameras. + */ public class DriverCameraView( totalDriverCams: Int = 1, private val defaultResWidth: Int, diff --git a/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/BasicDifferentialDrivetrain.kt b/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/BasicDifferentialDrivetrain.kt index 6c2dde4f..2bea2ab1 100644 --- a/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/BasicDifferentialDrivetrain.kt +++ b/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/BasicDifferentialDrivetrain.kt @@ -3,51 +3,7 @@ package frc.chargers.hardware.subsystems.differentialdrive import edu.wpi.first.wpilibj.drive.DifferentialDrive import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.chargers.hardware.motorcontrol.EncoderMotorControllerGroup -import frc.chargers.hardware.configuration.HardwareConfigurable -import frc.chargers.hardware.configuration.HardwareConfiguration -import frc.chargers.hardware.motorcontrol.ctre.TalonSRXConfiguration -import frc.chargers.hardware.motorcontrol.rev.SparkConfiguration -/** - * A convenience function to create a [BasicDifferentialDrivetrain] - * using Spark motor controllers. - */ -public inline fun sparkDrivetrain( - leftMotors: EncoderMotorControllerGroup, - rightMotors: EncoderMotorControllerGroup, - invertMotors: Boolean = false, - configure: SparkConfiguration.() -> Unit = {} -): BasicDifferentialDrivetrain = - BasicDifferentialDrivetrain(leftMotors, rightMotors, invertMotors, SparkConfiguration().apply(configure)) - -/** - * A convenience function to create a [BasicDifferentialDrivetrain] - * using Talon SRX motor controllers. - */ -public inline fun talonSRXDrivetrain( - leftMotors: EncoderMotorControllerGroup, - rightMotors: EncoderMotorControllerGroup, - invertMotors: Boolean = false, - configure: TalonSRXConfiguration.() -> Unit = {} -): BasicDifferentialDrivetrain = - BasicDifferentialDrivetrain(leftMotors, rightMotors, invertMotors, TalonSRXConfiguration().apply(configure)) - -/** - * A convenience function to create a [BasicDifferentialDrivetrain] - * allowing its motors to all be configured. - */ -public fun BasicDifferentialDrivetrain( - leftMotors: M, - rightMotors: M, - invertMotors: Boolean = false, - configuration: C -): BasicDifferentialDrivetrain where M : MotorControllerGroup, M : HardwareConfigurable = - BasicDifferentialDrivetrain( - leftMotors = leftMotors.apply { configure(configuration) }, - rightMotors = rightMotors.apply { configure(configuration) }, - invertMotors = invertMotors - ) /** * A simple implementation of a [DifferentialDrivetrain]. @@ -77,7 +33,7 @@ public open class BasicDifferentialDrivetrain( differentialDrive.tankDrive( leftPower * powerScale, rightPower * powerScale, - false + true ) } diff --git a/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/EncoderDifferentialDrivetrain.kt b/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/EncoderDifferentialDrivetrain.kt index 83d09ea4..ebc4d036 100644 --- a/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/EncoderDifferentialDrivetrain.kt +++ b/src/main/kotlin/frc/chargers/hardware/subsystems/differentialdrive/EncoderDifferentialDrivetrain.kt @@ -9,6 +9,7 @@ import com.batterystaple.kmeasure.units.volts import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.DifferentialDriveKinematics import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds +import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim import edu.wpi.first.wpilibj2.command.SubsystemBase import frc.chargers.advantagekitextensions.LoggableInputsProvider @@ -31,14 +32,8 @@ import frc.chargers.wpilibextensions.geometry.twodimensional.UnitPose2d import frc.chargers.wpilibextensions.kinematics.ChassisSpeeds import org.littletonrobotics.junction.Logger.recordOutput -public fun simulatedDrivetrain( - simMotors: DifferentialDrivetrainSim.KitbotMotor, - constants: DiffDriveHardwareData = DiffDriveHardwareData.andyMark(), - controlScheme: DiffDriveControlData = DiffDriveControlData.None -): EncoderDifferentialDrivetrain = EncoderDifferentialDrivetrain( - DiffDriveIOSim(logInputs = LoggableInputsProvider(namespace = "Drivetrain(Differential)"),simMotors), - constants, controlScheme -) +private val standardLogInputs = LoggableInputsProvider(namespace = "Drivetrain(Differential)") + /** * A convenience function to create a [EncoderDifferentialDrivetrain] @@ -54,8 +49,13 @@ public inline fun sparkMaxDrivetrain( vararg poseSuppliers: VisionPoseSupplier, configure: SparkMaxConfiguration.() -> Unit = {} ): EncoderDifferentialDrivetrain = - EncoderDifferentialDrivetrain(leftMotors, rightMotors, constants, controlScheme, gyro, startingPose, *poseSuppliers, - configuration = SparkMaxConfiguration().apply(configure)) + EncoderDifferentialDrivetrain( + leftMotors, rightMotors, constants, controlScheme, + DifferentialDrivetrainSim.KitbotMotor.kDoubleNEOPerSide, + gyro, startingPose, *poseSuppliers, + configuration = SparkMaxConfiguration().apply(configure) + ) + /** * A convenience function to create a [EncoderDifferentialDrivetrain] @@ -71,8 +71,12 @@ public inline fun talonFXDrivetrain( vararg poseSuppliers: VisionPoseSupplier, configure: TalonFXConfiguration.() -> Unit = {} ): EncoderDifferentialDrivetrain = - EncoderDifferentialDrivetrain(leftMotors, rightMotors, constants, controlScheme, gyro, startingPose, *poseSuppliers, - configuration = TalonFXConfiguration().apply(configure)) + EncoderDifferentialDrivetrain( + leftMotors, rightMotors, constants, controlScheme, + DifferentialDrivetrainSim.KitbotMotor.kDoubleFalcon500PerSide, + gyro, startingPose, *poseSuppliers, + configuration = TalonFXConfiguration().apply(configure) + ) /** * A convenience function to create an [EncoderDifferentialDrivetrain] @@ -83,19 +87,24 @@ public fun EncoderDifferentialDrivetrain( rightMotors: EncoderMotorControllerGroup, constants: DiffDriveHardwareData = DiffDriveHardwareData.andyMark(), controlScheme: DiffDriveControlData = DiffDriveControlData.None, + simMotors: DifferentialDrivetrainSim.KitbotMotor = DifferentialDrivetrainSim.KitbotMotor.kDualCIMPerSide, gyro: HeadingProvider? = null, startingPose: UnitPose2d = UnitPose2d(), vararg poseSuppliers: VisionPoseSupplier, configuration: C -): EncoderDifferentialDrivetrain = - EncoderDifferentialDrivetrain( - lowLevel = DiffDriveIOReal( - logInputs = LoggableInputsProvider(namespace = "Drivetrain(Differential)"), - leftMotors = leftMotors.apply { configure(configuration) }, - rightMotors = rightMotors.apply { configure(configuration) } - ), - constants, controlScheme, gyro, startingPose, *poseSuppliers - ) +): EncoderDifferentialDrivetrain{ + val lowLevel = if (RobotBase.isReal()){ + DiffDriveIOReal( + standardLogInputs, + leftMotors.apply { configure(configuration) }, + rightMotors.apply { configure(configuration) } + ) + }else{ + DiffDriveIOSim(standardLogInputs, simMotors) + } + + return EncoderDifferentialDrivetrain(lowLevel, constants, controlScheme, gyro, startingPose, *poseSuppliers) +} @@ -207,6 +216,8 @@ public class EncoderDifferentialDrivetrain( ) ) + // arcade drive and curvature drive implementations + // are provided in the DifferentialDrivetrain interface override fun tankDrive(leftPower: Double, rightPower: Double) { setVoltages(leftPower * 12.volts, rightPower * 12.volts) } diff --git a/src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/TestSubystem.kt b/src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/TestSubystem.kt deleted file mode 100644 index 0f34e714..00000000 --- a/src/main/kotlin/frc/chargers/hardware/subsystems/swervedrive/TestSubystem.kt +++ /dev/null @@ -1,6 +0,0 @@ -package frc.chargers.hardware.subsystems.swervedrive - -import edu.wpi.first.wpilibj2.command.SubsystemBase - -public class TestSubystem: SubsystemBase() { -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/utils/math/equations/ejml/Multiline.kt b/src/main/kotlin/frc/chargers/utils/math/equations/ejml/Multiline.kt deleted file mode 100644 index eafbf908..00000000 --- a/src/main/kotlin/frc/chargers/utils/math/equations/ejml/Multiline.kt +++ /dev/null @@ -1,21 +0,0 @@ -package frc.chargers.utils.math.equations.ejml - -import org.ejml.equation.Equation -import org.ejml.equation.ParseError -import org.ejml.equation.Sequence - -public fun Equation.compileMultiline(equation: String, assignment: Boolean = true, debug: Boolean = false): Sequence = - equation.lineSequence() - .filterNot { it.all(Char::isWhitespace) } - .mapIndexed { i, line -> - try { - compile(line, assignment, debug) - } catch (e: Exception) { - throw ParseError("Error on line $i (\"$line\"):\n${e.message}").also { it.initCause(e) } - } - } - .reduce { a, b -> a + b } - -public fun Equation.processMultiline(equation: String, assignment: Boolean = true, debug: Boolean = false): Equation = apply { - compileMultiline(equation, assignment, debug).perform() -} \ No newline at end of file diff --git a/src/main/kotlin/frc/chargers/utils/math/equations/ejml/MultipleSequence.kt b/src/main/kotlin/frc/chargers/utils/math/equations/ejml/MultipleSequence.kt deleted file mode 100644 index f52e752e..00000000 --- a/src/main/kotlin/frc/chargers/utils/math/equations/ejml/MultipleSequence.kt +++ /dev/null @@ -1,29 +0,0 @@ -package frc.chargers.utils.math.equations.ejml - -import org.ejml.equation.Operation -import org.ejml.equation.Sequence - -public operator fun Sequence.plus(other: Sequence): Sequence { - return if (this is MultipleSequence) { - addSequence(other) - this - } else { - MultipleSequence(this, other) - } -} - -public class MultipleSequence(vararg sequences: Sequence) : Sequence() { - private val sequences = mutableListOf(Sequence() /* Ensure at least one sequence in list */, *sequences) - - public fun addSequence(sequence: Sequence) { - sequences.add(sequence) - } - - override fun addOperation(operation: Operation?) { - sequences.last().addOperation(operation) - } - - override fun perform() { - sequences.forEach { it.perform() } - } -} \ No newline at end of file diff --git a/src/test/java/frc/chargers/controls/pid/SuperPIDControllerTest.kt b/src/test/java/frc/chargers/controls/pid/SuperPIDControllerTest.kt index b40de01f..8c1de4d3 100644 --- a/src/test/java/frc/chargers/controls/pid/SuperPIDControllerTest.kt +++ b/src/test/java/frc/chargers/controls/pid/SuperPIDControllerTest.kt @@ -8,7 +8,7 @@ import edu.wpi.first.hal.HAL import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.controller.ProfiledPIDController import edu.wpi.first.wpilibj.simulation.SimHooks -import frc.chargers.controls.SetpointSupplier +import frc.chargers.controls.motionprofiling.AngularTrapezoidalSetpointSupplier import frc.chargers.utils.math.equations.epsilonEquals import frc.chargers.wpilibextensions.geometry.motion.AngularMotionConstraints import org.junit.jupiter.api.Assertions.* @@ -69,11 +69,11 @@ internal class SuperPIDControllerTest{ val wpiController = ProfiledPIDController(kP,kI,kD, constraints.siValue) wpiController.enableContinuousInput(0.0.degrees.siValue,360.degrees.siValue) - val chargerController = SuperPIDController( + val chargerController = SuperPIDController( PIDConstants(kP,kI,kD), ::getInput, target, - SetpointSupplier.AngularTrapezoidal(constraints), + AngularTrapezoidalSetpointSupplier(constraints), continuousInputRange = 0.degrees..360.degrees )