diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt index c6562f0..5fac136 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRX.kt @@ -7,11 +7,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode class LazyTalonSRX(deviceNumber: Int) : TalonSRX(deviceNumber) { - private var lastSet = Double.NaN - private var lastSecondarySet = Double.NaN - private var lastControlMode: ControlMode? = null - private var lastDemandType: DemandType? = null - private var lastBrakeMode: NeutralMode? = null + internal var lastSet = Double.NaN + internal var lastSecondarySet = Double.NaN + internal var lastControlMode: ControlMode? = null + internal var lastDemandType: DemandType? = null + internal var lastBrakeMode: NeutralMode? = null public override fun set(mode: ControlMode, value: Double) { if (value != lastSet || mode != lastControlMode) { diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt new file mode 100644 index 0000000..4615b6a --- /dev/null +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt @@ -0,0 +1,148 @@ +package org.team5499.monkeyLib.hardware + +import com.ctre.phoenix.ParamEnum +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.NeutralMode +import com.ctre.phoenix.motorcontrol.LimitSwitchSource +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced +import com.ctre.phoenix.motorcontrol.ControlFrame +import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod + +/** + * Creates CANTalon objects and configures all the parameters we care about to factory defaults. Closed-loop and sensor + * parameters are not set, as these are expected to be set by the application. + */ +public object LazyTalonSRXFactory { + + private const val kTimeoutMs = 100 + + public class Configuration { + public var NEUTRAL_MODE = NeutralMode.Coast + // This is factory default. + public var NEUTRAL_DEADBAND = 0.04 + + public var ENABLE_CURRENT_LIMIT = false + public var ENABLE_SOFT_LIMIT = false + public var ENABLE_LIMIT_SWITCH = false + public var FORWARD_SOFT_LIMIT = 0 + public var REVERSE_SOFT_LIMIT = 0 + + public var INVERTED = false + public var SENSOR_PHASE = false + + public var CONTROL_FRAME_PERIOD_MS = 5 + public var MOTION_CONTROL_FRAME_PERIOD_MS = 100 + public var GENERAL_STATUS_FRAME_RATE_MS = 5 + public var FEEDBACK_STATUS_FRAME_RATE_MS = 100 + public var QUAD_ENCODER_STATUS_FRAME_RATE_MS = 100 + public var ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 100 + public var PULSE_WIDTH_STATUS_FRAME_RATE_MS = 100 + + public val VELOCITY_MEASUREMENT_PERIOD = VelocityMeasPeriod.Period_100Ms + public var VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW = 64 + + public var OPEN_LOOP_RAMP_RATE = 0.0 + public var CLOSED_LOOP_RAMP_RATE = 0.0 + } + + private val kDefaultConfiguration = Configuration() + private val kSlaveConfiguration = Configuration() + + init { + // This control frame value seems to need to be something reasonable to avoid the Talon's + // LEDs behaving erratically. Potentially try to increase as much as possible. + kSlaveConfiguration.CONTROL_FRAME_PERIOD_MS = 100 + kSlaveConfiguration.MOTION_CONTROL_FRAME_PERIOD_MS = 1000 + kSlaveConfiguration.GENERAL_STATUS_FRAME_RATE_MS = 1000 + kSlaveConfiguration.FEEDBACK_STATUS_FRAME_RATE_MS = 1000 + kSlaveConfiguration.QUAD_ENCODER_STATUS_FRAME_RATE_MS = 1000 + kSlaveConfiguration.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS = 1000 + kSlaveConfiguration.PULSE_WIDTH_STATUS_FRAME_RATE_MS = 1000 + } + + // Create a CANTalon with the default (out of the box) configuration. + public fun createDefaultTalon(id: Int): LazyTalonSRX { + return createTalon(id, kDefaultConfiguration) + } + + public fun createPermanentSlaveTalon(id: Int, masterId: Int): LazyTalonSRX { + val talon = createTalon(id, kSlaveConfiguration) + talon.set(ControlMode.Follower, masterId.toDouble()) + return talon + } + + @Suppress("LongMethod") + public fun createTalon(id: Int, config: Configuration): LazyTalonSRX { + val talon = LazyTalonSRX(id) + talon.set(ControlMode.PercentOutput, 0.0) + + talon.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS) + talon.clearMotionProfileHasUnderrun(kTimeoutMs) + talon.clearMotionProfileTrajectories() + + talon.clearStickyFaults(kTimeoutMs) + + talon.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, + LimitSwitchNormal.NormallyOpen, kTimeoutMs) + talon.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, + LimitSwitchNormal.NormallyOpen, kTimeoutMs) + talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH) + + // Turn off re-zeroing by default. + talon.configSetParameter( + ParamEnum.eClearPositionOnLimitF, 0.0, 0, 0, kTimeoutMs) + talon.configSetParameter( + ParamEnum.eClearPositionOnLimitR, 0.0, 0, 0, kTimeoutMs) + + talon.configNominalOutputForward(0.0, kTimeoutMs) + talon.configNominalOutputReverse(0.0, kTimeoutMs) + talon.configNeutralDeadband(config.NEUTRAL_DEADBAND, kTimeoutMs) + + talon.configPeakOutputForward(1.0, kTimeoutMs) + talon.configPeakOutputReverse(-1.0, kTimeoutMs) + + talon.setNeutralMode(config.NEUTRAL_MODE) + + talon.configForwardSoftLimitThreshold(config.FORWARD_SOFT_LIMIT, kTimeoutMs) + talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs) + + talon.configReverseSoftLimitThreshold(config.REVERSE_SOFT_LIMIT, kTimeoutMs) + talon.configReverseSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs) + talon.overrideSoftLimitsEnable(config.ENABLE_SOFT_LIMIT) + + talon.setInverted(config.INVERTED) + talon.setSensorPhase(config.SENSOR_PHASE) + + talon.selectProfileSlot(0, 0) + + talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, kTimeoutMs) + talon.configVelocityMeasurementWindow(config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, + kTimeoutMs) + + talon.configOpenloopRamp(config.OPEN_LOOP_RAMP_RATE, kTimeoutMs) + talon.configClosedloopRamp(config.CLOSED_LOOP_RAMP_RATE, kTimeoutMs) + + talon.configVoltageCompSaturation(0.0, kTimeoutMs) + talon.configVoltageMeasurementFilter(32, kTimeoutMs) + talon.enableVoltageCompensation(false) + + talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT) + + talon.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, + config.GENERAL_STATUS_FRAME_RATE_MS, kTimeoutMs) + talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, + config.FEEDBACK_STATUS_FRAME_RATE_MS, kTimeoutMs) + + talon.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, + config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, kTimeoutMs) + talon.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, + config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, kTimeoutMs) + talon.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, + config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, kTimeoutMs) + + talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS) + + return talon + } +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/TalonSRXChecker.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/TalonSRXChecker.kt new file mode 100644 index 0000000..79da0ba --- /dev/null +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/TalonSRXChecker.kt @@ -0,0 +1,126 @@ +package org.team5499.monkeyLib.hardware + +import com.ctre.phoenix.motorcontrol.ControlMode +import org.team5499.monkeyLib.Subsystem +import org.team5499.monkeyLib.util.Utils +import edu.wpi.first.wpilibj.Timer + +import java.util.function.DoubleSupplier + +public object TalonSRXChecker { + public class CheckerConfig { + public var mCurrentFloor = 5 + public var mRPMFloor = 2000 + + public var mCurrentEpsilon = 5.0 + public var mRPMEpsilon = 500.0 + public var mRPMSupplier: DoubleSupplier? = null + + public var mRunTimeSec = 4.0 + public var mWaitTimeSec = 2.0 + public var mRunOutputPercentage = 0.5 + } + + public class TalonSRXConfig(var name: String, var talon: LazyTalonSRX) + + private class StoredTalonSRXConfiguration(var mode: ControlMode, var setValue: Double) + + @Suppress("ComplexMethod") + public fun checkTalons( + subsystem: Subsystem, + talonsToCheck: MutableList, + checkerConfig: CheckerConfig + ): Boolean { + var failure = false + println("////////////////////////////////////////////////") + println("Checking subsystem " + subsystem::class + + " for " + talonsToCheck.size + " talons.") + + val currents = mutableListOf() + val rpms = mutableListOf() + val storedConfigurations = mutableListOf() + + // Record previous configuration for all talons. + for (config in talonsToCheck) { + var talon = LazyTalonSRX::class.java.cast(config.talon) + var configuration = StoredTalonSRXConfiguration(talon.controlMode, talon.lastSet) + + storedConfigurations.add(configuration) + + // Now set to disabled. + talon.set(ControlMode.PercentOutput, 0.0) + } + + for (config: TalonSRXConfig in talonsToCheck) { + println("Checking: " + config.name) + + config.talon.set(ControlMode.PercentOutput, checkerConfig.mRunOutputPercentage) + Timer.delay(checkerConfig.mRunTimeSec) + + // Now poll the interesting information. + var current = config.talon.getOutputCurrent() + currents.add(current) + print("Current: " + current) + + var rpm = Double.NaN + if (checkerConfig.mRPMSupplier != null) { + rpm = checkerConfig.mRPMSupplier!!.getAsDouble() + rpms.add(rpm) + print(" RPM: " + rpm) + } + print('\n') + + config.talon.set(ControlMode.PercentOutput, 0.0) + + // And perform checks. + if (current < checkerConfig.mCurrentFloor) { + println(config.name + " has failed current floor check vs " + + checkerConfig.mCurrentFloor + "!!") + failure = true + } + if (checkerConfig.mRPMSupplier != null) { + if (rpm < checkerConfig.mRPMFloor) { + println(config.name + " has failed rpm floor check vs " + + checkerConfig.mRPMFloor + "!!") + failure = true + } + } + + Timer.delay(checkerConfig.mWaitTimeSec) + } + + // Now run aggregate checks. + + if (currents.size > 0) { + var average = 0.0 + for (current in currents) { + average = current + average + } + average = average / currents.size.toDouble() + if (!Utils.allCloseTo(currents, average, checkerConfig.mCurrentEpsilon)) { + println("Currents varied!!!!!!!!!!!") + failure = true + } + } + + if (rpms.size > 0) { + var average = 0.0 + for (rpm in rpms) { + average = rpm + average + } + average = average / rpms.size.toDouble() + if (!Utils.allCloseTo(rpms, average, checkerConfig.mRPMEpsilon)) { + println("RPMs varied!!!!!!!!") + failure = true + } + } + + // Restore Talon configurations + for (i in 0..(talonsToCheck.size - 1)) { + talonsToCheck.get(i).talon.set(storedConfigurations.get(i).mode, + storedConfigurations.get(i).setValue) + } + + return !failure + } +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt b/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt index f522bcb..1ca0ebb 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt @@ -1,5 +1,7 @@ package org.team5499.monkeyLib.util +import org.team5499.monkeyLib.math.Epsilon + @SuppressWarnings("MagicNumber") object Utils { @@ -104,4 +106,12 @@ object Utils { public fun degreesToTalonAngle(gyroTicksPerRotation: Int, degrees: Double): Int { return ((gyroTicksPerRotation / 360.0) * degrees).toInt() } + + public fun allCloseTo(list: MutableList, value: Double, epsilon: Double): Boolean { + var result = false + for (v in list) { + result = result && Epsilon.epsilonEquals(v, value, epsilon) + } + return result + } }