Manage talon configuration in TalonService

Author: jhh

build.gradle

@@ -47,7 +47,7 @@ dependencies {
     testCompile "org.junit.jupiter:junit-jupiter-params:${junit_version}"
     testRuntime "org.junit.jupiter:junit-jupiter-engine:${junit_version}"
     testCompile "org.assertj:assertj-core:3.11.1"
-    testCompile 'com.nhaarman.mockitokotlin2:mockito-kotlin:2.0.0'
+    testCompile "com.nhaarman.mockitokotlin2:mockito-kotlin:2.1.0"
 
 }
 

gradle.properties

@@ -1,5 +1,5 @@
 kotlin.code.style=official
 kotlin_version = 1.3.11
 junit_version = 5.2.0
-koin_version = 1.0.1
-thirdcoast_version = 19.0.0
+koin_version = 1.0.2
+thirdcoast_version = 19.1.0

src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt

@@ -2,50 +2,85 @@ package org.strykeforce.thirdcoast.device
 
 import com.ctre.phoenix.motorcontrol.ControlMode
 import com.ctre.phoenix.motorcontrol.NeutralMode
+import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
 import mu.KotlinLogging
-import org.strykeforce.thirdcoast.talon.TalonParameterCommand
 import org.strykeforce.thirdcoast.talon.SelectFeedbackSensorCommand
+import org.strykeforce.thirdcoast.talon.TalonParameterCommand
 import org.strykeforce.thirdcoast.telemetry.TelemetryService
 
 private val logger = KotlinLogging.logger {}
 
+private val CONTROL_MODE_DEFAULT = ControlMode.PercentOutput
 private const val ACTIVE_SLOT_DEFAULT = 0
-private const val VOLTAGE_COMPENSATION_DEFAULT = true
-private const val SENSOR_PHASE_DEFAULT = false
-private const val CURRENT_LIMIT_DEFAULT = false
-private const val INVERTED_DEFAULT = false
+private val NEUTRAL_MODE_DEFAULT = NeutralMode.EEPROMSetting
+private const val VOLTAGE_COMPENSATION_ENABLED_DEFAULT = true
+private const val CURRENT_LIMIT_ENABLED_DEFAULT = false
+private const val SENSOR_PHASE_INVERTED_DEFAULT = false
+private const val OUTPUT_INVERTED_DEFAULT = false
 
