Merge pull request #15 from strykeforce/mwitcpalek/talonFX

Mwitcpalek/talon fx
strykeforce · Jan 18, 2020 · 00d758a · 00d758a
2 parents 97b2776 + bb4e898
commit 00d758a
Show file tree

Hide file tree

Showing 31 changed files with 1,958 additions and 189 deletions.
diff --git a/build.gradle b/build.gradle
@@ -4,6 +4,8 @@ plugins {
     id "edu.wpi.first.GradleRIO" version "2020.1.2"
 }
 
+version = "20.0.0"
+
 sourceCompatibility = JavaVersion.VERSION_11
 targetCompatibility = JavaVersion.VERSION_11
 
@@ -94,7 +96,7 @@ jar {
     manifest {
         attributes(
                 'Implementation-Title': 'Third Coast Telemetry Utility (tct)',
-                'Implementation-Version': '20.0.0'
+                'Implementation-Version': version
         )
     }
 }
diff --git a/gradle.properties b/gradle.properties
@@ -0,0 +1 @@
+kotlin.code.style=official
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,6 @@
+#Sat Jan 18 13:27:09 EST 2020
+distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-all.zip
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip
-zipStoreBase=GRADLE_USER_HOME
 zipStorePath=permwrapper/dists
+zipStoreBase=GRADLE_USER_HOME
diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt b/src/main/kotlin/org/strykeforce/thirdcoast/Koin.kt
@@ -3,8 +3,10 @@ package org.strykeforce.thirdcoast
 import com.ctre.phoenix.CANifier
 import com.ctre.phoenix.motorcontrol.FeedbackDevice
 import com.ctre.phoenix.motorcontrol.NeutralMode
+import com.ctre.phoenix.motorcontrol.can.TalonFX
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
+import com.ctre.phoenix.sensors.PigeonIMU
 import edu.wpi.first.wpilibj.DigitalOutput
 import edu.wpi.first.wpilibj.Servo
 import edu.wpi.first.wpilibj.Solenoid
