WIP - before re-configure to re-use commands for FX/SRX

Author: mwitcpalek

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

@@ -6,7 +6,6 @@ import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
 import com.ctre.phoenix.motorcontrol.can.TalonFX
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration
 import mu.KotlinLogging
-import org.strykeforce.thirdcoast.talon.TalonFXItem
 import org.strykeforce.thirdcoast.telemetry.TelemetryService
 import java.lang.IllegalStateException
 import kotlin.math.log
@@ -64,7 +63,7 @@ class TalonFXService(private val telemetryService: TelemetryService, factory: (i
             it.enableVoltageCompensation(voltageCompensation)
             it.setSensorPhase(sensorPhase)
             it.setInverted(OUTPUT_INVERTED_DEFAULT)
-            telemetryService.register(TalonFXItem(it))
+            telemetryService.register(it)
         }
         telemetryService.start()
         return new

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

@@ -0,0 +1,63 @@
+package org.strykeforce.thirdcoast.talon
+
+import com.ctre.phoenix.motorcontrol.ControlMode
+import edu.wpi.first.wpilibj.Timer
+import mu.KotlinLogging
+import net.consensys.cava.toml.TomlTable
+import org.jline.reader.EndOfFileException
+import org.koin.standalone.inject
+import org.strykeforce.thirdcoast.command.AbstractCommand
+import org.strykeforce.thirdcoast.command.Command
+import org.strykeforce.thirdcoast.command.prompt
+import org.strykeforce.thirdcoast.device.TalonFXService
+import org.strykeforce.thirdcoast.warn
+import java.lang.Exception
+
+private val logger = KotlinLogging.logger {}
+
+class RunTalonFxCommand(
+        parent: Command?,
+        key: String,
+        toml: TomlTable
+) : AbstractCommand(parent, key, toml) {
+
+    private val talonFxService: TalonFXService by inject()
+
+    override fun execute(): Command {
+        var done = false
+
+        while (!done){
+            try {
+                val line = reader.readLine(prompt())
+                if(line.isEmpty()) throw EndOfFileException()
+                val setpoints = line.split(",")
+                val setpoint = setpoints[0].toDouble()
+                val duration = if(setpoints.size > 1) setpoints[1].toDouble() else 0.0
+                val mode = talonFxService.controlMode
+
+                //Check inputs
+                if(mode == ControlMode.PercentOutput && !(-1.0..1.0).contains(setpoint)){
+                    terminal.warn("setpoint must be in range -1.0 to 1.0 for percent output mode")
+                    continue
+                }
+                if((mode == ControlMode.MotionMagic || mode == ControlMode.Position) && duration > 0.0){
+                    terminal.warn("specifying a duration in position modes not allowed")
+                    continue
+                }
+
+                //Run Talon FX's
+                talonFxService.active.forEach{ it.set(mode, setpoint)}
+
+                if(duration > 0.0){
+                    logger.debug{ "run duration = $duration seconds" }
+                    Timer.delay(duration)
+                    logger.debug { "run duration expired, setting output = 0.0" }
+                    talonFxService.active.forEach{ it.set(mode, 0.0) }
+                }
+            } catch (e: Exception){
+                done = true
+            }
+        }
+        return super.execute()
+    }
+}

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

@@ -0,0 +1,41 @@
+package org.strykeforce.thirdcoast.talon
+
+import com.ctre.phoenix.motorcontrol.can.TalonFX
+import net.consensys.cava.toml.TomlTable
+import org.koin.standalone.inject
+import org.strykeforce.thirdcoast.command.AbstractCommand
+import org.strykeforce.thirdcoast.command.Command
+import org.strykeforce.thirdcoast.command.prompt
+import org.strykeforce.thirdcoast.device.TalonFXService
+import org.strykeforce.thirdcoast.info
+import org.strykeforce.thirdcoast.readIntList
+import org.strykeforce.thirdcoast.warn
+import java.lang.Exception
+import java.lang.IllegalArgumentException
+
+class SelectTalonFxCommand(parent: Command?,
+                           key: String,
+                           toml: TomlTable
+) : AbstractCommand(parent, key, toml) {
+
+    private val talonFXService: TalonFXService by inject()
+
+    override val menu: String
+        get() = formatMenu(talonFXService.active.map(TalonFX::getDeviceID).joinToString())
+
+    override fun execute(): Command {
+        while (true){
+            try {
+                val ids = reader.readIntList(this.prompt("ids"), talonFXService.active.map(TalonFX::getDeviceID))
+                val new = talonFXService.activate(ids)
+                if(new.isNotEmpty()){
+                    terminal.info("reset control mode, current limit enabled, brake, voltage compensation\\nand sensor phase \" +\n" +
+                            "                                \"for talonfxs: ${new.joinToString()}")
+                }
+                return super.execute()
+            } catch (e: IllegalArgumentException){
+                terminal.warn("Please enter a list of TalonFx ids separated by ','")
+            }
+        }
+    }
+}

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

@@ -5,9 +5,9 @@ import org.strykeforce.thirdcoast.command.AbstractParameter
 import org.strykeforce.thirdcoast.command.Command
 import org.strykeforce.thirdcoast.parseResource
 
-class TalonFXParameter(command: Command, toml: TomlTable, val enum: Enum) : AbstractParameter(command, toml) {
+class TalonFxParameter(command: Command, toml: TomlTable, val enum: fxEnum) : AbstractParameter(command, toml) {
 
-    enum class Enum{
+    enum class fxEnum{
         SLOT_P,
         SLOT_I,
         SLOT_D,
@@ -36,7 +36,10 @@ class TalonFXParameter(command: Command, toml: TomlTable, val enum: Enum) : Abst
         STATOR_CURRENT_LIMIT,
         STATOR_CURRENT_LIMIT_THRES_CURRENT,
         STATOR_CURRENT_LIMIT_THRES_TIME,
-        CURRENT_LIMIT_PEAK_DURATION,
+        SUPPLY_CURRENT_LIMIT_ENABLE,
+        SUPPLY_CURRENT_LIMIT,
+        SUPPLY_CURRENT_LIMIT_THRES_CURRENT,
+        SUPPLY_CURRENT_LIMIT_THRES_TIME,
         STATUS_GENERAL,
         STATUS_FEEDBACK0,
         STATUS_QUAD_ENCODER,
@@ -49,15 +52,16 @@ class TalonFXParameter(command: Command, toml: TomlTable, val enum: Enum) : Abst
         SOFT_LIMIT_THRESHOLD_FORWARD,
         SOFT_LIMIT_THRESHOLD_REVERSE,
         VELOCITY_MEASUREMENT_WINDOW,
+        INTEGRATED_SENSOR_OFFSET_DEGREES,
         FACTORY_DEFAULTS,
     }
 
     companion object {
         private val tomlTable by lazy { parseResource("/talonFx.toml") }
 
-        fun create(command: Command, param: String): TalonFXParameter {
+        fun create(command: Command, param: String): TalonFxParameter {
             val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param: $param")
-            return TalonFXParameter(command, toml, Enum.valueOf(param))
+            return TalonFxParameter(command, toml, fxEnum.valueOf(param))
         }
     }
 }