+/**
+ * Holds the active state for all Talons that have been activated by the user. Talons that are currently active
+ * can be configured and ran. All Talons that have been made active will be available to the telemetry client (Grapher),
+ * for the remainder of the `tct` session, even if not currently active.
+ *
+ * Persistent configuration settings will be refreshed from the Talon when the `TalonService` is marked as *dirty*. This
+ * prevents repeated, time-consuming Phoenix API calls to read Talon state.
+ *
+ * For non-persistent settings, Talons will be set to a known state when made active. These include:
+ * - control mode = PercentOutput
+ * - active slot index = 0
+ * - brake in neutral mode = EEPROM setting
+ * - voltage compensation = enabled
+ * - current limit = enabled
+ * - sensor phase  = not inverted
+ * - output = not inverted
+ *
+ *
+ * @param telemetryService the telemetry service.
+ * @param factory a factory lambda expression to instantiate Talons.
+ */
 class TalonService(private val telemetryService: TelemetryService, factory: (id: Int) -> TalonSRX) :
     AbstractDeviceService<TalonSRX>(factory) {
 
     val timeout = 10
+    var dirty = true
+    var controlMode = CONTROL_MODE_DEFAULT
     var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT
-    var controlMode = ControlMode.PercentOutput
-    var neutralMode = NeutralMode.EEPROMSetting
-    var voltageCompensation = VOLTAGE_COMPENSATION_DEFAULT
-    var sensorPhase = SENSOR_PHASE_DEFAULT
-    var currentLimit = CURRENT_LIMIT_DEFAULT
+    var neutralMode = NEUTRAL_MODE_DEFAULT
+    var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
+    var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
+    var currentLimit = CURRENT_LIMIT_ENABLED_DEFAULT
 
     var activeConfiguration = TalonSRXConfiguration()
         get() {
+            if (!dirty) return field
             active.firstOrNull()?.getAllConfigs(field) ?: logger.debug("no active talons, returning default config")
+            dirty = false
             return field
         }
 
-    val outputReverse: Boolean
-        get() = active.firstOrNull()?.inverted ?: INVERTED_DEFAULT
+    val activeSlot: SlotConfiguration
+        get() = when (activeSlotIndex) {
+            0 -> activeConfiguration.slot0
+            1 -> activeConfiguration.slot1
+            2 -> activeConfiguration.slot2
+            3 -> activeConfiguration.slot3
+            else -> throw IllegalStateException("invalid slot: $activeSlotIndex")
+        }
+
+    val outputInverted: Boolean
+        get() = active.firstOrNull()?.inverted ?: OUTPUT_INVERTED_DEFAULT
 
     override fun activate(ids: Collection<Int>) {
-        TalonParameterCommand.reset = true
-        SelectFeedbackSensorCommand.reset = true
+        dirty = true
         activeSlotIndex = ACTIVE_SLOT_DEFAULT
-        controlMode = ControlMode.PercentOutput
-        neutralMode = NeutralMode.EEPROMSetting
-        voltageCompensation = VOLTAGE_COMPENSATION_DEFAULT
-        sensorPhase = SENSOR_PHASE_DEFAULT
-        currentLimit = CURRENT_LIMIT_DEFAULT
+        controlMode = CONTROL_MODE_DEFAULT
+        neutralMode = NEUTRAL_MODE_DEFAULT
+        voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
+        sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
+        currentLimit = CURRENT_LIMIT_ENABLED_DEFAULT
 
         super.activate(ids)
         telemetryService.stop()
@@ -54,7 +89,7 @@ class TalonService(private val telemetryService: TelemetryService, factory: (id:
             it.selectProfileSlot(activeSlotIndex, 0)
             it.enableVoltageCompensation(voltageCompensation)
             it.setSensorPhase(sensorPhase)
-            it.inverted = INVERTED_DEFAULT
+            it.inverted = OUTPUT_INVERTED_DEFAULT
             it.enableCurrentLimit(currentLimit)
             telemetryService.register(it)
         }

src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectFeedbackSensorCommand.kt

@@ -37,22 +37,13 @@ class SelectFeedbackSensorCommand(
     toml: TomlTable
 ) : AbstractSelectCommand<FeedbackDevice>(parent, key, toml, SENSORS, LABELS) {
 
-    companion object {
-        var reset = true
-    }
-
     private val talonService: TalonService by inject()
 
     private val pidIndex = toml.getLong("pid")?.toInt() ?: 0
 
-    private var config = talonService.activeConfiguration
-
     override val activeIndex: Int
         get() {
-            if (reset) {
-                config = talonService.activeConfiguration
-                reset = false
-            }
+            val config = talonService.activeConfiguration
 
             val sensor = when (pidIndex) {
                 0 -> config.primaryPID.selectedFeedbackSensor
@@ -69,7 +60,12 @@ class SelectFeedbackSensorCommand(
         }
 
     override fun setActive(index: Int) {
-        talonService.active.forEach { it.configSelectedFeedbackSensor(values[index], pidIndex, talonService.timeout) }
-        reset = true
+        val sensor = values[index]
+        talonService.active.forEach { it.configSelectedFeedbackSensor(sensor, pidIndex, talonService.timeout) }
+        when (pidIndex) {
+            0 -> talonService.activeConfiguration.primaryPID.selectedFeedbackSensor = sensor
+            else -> talonService.activeConfiguration.auxiliaryPID.selectedFeedbackSensor = sensor
+        }
+
     }
 }

src/main/kotlin/org/strykeforce/thirdcoast/talon/SelectSlotCommand.kt

@@ -21,6 +21,5 @@ class SelectSlotCommand(
 
     override fun setActive(index: Int) {
         talonService.activeSlotIndex = index
-        TalonParameterCommand.reset = true
     }
 }

src/main/kotlin/org/strykeforce/thirdcoast/talon/TalonParameterCommand.kt

@@ -2,9 +2,7 @@ package org.strykeforce.thirdcoast.talon
 
 import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
 import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced.*
-import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
-import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
 import mu.KotlinLogging
 import net.consensys.cava.toml.TomlTable
 import org.koin.standalone.inject
@@ -21,19 +19,14 @@ class TalonParameterCommand(
     toml: TomlTable
 ) : AbstractCommand(parent, key, toml) {
 
-    companion object {
-        var reset = true
-        private var slot: SlotConfiguration = SlotConfiguration()
-        private var config = TalonSRXConfiguration()
-    }
-
     private val talonService: TalonService by inject()
     private val timeout = talonService.timeout
     private val param = TalonParameter.create(this, toml.getString("param") ?: "UNKNOWN")
 
     override val menu: String
         get() {
-            resetConfigIfNeeded()
+            val config = talonService.activeConfiguration
+            val slot = talonService.activeSlot
             return when (param.enum) {
                 SLOT_P -> formatMenu(slot.kP)
                 SLOT_I -> formatMenu(slot.kI)
@@ -43,7 +36,7 @@ class TalonParameterCommand(
                 SLOT_ALLOWABLE_ERR -> formatMenu(slot.allowableClosedloopError)
                 SLOT_MAX_I_ACCUM -> formatMenu(slot.maxIntegralAccumulator)
                 SLOT_PEAK_OUTPUT -> formatMenu(slot.closedLoopPeakOutput)
-                OUTPUT_REVERSED -> formatMenu(talonService.outputReverse)
+                OUTPUT_REVERSED -> formatMenu(talonService.outputInverted)
                 OPEN_LOOP_RAMP -> formatMenu(config.openloopRamp)
                 CLOSED_LOOP_RAMP -> formatMenu(config.closedloopRamp)
                 PEAK_OUTPUT_FORWARD -> formatMenu(config.peakOutputForward)
@@ -78,7 +71,9 @@ class TalonParameterCommand(
         }
 
     override fun execute(): Command {
-        resetConfigIfNeeded()
+        val config = talonService.activeConfiguration
+        val slot = talonService.activeSlot
+
         when (param.enum) {
             SLOT_P -> configDoubleParam(slot.kP) { talon, value ->
                 talon.config_kP(talonService.activeSlotIndex, value, timeout)
@@ -112,7 +107,7 @@ class TalonParameterCommand(
                 talon.configClosedLoopPeakOutput(talonService.activeSlotIndex, value, timeout)
                 slot.closedLoopPeakOutput = value
             }
-            OUTPUT_REVERSED -> configBooleanParam(talonService.outputReverse) { talon, value ->
+            OUTPUT_REVERSED -> configBooleanParam(talonService.outputInverted) { talon, value ->
                 talon.inverted = value
             }
             OPEN_LOOP_RAMP -> configDoubleParam(config.openloopRamp) { talon, value ->
@@ -250,21 +245,5 @@ class TalonParameterCommand(
     private fun defaultFor(frame: StatusFrameEnhanced): Int =
         talonService.active.first().getStatusFramePeriod(frame)
 
-    private fun resetConfigIfNeeded() {
-        if (reset) {
-            config = talonService.activeConfiguration
-            slot = when (talonService.activeSlotIndex) {
-                0 -> config.slot0
-                1 -> config.slot1
-                2 -> config.slot2
-                3 -> config.slot3
-                else -> throw IllegalStateException()
-            }
-            reset = false
-            logger.debug { "reset active config to:\n${config.toString("active")}" }
-            logger.debug { "reset active slot to: \n${slot.toString("active")}" }
-        }
-    }
-
 }