Dev/talon factory

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) {

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
+    }
+}

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<TalonSRXConfig>,
+        checkerConfig: CheckerConfig
+    ): Boolean {
+        var failure = false
+        println("////////////////////////////////////////////////")
+        println("Checking subsystem " + subsystem::class +
+                " for " + talonsToCheck.size + " talons.")
+
+        val currents = mutableListOf<Double>()
+        val rpms = mutableListOf<Double>()
+        val storedConfigurations = mutableListOf<StoredTalonSRXConfiguration>()
+
+        // 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
+    }
+}

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<Double>, value: Double, epsilon: Double): Boolean {
+        var result = false
+        for (v in list) {
+            result = result && Epsilon.epsilonEquals(v, value, epsilon)
+        }
+        return result
+    }
 }