Add velocity measurement configuration

Author: jhh

build.gradle

@@ -4,7 +4,7 @@ plugins {
     id "idea"
 }
 
-version = "19.0.0"
+version = "19.1.0"
 
 repositories {
     jcenter()

src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt

@@ -41,7 +41,7 @@ interface Command {
                 "menu" -> createMenuCommand(parent, key, toml)
                 "talon.select" -> SelectTalonsCommand(parent, key, toml)
                 "talon.mode" -> SelectControlModeCommand(parent, key, toml)
-                "talon.brake" -> SelectBrakeModeCommand(parent, key, toml)
+                "talon.brake" -> SelectVelocityMeasurmentPeriodCommand(parent, key, toml)
                 "talon.run" -> RunTalonsCommand(parent, key, toml)
                 "talon.status" -> TalonsStatusCommand(parent, key, toml)
                 "talon.slot.select" -> SelectSlotCommand(parent, key, toml)
@@ -51,6 +51,7 @@ interface Command {
                 "talon.sensor.position" -> SetSensorPositionCommand(parent, key, toml)
                 "talon.hard.source" -> SelectHardLimitSourceCommand(parent, key, toml)
                 "talon.hard.normal" -> SelectHardLimitNormalCommand(parent, key, toml)
+                "talon.velocity.period" -> SelectVelocityMeasurmentPeriodCommand(parent, key, toml)
                 "servo.select" -> SelectServosCommand(parent, key, toml)
                 "servo.run" -> RunServosCommand(parent, key, toml)
                 "digital_output.select" -> SelectDigitalOutputsCommand(parent, key, toml)

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

@@ -0,0 +1,31 @@
+package org.strykeforce.thirdcoast.talon
+
+import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod
+import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod.*
+import net.consensys.cava.toml.TomlTable
+import org.koin.standalone.inject
+import org.strykeforce.thirdcoast.command.AbstractSelectCommand
+import org.strykeforce.thirdcoast.command.Command
+import org.strykeforce.thirdcoast.device.TalonService
+
+class SelectVelocityMeasurmentPeriodCommand(
+    parent: Command?,
+    key: String,
+    toml: TomlTable
+) : AbstractSelectCommand<VelocityMeasPeriod>(
+    parent,
+    key,
+    toml,
+    listOf(Period_1Ms, Period_2Ms, Period_5Ms, Period_10Ms, Period_20Ms, Period_50Ms, Period_100Ms),
+    listOf("1 ms", "2 ms", "5 ms", "10 ms", "20 ms", "50 ms", "100 ms")
+) {
+    private val talonService: TalonService by inject()
+    override val activeIndex
+        get() = values.indexOf(talonService.activeConfiguration.velocityMeasurementPeriod)
+
+    override fun setActive(index: Int) {
+        val period = values[index]
+        talonService.active.forEach { it.configVelocityMeasurementPeriod(period, talonService.timeout) }
+        talonService.activeConfiguration.velocityMeasurementPeriod = period
+    }
+}

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

@@ -47,6 +47,7 @@ class TalonParameter(command: Command, toml: TomlTable, val enum: Enum) : Abstra
         SOFT_LIMIT_ENABLE_REVERSE,
         SOFT_LIMIT_THRESHOLD_FORWARD,
         SOFT_LIMIT_THRESHOLD_REVERSE,
+        VELOCITY_MEASUREMENT_WINDOW,
         FACTORY_DEFAULTS,
     }
 

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

@@ -65,6 +65,7 @@ class TalonParameterCommand(
                 SOFT_LIMIT_ENABLE_REVERSE -> formatMenu(config.reverseSoftLimitEnable)
                 SOFT_LIMIT_THRESHOLD_FORWARD -> formatMenu(config.forwardSoftLimitThreshold)
                 SOFT_LIMIT_THRESHOLD_REVERSE -> formatMenu(config.reverseSoftLimitThreshold)
+                VELOCITY_MEASUREMENT_WINDOW -> formatMenu(config.velocityMeasurementWindow)
                 FACTORY_DEFAULTS -> tomlMenu
                 else -> TODO("${param.enum} not implemented")
             }
@@ -215,6 +216,10 @@ class TalonParameterCommand(
                 talon.configReverseSoftLimitThreshold(value, timeout)
                 config.reverseSoftLimitThreshold = value
             }
+            VELOCITY_MEASUREMENT_WINDOW -> configIntParam(config.velocityMeasurementWindow) { talon, value ->
+                talon.configVelocityMeasurementWindow(value, timeout)
+                talonService.dirty = true // let the talon round down the number to a legal value
+            }
             FACTORY_DEFAULTS -> configBooleanParam(false) { talon, value ->
                 if (value) talon.configFactoryDefault(timeout)
             }

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

@@ -197,6 +197,19 @@ type = "menu"
       order = 90
       menu = "neutral deadband"
       param = "NEUTRAL_DEADBAND"
+  [talon.velocity_measurement]
+    type = "menu"
+    order = 26
+    menu = "configure velocity measurement"
+    [talon.velocity_measurement.period]
+      type = "talon.velocity.period"
+      order = 10
+      menu = "velocity measurement period"
+    [talon.velocity_measurement.window]
+      type = "talon.param"
+      order = 20
+      menu = "velocity measurement window"
+      param = "VELOCITY_MEASUREMENT_WINDOW"
   [talon.voltage_compensation]
     type = "menu"
     order = 27

src/main/resources/commands.toml

@@ -90,6 +90,15 @@
   type = "INTEGER"
   help = "Number of samples in the rolling average of voltage measurement."
 
+#
+# velocity measurement
+#
+[VELOCITY_MEASUREMENT_WINDOW]
+  name = "Velocity Measurement Window"
+  type = "INTEGER"
+  range = [1.0, 32.0]
+  help = "Sets the number of velocity samples used in the rolling average velocity measurement."
+
 #
 # PID slots
 #