From 0e299783dfc0e1c22b4b85ab45c9f2503e2250e6 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Fri, 1 Mar 2019 13:39:22 -0800 Subject: [PATCH 1/5] started, making a freshy do this --- .../team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt 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..e00ff0f --- /dev/null +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt @@ -0,0 +1,10 @@ +package org.team5499.monkeyLib.hardware + +public object LazyTalonSRXFactory { + + public class TalonConfig + + public fun createTalon(port: Int, config: TalonConfig): LazyTalonSRX { + return LazyTalonSRX(port) + } +} From 4191e02fa34f434dd782089caa2bb85c03a93cd8 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Wed, 20 Mar 2019 16:37:06 -0700 Subject: [PATCH 2/5] commit --- .../monkeyLib/hardware/LazyTalonSRXFactory.kt | 146 +++++++++++++++++- 1 file changed, 145 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt index e00ff0f..eb406b9 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt @@ -1,4 +1,4 @@ -package org.team5499.monkeyLib.hardware +/**package org.team5499.monkeyLib.hardware public object LazyTalonSRXFactory { @@ -7,4 +7,148 @@ public object LazyTalonSRXFactory { public fun createTalon(port: Int, config: TalonConfig): LazyTalonSRX { return LazyTalonSRX(port) } +}*/ +package org.team5499.monkeyLib.hardware + +import com.ctre.phoenix.ParamEnum; +import com.ctre.phoenix.motorcontrol.* +import com.ctre.phoenix.motorcontrol.can.TalonSRX +import org.team5499.monkeyLib.hardware.LazyTalonSRX + + +/** + * 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 NeutralMode 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, master_id : Int) : LazyTalonSRX { + val talon = createTalon(id, kSlaveConfiguration) + talon.set(ControlMode.Follower, master_id.toDouble()) + return talon + } + + 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, kTimeoutMs) + talon.configSetParameter( + ParamEnum.eClearPositionOnLimitR, 0, 0, 0, kTimeoutMs) + + talon.configNominalOutputForward(0, kTimeoutMs) + talon.configNominalOutputReverse(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 + } } From 89db0a8cf5a308948faf4fa142f660af4e485850 Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Wed, 20 Mar 2019 16:59:22 -0700 Subject: [PATCH 3/5] factory --- .../team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt index eb406b9..70b1f15 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt @@ -25,7 +25,7 @@ public object LazyTalonSRXFactory { private const val kTimeoutMs = 100 public class Configuration { - public NeutralMode NEUTRAL_MODE = NeutralMode.Coast + public var NEUTRAL_MODE = NeutralMode.Coast // This is factory default. public var NEUTRAL_DEADBAND = 0.04 @@ -97,12 +97,12 @@ public object LazyTalonSRXFactory { // Turn off re-zeroing by default. talon.configSetParameter( - ParamEnum.eClearPositionOnLimitF, 0, 0, 0, kTimeoutMs) + ParamEnum.eClearPositionOnLimitF, 0.0, 0, 0, kTimeoutMs) talon.configSetParameter( - ParamEnum.eClearPositionOnLimitR, 0, 0, 0, kTimeoutMs) + ParamEnum.eClearPositionOnLimitR, 0.0, 0, 0, kTimeoutMs) - talon.configNominalOutputForward(0, kTimeoutMs) - talon.configNominalOutputReverse(0, kTimeoutMs) + talon.configNominalOutputForward(0.0, kTimeoutMs) + talon.configNominalOutputReverse(0.0, kTimeoutMs) talon.configNeutralDeadband(config.NEUTRAL_DEADBAND, kTimeoutMs) talon.configPeakOutputForward(1.0, kTimeoutMs) From f95d74117b1882b04c82c1a7bf55403c766fc43f Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Thu, 21 Mar 2019 17:09:59 -0700 Subject: [PATCH 4/5] yeet --- .../monkeyLib/hardware/LazyTalonSRX.kt | 10 +- .../monkeyLib/hardware/LazyTalonSRXChecker.kt | 125 ++++++++++++++++++ .../org/team5499/monkeyLib/util/Utils.kt | 11 ++ 3 files changed, 141 insertions(+), 5 deletions(-) create mode 100644 src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt 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/LazyTalonSRXChecker.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt new file mode 100644 index 0000000..dcd9f0b --- /dev/null +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt @@ -0,0 +1,125 @@ +package org.team5499.monkeyLib.hardware + +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.can.TalonSRX +import org.team5499.monkeyLib.Subsystem +import org.team5499.monkeyLib.util.Utils +import edu.wpi.first.wpilibj.Timer + +import java.util.ArrayList +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) + + 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..9de676f 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt @@ -1,5 +1,8 @@ package org.team5499.monkeyLib.util + +import org.team5499.monkeyLib.math.Epsilon + @SuppressWarnings("MagicNumber") object Utils { @@ -104,4 +107,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 + } } From 937b72e21ac0c4fd0d024307eae779ea6f36742b Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Sun, 24 Mar 2019 22:55:19 -0700 Subject: [PATCH 5/5] fixes --- .../monkeyLib/hardware/LazyTalonSRXChecker.kt | 125 ------------ .../monkeyLib/hardware/LazyTalonSRXFactory.kt | 190 +++++++++--------- .../monkeyLib/hardware/TalonSRXChecker.kt | 126 ++++++++++++ .../org/team5499/monkeyLib/util/Utils.kt | 3 +- 4 files changed, 219 insertions(+), 225 deletions(-) delete mode 100644 src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt create mode 100644 src/main/kotlin/org/team5499/monkeyLib/hardware/TalonSRXChecker.kt diff --git a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt deleted file mode 100644 index dcd9f0b..0000000 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXChecker.kt +++ /dev/null @@ -1,125 +0,0 @@ -package org.team5499.monkeyLib.hardware - -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.can.TalonSRX -import org.team5499.monkeyLib.Subsystem -import org.team5499.monkeyLib.util.Utils -import edu.wpi.first.wpilibj.Timer - -import java.util.ArrayList -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) - - 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/hardware/LazyTalonSRXFactory.kt b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt index 70b1f15..4615b6a 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/hardware/LazyTalonSRXFactory.kt @@ -1,154 +1,148 @@ -/**package org.team5499.monkeyLib.hardware - -public object LazyTalonSRXFactory { - - public class TalonConfig - - public fun createTalon(port: Int, config: TalonConfig): LazyTalonSRX { - return LazyTalonSRX(port) - } -}*/ package org.team5499.monkeyLib.hardware -import com.ctre.phoenix.ParamEnum; -import com.ctre.phoenix.motorcontrol.* -import com.ctre.phoenix.motorcontrol.can.TalonSRX -import org.team5499.monkeyLib.hardware.LazyTalonSRX - +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 { +public object LazyTalonSRXFactory { - private const val kTimeoutMs = 100 + private const val kTimeoutMs = 100 public class Configuration { - public var NEUTRAL_MODE = NeutralMode.Coast + 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 + 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() + 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 + 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 createDefaultTalon(id: Int): LazyTalonSRX { + return createTalon(id, kDefaultConfiguration) } - public fun createPermanentSlaveTalon(id : Int, master_id : Int) : LazyTalonSRX { - val talon = createTalon(id, kSlaveConfiguration) - talon.set(ControlMode.Follower, master_id.toDouble()) - return talon + public fun createPermanentSlaveTalon(id: Int, masterId: Int): LazyTalonSRX { + val talon = createTalon(id, kSlaveConfiguration) + talon.set(ControlMode.Follower, masterId.toDouble()) + return talon } - public fun createTalon(id : Int, config : Configuration) : LazyTalonSRX { - val talon = LazyTalonSRX(id) - talon.set(ControlMode.PercentOutput, 0.0) + @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.changeMotionControlFramePeriod(config.MOTION_CONTROL_FRAME_PERIOD_MS) + talon.clearMotionProfileHasUnderrun(kTimeoutMs) + talon.clearMotionProfileTrajectories() - talon.clearStickyFaults(kTimeoutMs) + talon.clearStickyFaults(kTimeoutMs) talon.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, - LimitSwitchNormal.NormallyOpen, kTimeoutMs) + LimitSwitchNormal.NormallyOpen, kTimeoutMs) talon.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, - LimitSwitchNormal.NormallyOpen, kTimeoutMs) - talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH) + LimitSwitchNormal.NormallyOpen, kTimeoutMs) + talon.overrideLimitSwitchesEnable(config.ENABLE_LIMIT_SWITCH) // Turn off re-zeroing by default. talon.configSetParameter( - ParamEnum.eClearPositionOnLimitF, 0.0, 0, 0, kTimeoutMs) + ParamEnum.eClearPositionOnLimitF, 0.0, 0, 0, kTimeoutMs) talon.configSetParameter( - ParamEnum.eClearPositionOnLimitR, 0.0, 0, 0, kTimeoutMs) + ParamEnum.eClearPositionOnLimitR, 0.0, 0, 0, kTimeoutMs) - talon.configNominalOutputForward(0.0, kTimeoutMs) - talon.configNominalOutputReverse(0.0, kTimeoutMs) - talon.configNeutralDeadband(config.NEUTRAL_DEADBAND, 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.configPeakOutputForward(1.0, kTimeoutMs) + talon.configPeakOutputReverse(-1.0, kTimeoutMs) - talon.setNeutralMode(config.NEUTRAL_MODE) + talon.setNeutralMode(config.NEUTRAL_MODE) - talon.configForwardSoftLimitThreshold(config.FORWARD_SOFT_LIMIT, kTimeoutMs) - talon.configForwardSoftLimitEnable(config.ENABLE_SOFT_LIMIT, kTimeoutMs) + 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.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.setInverted(config.INVERTED) + talon.setSensorPhase(config.SENSOR_PHASE) - talon.selectProfileSlot(0, 0) + talon.selectProfileSlot(0, 0) - talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, kTimeoutMs) + talon.configVelocityMeasurementPeriod(config.VELOCITY_MEASUREMENT_PERIOD, kTimeoutMs) talon.configVelocityMeasurementWindow(config.VELOCITY_MEASUREMENT_ROLLING_AVERAGE_WINDOW, - kTimeoutMs) + kTimeoutMs) - talon.configOpenloopRamp(config.OPEN_LOOP_RAMP_RATE, kTimeoutMs) - talon.configClosedloopRamp(config.CLOSED_LOOP_RAMP_RATE, 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.configVoltageCompSaturation(0.0, kTimeoutMs) + talon.configVoltageMeasurementFilter(32, kTimeoutMs) + talon.enableVoltageCompensation(false) - talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT) + talon.enableCurrentLimit(config.ENABLE_CURRENT_LIMIT) talon.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, - config.GENERAL_STATUS_FRAME_RATE_MS, kTimeoutMs) + config.GENERAL_STATUS_FRAME_RATE_MS, kTimeoutMs) talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, - config.FEEDBACK_STATUS_FRAME_RATE_MS, kTimeoutMs) + config.FEEDBACK_STATUS_FRAME_RATE_MS, kTimeoutMs) talon.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, - config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, kTimeoutMs) + config.QUAD_ENCODER_STATUS_FRAME_RATE_MS, kTimeoutMs) talon.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, - config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, kTimeoutMs) + config.ANALOG_TEMP_VBAT_STATUS_FRAME_RATE_MS, kTimeoutMs) talon.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, - config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, kTimeoutMs) + config.PULSE_WIDTH_STATUS_FRAME_RATE_MS, kTimeoutMs) - talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS) + talon.setControlFramePeriod(ControlFrame.Control_3_General, config.CONTROL_FRAME_PERIOD_MS) - return talon + 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 9de676f..1ca0ebb 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/util/Utils.kt @@ -1,6 +1,5 @@ package org.team5499.monkeyLib.util - import org.team5499.monkeyLib.math.Epsilon @SuppressWarnings("MagicNumber") @@ -110,7 +109,7 @@ object Utils { public fun allCloseTo(list: MutableList, value: Double, epsilon: Double): Boolean { var result = false - for(v in list) { + for (v in list) { result = result && Epsilon.epsilonEquals(v, value, epsilon) } return result