@@ -34,6 +36,8 @@ val tctModule = module {
     single { TelemetryService(Function { inventory -> TelemetryController(inventory, get(), SERVER_PORT) }) }
 
     single { TalonService(get()) { id -> TalonSRX(id) } }
+
+    single { TalonFxService(get()) { id -> TalonFX(id) } }
 
     single { ServoService { id -> Servo(id) } }
 
@@ -43,6 +47,8 @@ val tctModule = module {
 
     single { CanifierService(get()) { id -> CANifier(id) } }
 
+    single { PigeonService(get()) { id -> PigeonIMU(id) } }
+
     single { (command: Command) -> Shell(command, get()) }
 
     single<Terminal> { TerminalBuilder.terminal() }

diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt b/src/main/kotlin/org/strykeforce/thirdcoast/command/Command.kt
@@ -1,5 +1,6 @@
 package org.strykeforce.thirdcoast.command
 
+import mu.KotlinLogging
 import net.consensys.cava.toml.TomlTable
 import org.jline.reader.LineReader
 import org.jline.terminal.Terminal
@@ -10,6 +11,8 @@ import org.koin.standalone.inject
 import org.strykeforce.thirdcoast.canifier.*
 import org.strykeforce.thirdcoast.dio.RunDigitalOutputsCommand
 import org.strykeforce.thirdcoast.dio.SelectDigitalOutputsCommand
+import org.strykeforce.thirdcoast.gyro.PigeonParameterCommand
+import org.strykeforce.thirdcoast.gyro.SelectPigeonCommand
 import org.strykeforce.thirdcoast.servo.RunServosCommand
 import org.strykeforce.thirdcoast.servo.SelectServosCommand
 import org.strykeforce.thirdcoast.solenoid.RunSolenoidsCommand
@@ -20,7 +23,7 @@ import org.strykeforce.thirdcoast.swerve.SelectAzimuthCommand
 import org.strykeforce.thirdcoast.swerve.SetAzimuthCommand
 import org.strykeforce.thirdcoast.talon.*
 
-//private val logger = KotlinLogging.logger {}
+private val logger = KotlinLogging.logger {}
 
 interface Command {
     val key: String
@@ -34,10 +37,12 @@ interface Command {
         const val MENU_KEY = "menu"
         const val TYPE_KEY = "type"
         const val ORDER_KEY = "order"
+        const val DEVICE_KEY = "device"
 
         fun createFromToml(toml: TomlTable, parent: MenuCommand? = null, key: String = "ROOT"): Command {
 
             val type = toml.getString(TYPE_KEY) ?: throw Exception("$key: $TYPE_KEY missing")
+            logger.info { "type: $type, key: $key" }
 
             return when (type) {
                 "menu" -> createMenuCommand(parent, key, toml)
@@ -54,6 +59,9 @@ interface Command {
                 "talon.hard.source" -> SelectHardLimitSourceCommand(parent, key, toml)
                 "talon.hard.normal" -> SelectHardLimitNormalCommand(parent, key, toml)
                 "talon.velocity.period" -> SelectVelocityMeasurmentPeriodCommand(parent, key, toml)
+                "talon.commutation" -> SelectMotorCommutationCommand(parent, key, toml)
+                "talon.absoluteRange" -> SelectAbsoluteSensorRange(parent, key, toml)
+                "talon.initStrategy" -> SelectInitializationStrategy(parent, key, toml)
                 "servo.select" -> SelectServosCommand(parent, key, toml)
                 "servo.run" -> RunServosCommand(parent, key, toml)
                 "solenoid.select" -> SelectSolenoidsCommand(parent, key, toml)
@@ -71,6 +79,8 @@ interface Command {
                 "swerve.azimuth.save" -> SaveZeroCommand(parent, key, toml)
                 "swerve.azimuth.select" -> SelectAzimuthCommand(parent, key, toml)
                 "swerve.azimuth.adjust" -> AdjustAzimuthCommand(parent, key, toml)
+                "pigeon.select" -> SelectPigeonCommand(parent, key, toml)
+                "pigeon.param" -> PigeonParameterCommand(parent, key, toml)
                 else -> DefaultCommand(parent, key, toml)
             }
         }
@@ -81,6 +91,7 @@ interface Command {
                 .forEach { k ->
                     val child = createFromToml(toml.getTable(k)!!, command, k)
                     command.children.add(child)
+                    logger.info { "Create Menu: $k, ${command.validMenuChoices}" }
                 }
             return command
         }

diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/PigeonService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/PigeonService.kt
@@ -0,0 +1,33 @@
+package org.strykeforce.thirdcoast.device
+
+import com.ctre.phoenix.sensors.PigeonIMU
+import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame
+import org.strykeforce.thirdcoast.telemetry.item.PigeonIMUItem
+import mu.KotlinLogging
+import org.strykeforce.thirdcoast.telemetry.TelemetryService
+
+private val logger = KotlinLogging.logger {}
+
+private const val DEFAULT_TEMP_COMP = false
+
+
+class PigeonService(private val telemetryService: TelemetryService, factory: (id: Int) -> PigeonIMU) :
+    AbstractDeviceService<PigeonIMU>(factory) {
+
+    val timeout = 10
+    var tempCompensation = DEFAULT_TEMP_COMP
+
+
+    override fun activate(ids: Collection<Int>): Set<Int> {
+        val new = super.activate(ids)
+        telemetryService.stop()
+        active.forEach {telemetryService.register(PigeonIMUItem(it)) }
+        active.forEach {
+            it.setTemperatureCompensationDisable(tempCompensation)
+        }
+
+        telemetryService.start()
+        return new
+    }
+
+}
diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonFxService.kt
@@ -0,0 +1,73 @@
+package org.strykeforce.thirdcoast.device
+
+import com.ctre.phoenix.motorcontrol.ControlMode
+import com.ctre.phoenix.motorcontrol.NeutralMode
+import com.ctre.phoenix.motorcontrol.StatusFrame
+import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced
+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.telemetry.TelemetryService
+import java.lang.IllegalStateException
+import kotlin.math.log
+
+private val logger = KotlinLogging.logger {}
+
+private val CONTROL_MODE_DEFAULT = ControlMode.PercentOutput
+private const val ACTIVE_SLOT_DEFAULT = 0
+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
+
+
+class TalonFxService(private val telemetryService: TelemetryService, factory: (id:Int) -> TalonFX ):
+        AbstractDeviceService<TalonFX>(factory) {
+
+    val timeout = 10
+    var dirty = true
+    var neutralMode = NEUTRAL_MODE_DEFAULT
+    var controlMode = CONTROL_MODE_DEFAULT
+    var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
+    var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
+    var activeSlotIndex: Int = ACTIVE_SLOT_DEFAULT
+
+    var activeConfiguration = TalonFXConfiguration()
+        get() {
+            if(!dirty) return field
+            active.firstOrNull()?.getAllConfigs(field,timeout)?: logger.debug("no active talon fx's, returning default config")
+            dirty = false
+            return field
+        }
+
+    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>): Set<Int> {
+        dirty = true
+
+        val new = super.activate(ids)
+        telemetryService.stop()
+        active.filter { new.contains(it.deviceID) }.forEach(){
+            it.setNeutralMode(neutralMode)
+            it.selectProfileSlot(activeSlotIndex,0)
+            it.enableVoltageCompensation(voltageCompensation)
+            it.setSensorPhase(sensorPhase)
+            it.setInverted(OUTPUT_INVERTED_DEFAULT)
+            telemetryService.register(it)
+        }
+        telemetryService.start()
+        return new
+    }
+}
diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/device/TalonService.kt
@@ -2,6 +2,7 @@ package org.strykeforce.thirdcoast.device
 
 import com.ctre.phoenix.motorcontrol.ControlMode
 import com.ctre.phoenix.motorcontrol.NeutralMode
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration
 import com.ctre.phoenix.motorcontrol.can.SlotConfiguration
 import com.ctre.phoenix.motorcontrol.can.TalonSRX
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration
@@ -17,6 +18,10 @@ 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
+private const val SUPPLY_CURRENT_LIMIT_ENABLE_DEFAULT = false
+private const val SUPPLY_CURRENT_LIMIT_DEFAULT = 0.0
+private const val SUPPLY_CURRENT_LIMIT_TRIG_CURRENT_DEFAULT = 0.0
+private const val SUPPLY_CURRENT_LIMIT_TRIG_TIME_DEFAULT = 0.0
 
 /**
  * Holds the active state for all Talons that have been activated by the user. Talons that are currently active
@@ -50,6 +55,7 @@ class TalonService(private val telemetryService: TelemetryService, factory: (id:
     var voltageCompensation = VOLTAGE_COMPENSATION_ENABLED_DEFAULT
     var sensorPhase = SENSOR_PHASE_INVERTED_DEFAULT
     var currentLimit = CURRENT_LIMIT_ENABLED_DEFAULT
+    var supplyCurrentLimit = SupplyCurrentLimitConfiguration(SUPPLY_CURRENT_LIMIT_ENABLE_DEFAULT, SUPPLY_CURRENT_LIMIT_DEFAULT, SUPPLY_CURRENT_LIMIT_TRIG_CURRENT_DEFAULT, SUPPLY_CURRENT_LIMIT_TRIG_TIME_DEFAULT)
 
     var activeConfiguration = TalonSRXConfiguration()
         get() {

diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/gyro/PigeonParameter.kt b/src/main/kotlin/org/strykeforce/thirdcoast/gyro/PigeonParameter.kt
@@ -0,0 +1,38 @@
+package org.strykeforce.thirdcoast.gyro
+
+import net.consensys.cava.toml.TomlTable
+import org.strykeforce.thirdcoast.command.AbstractParameter
+import org.strykeforce.thirdcoast.command.Command
+import org.strykeforce.thirdcoast.parseResource
+import org.strykeforce.thirdcoast.talon.TalonParameter
+
+class PigeonParameter(command: Command, toml: TomlTable, val enum: PigeonParameter.Enum) : AbstractParameter(command, toml) {
+
+    enum class Enum {
+        ACCUM_Z_ANGLE,
+        FUSED_HEADING,
+        TEMP_COMP_DISABLE,
+        YAW,
+        GENERAL_STATUS,
+        SIX_DEG_STATUS,
+        FUSED_STATUS,
+        GYRO_ACCUM_STATUS,
+        GEN_COMPASS_STATUS,
+        GEN_ACCEL_STATUS,
+        SIX_QUAT_STATUS,
+        MAG_STATUS,
+        BIAS_GYRO_STATUS,
+        BIAS_MAG_STATUS,
+        BIAS_ACCEL_STATUS,
+        FACTORY_DEFAULT,
+    }
+
+    companion object {
+        private val tomlTable by lazy { parseResource("/pigeon.toml") }
+
+        fun create(command: Command, param: String): PigeonParameter {
+            val toml = tomlTable.getTable(param) ?: throw java.lang.IllegalArgumentException("missing param: $param")
+            return PigeonParameter(command, toml, Enum.valueOf(param))
+        }
+    }
+}